.. _program_listing_file_src_ompl_datastructures_GridN.h:

Program Listing for File GridN.h
================================

|exhale_lsh| :ref:`Return to documentation for file <file_src_ompl_datastructures_GridN.h>` (``src/ompl/datastructures/GridN.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_N_
   #define OMPL_DATASTRUCTURES_GRID_N_
   
   #include "ompl/datastructures/Grid.h"
   
   namespace ompl
   {
       template <typename _T>
       class GridN : public Grid<_T>
       {
       public:
           using BaseCell = typename Grid<_T>::Cell;
   
           using BaseCellArray = typename Grid<_T>::CellArray;
   
           using Coord = typename Grid<_T>::Coord;
   
           struct Cell : public BaseCell
           {
               unsigned int neighbors{0};
   
               bool border{true};
   
               Cell() = default;
   
               ~Cell() override = default;
   
               EIGEN_MAKE_ALIGNED_OPERATOR_NEW
           };
   
           using CellArray = std::vector<Cell *>;
   
           explicit GridN(unsigned int dimension) : Grid<_T>(dimension)
           {
               hasBounds_ = false;
               overrideCellNeighborsLimit_ = false;
               setDimension(dimension);
           }
   
           ~GridN() override = default;
   
           void setDimension(unsigned int dimension)
           {
               assert(Grid<_T>::empty() == true);
               Grid<_T>::dimension_ = dimension;
               Grid<_T>::maxNeighbors_ = 2 * dimension;
               if (!overrideCellNeighborsLimit_)
                   interiorCellNeighborsLimit_ = Grid<_T>::maxNeighbors_;
           }
   
           void setBounds(const Coord &low, const Coord &up)
           {
               lowBound_ = low;
               upBound_ = up;
               hasBounds_ = true;
           }
   
           void setInteriorCellNeighborLimit(unsigned int count)
           {
               interiorCellNeighborsLimit_ = count;
               assert(interiorCellNeighborsLimit_ > 0);
               overrideCellNeighborsLimit_ = true;
           }
   
           Cell *getCell(const Coord &coord) const
           {
               return static_cast<Cell *>(Grid<_T>::getCell(coord));
           }
   
           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
           {
               BaseCellArray baselist;
               Grid<_T>::neighbors(coord, baselist);
               list.reserve(list.size() + baselist.size());
               for (const auto &c : baselist)
                   list.push_back(static_cast<Cell *>(c));
           }
   
           BaseCell *createCell(const Coord &coord, BaseCellArray *nbh = nullptr) override
           {
               auto *cell = new Cell();
               cell->coord = coord;
   
               BaseCellArray *list = nbh ? nbh : new BaseCellArray();
               Grid<_T>::neighbors(cell->coord, *list);
   
               for (auto cl = list->begin(); cl != list->end(); ++cl)
               {
                   auto *c = static_cast<Cell *>(*cl);
                   c->neighbors++;
                   if (c->border && c->neighbors >= interiorCellNeighborsLimit_)
                       c->border = false;
               }
   
               cell->neighbors = numberOfBoundaryDimensions(cell->coord) + list->size();
               if (cell->border && cell->neighbors >= interiorCellNeighborsLimit_)
                   cell->border = false;
   
               if (!nbh)
                   delete list;
   
               return cell;
           }
   
           bool remove(BaseCell *cell) override
           {
               if (cell)
               {
                   auto *list = new BaseCellArray();
                   Grid<_T>::neighbors(cell->coord, *list);
                   for (auto cl = list->begin(); cl != list->end(); ++cl)
                   {
                       auto *c = static_cast<Cell *>(*cl);
                       c->neighbors--;
                       if (!c->border && c->neighbors < interiorCellNeighborsLimit_)
                           c->border = true;
                   }
                   delete list;
                   auto pos = Grid<_T>::hash_.find(&cell->coord);
                   if (pos != Grid<_T>::hash_.end())
                   {
                       Grid<_T>::hash_.erase(pos);
                       return true;
                   }
               }
               return false;
           }
   
           void getCells(CellArray &cells) const
           {
               for (const auto &h : Grid<_T>::hash_)
                   cells.push_back(static_cast<Cell *>(h.second));
           }
   
       protected:
           unsigned int numberOfBoundaryDimensions(const Coord &coord) const
           {
               unsigned int result = 0;
               if (hasBounds_)
               {
                   for (unsigned int i = 0; i < Grid<_T>::dimension_; ++i)
                       if (coord[i] == lowBound_[i] || coord[i] == upBound_[i])
                           result++;
               }
               return result;
           }
   
           bool hasBounds_;
   
           Coord lowBound_;
   
           Coord upBound_;
   
           unsigned int interiorCellNeighborsLimit_;
   
           bool overrideCellNeighborsLimit_;
   
       public:
           EIGEN_MAKE_ALIGNED_OPERATOR_NEW
       };
   }
   
   #endif