37 #ifndef OMPL_DATASTRUCTURES_GRID_B_
38 #define OMPL_DATASTRUCTURES_GRID_B_
40 #include "ompl/datastructures/GridN.h"
41 #include "ompl/datastructures/BinaryHeap.h"
42 #include "ompl/util/DisableCompilerWarning.h"
44 OMPL_PUSH_DISABLE_CLANG_WARNING(-Woverloaded-
virtual)
50 template <
typename _T,
class LessThanExternal = std::less<_T>,
class LessThanInternal = LessThanExternal>
51 class GridB :
public GridN<_T>
55 using Cell =
typename GridN<_T>::Cell;
58 using CellArray =
typename GridN<_T>::CellArray;
61 using Coord =
typename GridN<_T>::Coord;
67 struct CellX :
public Cell
73 ~CellX()
override =
default;
77 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
84 using EventCellUpdate = void (*)(Cell *,
void *);
87 explicit GridB(
unsigned int dimension) : GridN<_T>(dimension)
99 void onCellUpdate(EventCellUpdate event,
void *arg)
101 eventCellUpdate_ = event;
102 eventCellUpdateData_ = arg;
106 Cell *topInternal()
const
108 auto *top =
static_cast<Cell *
>(internal_.top()->data);
109 return top ? top : topExternal();
113 Cell *topExternal()
const
115 auto *top =
static_cast<Cell *
>(external_.top()->data);
116 return top ? top : topInternal();
120 unsigned int countInternal()
const
122 return internal_.size();
126 unsigned int countExternal()
const
128 return external_.size();
132 double fracExternal()
const
134 return external_.empty() ? 0.0 : (double)(external_.size()) / (
double)(external_.size() + internal_.size());
138 double fracInternal()
const
140 return 1.0 - fracExternal();
144 void update(Cell *cell)
146 eventCellUpdate_(cell, eventCellUpdateData_);
149 reinterpret_cast<typename externalBHeap::Element *
>(
static_cast<CellX *
>(cell)->heapElement));
152 reinterpret_cast<typename internalBHeap::Element *
>(
static_cast<CellX *
>(cell)->heapElement));
158 std::vector<Cell *> cells;
159 this->getCells(cells);
160 for (
int i = cells.size() - 1; i >= 0; --i)
161 eventCellUpdate_(cells[i], eventCellUpdateData_);
167 virtual Cell *createCell(
const Coord &coord, CellArray *nbh =
nullptr)
169 auto *cell =
new CellX();
173 this->neighbors(cell->coord, *list);
175 for (
auto cl = list->begin(); cl != list->end(); ++cl)
177 auto *c =
static_cast<CellX *
>(*cl);
178 bool wasBorder = c->border;
183 eventCellUpdate_(c, eventCellUpdateData_);
186 external_.update(
reinterpret_cast<typename externalBHeap::Element *
>(c->heapElement));
191 external_.remove(
reinterpret_cast<typename externalBHeap::Element *
>(c->heapElement));
195 internal_.update(
reinterpret_cast<typename internalBHeap::Element *
>(c->heapElement));
199 cell->neighbors = GridN<_T>::numberOfBoundaryDimensions(cell->coord) + list->size();
200 if (cell->border && cell->neighbors >= GridN<_T>::interiorCellNeighborsLimit_)
201 cell->border =
false;
206 return static_cast<Cell *
>(cell);
210 virtual void add(Cell *cell)
212 auto *ccell =
static_cast<CellX *
>(cell);
213 eventCellUpdate_(ccell, eventCellUpdateData_);
218 external_.insert(ccell);
220 internal_.insert(ccell);
224 virtual bool remove(Cell *cell)
228 auto *list =
new CellArray();
229 this->neighbors(cell->coord, *list);
231 for (
auto cl = list->begin(); cl != list->end(); ++cl)
233 auto *c =
static_cast<CellX *
>(*cl);
234 bool wasBorder = c->border;
239 eventCellUpdate_(c, eventCellUpdateData_);
244 external_.update(
reinterpret_cast<typename externalBHeap::Element *
>(c->heapElement));
247 internal_.remove(
reinterpret_cast<typename internalBHeap::Element *
>(c->heapElement));
252 internal_.update(
reinterpret_cast<typename internalBHeap::Element *
>(c->heapElement));
261 auto *cx =
static_cast<CellX *
>(cell);
263 external_.
remove(
reinterpret_cast<typename externalBHeap::Element *
>(cx->heapElement));
265 internal_.remove(
reinterpret_cast<typename internalBHeap::Element *
>(cx->heapElement));
272 void clear()
override
278 void status(std::ostream &out = std::cout)
const override
281 out << countInternal() <<
" internal cells" << std::endl;
282 out << countExternal() <<
" external cells" << std::endl;
287 EventCellUpdate eventCellUpdate_;
290 void *eventCellUpdateData_;
293 static void noCellUpdate(Cell * ,
void * )
300 eventCellUpdate_ = &noCellUpdate;
301 eventCellUpdateData_ =
nullptr;
302 internal_.onAfterInsert(&setHeapElementI,
nullptr);
303 external_.onAfterInsert(&setHeapElementE,
nullptr);
314 struct LessThanInternalCell
316 bool operator()(
const CellX *
const a,
const CellX *
const b)
const
318 return lt_(a->data, b->data);
322 LessThanInternal lt_;
326 struct LessThanExternalCell
328 bool operator()(
const CellX *
const a,
const CellX *
const b)
const
330 return lt_(a->data, b->data);
334 LessThanExternal lt_;
344 static void setHeapElementI(
typename internalBHeap::Element *element,
void * )
346 element->data->heapElement =
reinterpret_cast<void *
>(element);
350 static void setHeapElementE(
typename externalBHeap::Element *element,
void * )
352 element->data->heapElement =
reinterpret_cast<void *
>(element);