.. _program_listing_file_src_ompl_datastructures_GridB.h:

Program Listing for File GridB.h
================================

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

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

.. code-block:: cpp

   /*********************************************************************
   * Software License Agreement (BSD License)
   *
   *  Copyright (c) 2008, Willow Garage, Inc.
   *  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 Willow Garage 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_B_
   #define OMPL_DATASTRUCTURES_GRID_B_
   
   #include "ompl/datastructures/GridN.h"
   #include "ompl/datastructures/BinaryHeap.h"
   #include "ompl/util/DisableCompilerWarning.h"
   
   OMPL_PUSH_DISABLE_CLANG_WARNING(-Woverloaded-virtual)
   
   namespace ompl
   {
       template <typename _T, class LessThanExternal = std::less<_T>, class LessThanInternal = LessThanExternal>
       class GridB : public GridN<_T>
       {
       public:
           using Cell = typename GridN<_T>::Cell;
           using BaseCell = typename GridN<_T>::BaseCell;
   
           using CellArray = typename GridN<_T>::CellArray;
   
           using Coord = typename GridN<_T>::Coord;
   
       protected:
           // the type of cell here needs an extra pointer to allow the updatable heap to work fast
           // however, this stays hidden from the user
           struct CellX : public Cell
           {
               CellX() : Cell()
               {
               }
   
               ~CellX() override = default;
   
               void *heapElement;
   
               EIGEN_MAKE_ALIGNED_OPERATOR_NEW
           };
   
   
       public:
           using EventCellUpdate = void (*)(Cell *, void *);
   
           explicit GridB(unsigned int dimension) : GridN<_T>(dimension)
           {
               setupHeaps();
           }
   
           ~GridB() override
           {
               clearHeaps();
           }
   
           void onCellUpdate(EventCellUpdate event, void *arg)
           {
               eventCellUpdate_ = event;
               eventCellUpdateData_ = arg;
           }
   
           Cell *topInternal() const
           {
               auto *top = static_cast<Cell *>(internal_.top()->data);
               return top ? top : topExternal();
           }
   
           Cell *topExternal() const
           {
               auto *top = static_cast<Cell *>(external_.top()->data);
               return top ? top : topInternal();
           }
   
           unsigned int countInternal() const
           {
               return internal_.size();
           }
   
           unsigned int countExternal() const
           {
               return external_.size();
           }
   
           double fracExternal() const
           {
               return external_.empty() ? 0.0 : (double)(external_.size()) / (double)(external_.size() + internal_.size());
           }
   
           double fracInternal() const
           {
               return 1.0 - fracExternal();
           }
   
           void update(Cell *cell)
           {
               eventCellUpdate_(cell, eventCellUpdateData_);
               if (cell->border)
                   external_.update(
                       reinterpret_cast<typename externalBHeap::Element *>(static_cast<CellX *>(cell)->heapElement));
               else
                   internal_.update(
                       reinterpret_cast<typename internalBHeap::Element *>(static_cast<CellX *>(cell)->heapElement));
           }
   
           void updateAll()
           {
               std::vector<Cell *> cells;
               this->getCells(cells);
               for (int i = cells.size() - 1; i >= 0; --i)
                   eventCellUpdate_(cells[i], eventCellUpdateData_);
               external_.rebuild();
               internal_.rebuild();
           }
   
           virtual Cell *createCell(const Coord &coord, CellArray *nbh = nullptr)
           {
               auto *cell = new CellX();
               cell->coord = coord;
   
               CellArray *list = nbh ? nbh : new CellArray();
               this->neighbors(cell->coord, *list);
   
               for (auto cl = list->begin(); cl != list->end(); ++cl)
               {
                   auto *c = static_cast<CellX *>(*cl);
                   bool wasBorder = c->border;
                   c->neighbors++;
                   if (c->border && c->neighbors >= GridN<_T>::interiorCellNeighborsLimit_)
                       c->border = false;
   
                   eventCellUpdate_(c, eventCellUpdateData_);
   
                   if (c->border)
                       external_.update(reinterpret_cast<typename externalBHeap::Element *>(c->heapElement));
                   else
                   {
                       if (wasBorder)
                       {
                           external_.remove(reinterpret_cast<typename externalBHeap::Element *>(c->heapElement));
                           internal_.insert(c);
                       }
                       else
                           internal_.update(reinterpret_cast<typename internalBHeap::Element *>(c->heapElement));
                   }
               }
   
               cell->neighbors = GridN<_T>::numberOfBoundaryDimensions(cell->coord) + list->size();
               if (cell->border && cell->neighbors >= GridN<_T>::interiorCellNeighborsLimit_)
                   cell->border = false;
   
               if (!nbh)
                   delete list;
   
               return static_cast<Cell *>(cell);
           }
   
           virtual void add(Cell *cell)
           {
               auto *ccell = static_cast<CellX *>(cell);
               eventCellUpdate_(ccell, eventCellUpdateData_);
   
               GridN<_T>::add(cell);
   
               if (cell->border)
                   external_.insert(ccell);
               else
                   internal_.insert(ccell);
           }
   
           bool remove(BaseCell *cell) override
           {
               if (cell)
               {
                   auto *list = new CellArray();
                   this->neighbors(cell->coord, *list);
   
                   for (auto cl = list->begin(); cl != list->end(); ++cl)
                   {
                       auto *c = static_cast<CellX *>(*cl);
                       bool wasBorder = c->border;
                       c->neighbors--;
                       if (!c->border && c->neighbors < GridN<_T>::interiorCellNeighborsLimit_)
                           c->border = true;
   
                       eventCellUpdate_(c, eventCellUpdateData_);
   
                       if (c->border)
                       {
                           if (wasBorder)
                               external_.update(reinterpret_cast<typename externalBHeap::Element *>(c->heapElement));
                           else
                           {
                               internal_.remove(reinterpret_cast<typename internalBHeap::Element *>(c->heapElement));
                               external_.insert(c);
                           }
                       }
                       else
                           internal_.update(reinterpret_cast<typename internalBHeap::Element *>(c->heapElement));
                   }
   
                   delete list;
   
                   auto pos = GridN<_T>::hash_.find(&cell->coord);
                   if (pos != GridN<_T>::hash_.end())
                   {
                       GridN<_T>::hash_.erase(pos);
                       auto *cx = static_cast<CellX *>(cell);
                       if (cx->border)
                           external_.remove(reinterpret_cast<typename externalBHeap::Element *>(cx->heapElement));
                       else
                           internal_.remove(reinterpret_cast<typename internalBHeap::Element *>(cx->heapElement));
                       return true;
                   }
               }
               return false;
           }
   
           void clear() override
           {
               GridN<_T>::clear();
               clearHeaps();
           }
   
           void status(std::ostream &out = std::cout) const override
           {
               GridN<_T>::status(out);
               out << countInternal() << " internal cells" << std::endl;
               out << countExternal() << " external cells" << std::endl;
           }
   
       protected:
           EventCellUpdate eventCellUpdate_;
   
           void *eventCellUpdateData_;
   
           static void noCellUpdate(Cell * /*unused*/, void * /*unused*/)
           {
           }
   
           void setupHeaps()
           {
               eventCellUpdate_ = &noCellUpdate;
               eventCellUpdateData_ = nullptr;
               internal_.onAfterInsert(&setHeapElementI, nullptr);
               external_.onAfterInsert(&setHeapElementE, nullptr);
           }
   
           void clearHeaps()
           {
               internal_.clear();
               external_.clear();
           }
   
           struct LessThanInternalCell
           {
               bool operator()(const CellX *const a, const CellX *const b) const
               {
                   return lt_(a->data, b->data);
               }
   
           private:
               LessThanInternal lt_;
           };
   
           struct LessThanExternalCell
           {
               bool operator()(const CellX *const a, const CellX *const b) const
               {
                   return lt_(a->data, b->data);
               }
   
           private:
               LessThanExternal lt_;
           };
   
           using internalBHeap = BinaryHeap<CellX *, LessThanInternalCell>;
   
           using externalBHeap = BinaryHeap<CellX *, LessThanExternalCell>;
   
           static void setHeapElementI(typename internalBHeap::Element *element, void * /*unused*/)
           {
               element->data->heapElement = reinterpret_cast<void *>(element);
           }
   
           static void setHeapElementE(typename externalBHeap::Element *element, void * /*unused*/)
           {
               element->data->heapElement = reinterpret_cast<void *>(element);
           }
   
           internalBHeap internal_;
   
           externalBHeap external_;
       };
   }
   
   OMPL_POP_CLANG
   
   #endif