.. _program_listing_file_src_ompl_datastructures_Grid.h:

Program Listing for File Grid.h
===============================

|exhale_lsh| :ref:`Return to documentation for file <file_src_ompl_datastructures_Grid.h>` (``src/ompl/datastructures/Grid.h``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   /*********************************************************************
    * Software License Agreement (BSD License)
    *
    *  Copyright (c) 2010, Rice University
    *  All rights reserved.
    *
    *  Redistribution and use in source and binary forms, with or without
    *  modification, are permitted provided that the following conditions
    *  are met:
    *
    *   * Redistributions of source code must retain the above copyright
    *     notice, this list of conditions and the following disclaimer.
    *   * Redistributions in binary form must reproduce the above
    *     copyright notice, this list of conditions and the following
    *     disclaimer in the documentation and/or other materials provided
    *     with the distribution.
    *   * Neither the name of the Rice University nor the names of its
    *     contributors may be used to endorse or promote products derived
    *     from this software without specific prior written permission.
    *
    *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
    *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
    *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
    *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
    *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
    *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
    *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
    *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
    *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
    *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
    *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
    *  POSSIBILITY OF SUCH DAMAGE.
    *********************************************************************/
   
   /* Author: Ioan Sucan */
   
   #ifndef OMPL_DATASTRUCTURES_GRID_
   #define OMPL_DATASTRUCTURES_GRID_
   
   #include <Eigen/Core>
   #include <vector>
   #include <iostream>
   #include <cstdlib>
   #include <unordered_map>
   #include <algorithm>
   
   namespace ompl
   {
       template <typename _T>
       class Grid
       {
       public:
           using Coord = Eigen::VectorXi;
   
           struct Cell
           {
               _T data;
   
               Coord coord;
   
               Cell() = default;
   
               virtual ~Cell() = default;
   
               EIGEN_MAKE_ALIGNED_OPERATOR_NEW
           };
   
           using CellArray = std::vector<Cell *>;
   
           explicit Grid(unsigned int dimension)
           {
               setDimension(dimension);
           }
   
           virtual ~Grid()
           {
               freeMemory();
           }
   
           virtual void clear()
           {
               freeMemory();
           }
   
           unsigned int getDimension() const
           {
               return dimension_;
           }
   
           void setDimension(unsigned int dimension)
           {
               if (!empty())
                   throw;
               dimension_ = dimension;
               maxNeighbors_ = 2 * dimension_;
           }
   
           bool has(const Coord &coord) const
           {
               return getCell(coord) != nullptr;
           }
   
           Cell *getCell(const Coord &coord) const
           {
               auto pos = hash_.find(const_cast<Coord *>(&coord));
               Cell *c = (pos != hash_.end()) ? pos->second : nullptr;
               return c;
           }
   
           void neighbors(const Cell *cell, CellArray &list) const
           {
               Coord test = cell->coord;
               neighbors(test, list);
           }
   
           void neighbors(const Coord &coord, CellArray &list) const
           {
               Coord test = coord;
               neighbors(test, list);
           }
   
           void neighbors(Coord &coord, CellArray &list) const
           {
               list.reserve(list.size() + maxNeighbors_);
   
               for (int i = dimension_ - 1; i >= 0; --i)
               {
                   coord[i]--;
   
                   auto pos = hash_.find(&coord);
                   Cell *cell = (pos != hash_.end()) ? pos->second : nullptr;
   
                   if (cell)
                       list.push_back(cell);
                   coord[i] += 2;
   
                   pos = hash_.find(&coord);
                   cell = (pos != hash_.end()) ? pos->second : nullptr;
   
                   if (cell)
                       list.push_back(cell);
                   coord[i]--;
               }
           }
   
           std::vector<std::vector<Cell *>> components() const
           {
               using ComponentHash = std::unordered_map<Coord *, int, HashFunCoordPtr, EqualCoordPtr>;
   
               int components = 0;
               ComponentHash ch;
               std::vector<std::vector<Cell *>> res;
   
               for (auto & i: hash_)
               {
                   Cell *c0 = i.second;
                   auto pos = ch.find(&c0->coord);
                   int comp = (pos != ch.end()) ? pos->second : -1;
   
                   if (comp < 0)
                   {
                       res.resize(res.size() + 1);
                       std::vector<Cell *> &q = res.back();
                       q.push_back(c0);
                       std::size_t index = 0;
                       while (index < q.size())
                       {
                           Cell *c = q[index++];
                           pos = ch.find(&c->coord);
                           comp = (pos != ch.end()) ? pos->second : -1;
   
                           if (comp < 0)
                           {
                               ch.insert(std::make_pair(&c->coord, components));
                               std::vector<Cell *> nbh;
                               neighbors(c, nbh);
                               for (const auto &n : nbh)
                               {
                                   pos = ch.find(&n->coord);
                                   comp = (pos != ch.end()) ? pos->second : -1;
                                   if (comp < 0)
                                       q.push_back(n);
                               }
                           }
                           else
                           {
                               --index;
                               q.erase(q.begin() + index);
                           }
                       }
                       ++components;
                   }
               }
               std::sort(res.begin(), res.end(), SortComponents());
               return res;
           }
   
           virtual Cell *createCell(const Coord &coord, CellArray *nbh = nullptr)
           {
               auto *cell = new Cell();
               cell->coord = coord;
               if (nbh)
                   neighbors(cell->coord, *nbh);
               return cell;
           }
   
           virtual bool remove(Cell *cell)
           {
               if (cell)
               {
                   auto pos = hash_.find(&cell->coord);
                   if (pos != hash_.end())
                   {
                       hash_.erase(pos);
                       return true;
                   }
               }
               return false;
           }
   
           virtual void add(Cell *cell)
           {
               hash_.insert(std::make_pair(&cell->coord, cell));
           }
   
           virtual void destroyCell(Cell *cell) const
           {
               delete cell;
           }
   
           void getContent(std::vector<_T> &content) const
           {
               for (const auto &h : hash_)
                   content.push_back(h.second->data);
           }
   
           void getCoordinates(std::vector<Coord *> &coords) const
           {
               for (const auto &h : hash_)
                   coords.push_back(h.first);
           }
   
           void getCells(CellArray &cells) const
           {
               for (const auto &h : hash_)
                   cells.push_back(h.second);
           }
   
           void printCoord(Coord &coord, std::ostream &out = std::cout) const
           {
               out << "[ ";
               for (unsigned int i = 0; i < dimension_; ++i)
                   out << coord[i] << " ";
               out << "]" << std::endl;
           }
   
           bool empty() const
           {
               return hash_.empty();
           }
   
           unsigned int size() const
           {
               return hash_.size();
           }
   
           virtual void status(std::ostream &out = std::cout) const
           {
               out << size() << " total cells " << std::endl;
               const std::vector<std::vector<Cell *>> &comp = components();
               out << comp.size() << " connected components: ";
               for (const auto &c : comp)
                   out << c.size() << " ";
               out << std::endl;
           }
   
       protected:
           void freeMemory()
           {
               CellArray content;
               getCells(content);
               hash_.clear();
   
               for (auto &c : content)
                   delete c;
           }
   
           struct HashFunCoordPtr
           {
               std::size_t operator()(const Coord *const s) const
               {
                   unsigned long h = 0;
                   for (int i = s->size() - 1; i >= 0; --i)
                   {
                       int high = h & 0xf8000000;
                       h = h << 5;
                       h = h ^ (high >> 27);
                       h = h ^ (*s)[i];
                   }
                   return (std::size_t)h;
               }
           };
   
           struct EqualCoordPtr
           {
               bool operator()(const Coord *const c1, const Coord *const c2) const
               {
                   return *c1 == *c2;
               }
           };
   
           using CoordHash = std::unordered_map<Coord *, Cell *, HashFunCoordPtr, EqualCoordPtr>;
   
           struct SortComponents
           {
               bool operator()(const std::vector<Cell *> &a, const std::vector<Cell *> &b) const
               {
                   return a.size() > b.size();
               }
           };
   
       public:
           using iterator = typename CoordHash::const_iterator;
   
           iterator begin() const
           {
               return hash_.begin();
           }
   
           iterator end() const
           {
               return hash_.end();
           }
   
       protected:
           unsigned int dimension_;
   
           unsigned int maxNeighbors_;
   
           CoordHash hash_;
       };
   }  // namespace ompl
   
   #endif