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;
56 using BaseCell =
typename GridN<_T>::BaseCell;
59 using CellArray =
typename GridN<_T>::CellArray;
62 using Coord =
typename GridN<_T>::Coord;
68 struct CellX :
public Cell
74 ~CellX()
override =
default;
78 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
88 explicit GridB(
unsigned int dimension) :
GridN<_T>(dimension)
100 void onCellUpdate(EventCellUpdate event,
void *arg)
102 eventCellUpdate_ = event;
103 eventCellUpdateData_ = arg;
107 Cell *topInternal()
const
109 auto *top =
static_cast<Cell *
>(internal_.top()->data);
110 return top ? top : topExternal();
114 Cell *topExternal()
const
116 auto *top =
static_cast<Cell *
>(external_.top()->data);
117 return top ? top : topInternal();
121 unsigned int countInternal()
const
123 return internal_.size();
127 unsigned int countExternal()
const
129 return external_.size();
133 double fracExternal()
const
135 return external_.empty() ? 0.0 : (double)(external_.size()) / (
double)(external_.size() + internal_.size());
139 double fracInternal()
const
141 return 1.0 - fracExternal();
145 void update(Cell *cell)
147 eventCellUpdate_(cell, eventCellUpdateData_);
150 reinterpret_cast<typename externalBHeap::Element *
>(
static_cast<CellX *
>(cell)->heapElement));
153 reinterpret_cast<typename internalBHeap::Element *
>(
static_cast<CellX *
>(cell)->heapElement));
159 std::vector<Cell *> cells;
160 this->getCells(cells);
161 for (
int i = cells.size() - 1; i >= 0; --i)
162 eventCellUpdate_(cells[i], eventCellUpdateData_);
168 virtual Cell *createCell(
const Coord &coord, CellArray *nbh =
nullptr)
170 auto *cell =
new CellX();
174 this->neighbors(cell->coord, *list);
176 for (
auto cl = list->begin(); cl != list->end(); ++cl)
178 auto *c =
static_cast<CellX *
>(*cl);
179 bool wasBorder = c->border;
184 eventCellUpdate_(c, eventCellUpdateData_);
187 external_.update(
reinterpret_cast<typename externalBHeap::Element *
>(c->heapElement));
192 external_.remove(
reinterpret_cast<typename externalBHeap::Element *
>(c->heapElement));
196 internal_.update(
reinterpret_cast<typename internalBHeap::Element *
>(c->heapElement));
200 cell->neighbors = GridN<_T>::numberOfBoundaryDimensions(cell->coord) + list->size();
201 if (cell->border && cell->neighbors >= GridN<_T>::interiorCellNeighborsLimit_)
202 cell->border =
false;
207 return static_cast<Cell *
>(cell);
211 virtual void add(Cell *cell)
213 auto *ccell =
static_cast<CellX *
>(cell);
214 eventCellUpdate_(ccell, eventCellUpdateData_);
219 external_.insert(ccell);
221 internal_.insert(ccell);
225 bool remove(BaseCell *cell)
override
229 auto *list =
new CellArray();
230 this->neighbors(cell->coord, *list);
232 for (
auto cl = list->begin(); cl != list->end(); ++cl)
234 auto *c =
static_cast<CellX *
>(*cl);
235 bool wasBorder = c->border;
240 eventCellUpdate_(c, eventCellUpdateData_);
245 external_.update(
reinterpret_cast<typename externalBHeap::Element *
>(c->heapElement));
248 internal_.remove(
reinterpret_cast<typename internalBHeap::Element *
>(c->heapElement));
253 internal_.update(
reinterpret_cast<typename internalBHeap::Element *
>(c->heapElement));
262 auto *cx =
static_cast<CellX *
>(cell);
264 external_.
remove(
reinterpret_cast<typename externalBHeap::Element *
>(cx->heapElement));
266 internal_.remove(
reinterpret_cast<typename internalBHeap::Element *
>(cx->heapElement));
273 void clear()
override
279 void status(std::ostream &out = std::cout)
const override
282 out << countInternal() <<
" internal cells" << std::endl;
283 out << countExternal() <<
" external cells" << std::endl;
288 EventCellUpdate eventCellUpdate_;
291 void *eventCellUpdateData_;
294 static void noCellUpdate(Cell * ,
void * )
301 eventCellUpdate_ = &noCellUpdate;
302 eventCellUpdateData_ =
nullptr;
303 internal_.onAfterInsert(&setHeapElementI,
nullptr);
304 external_.onAfterInsert(&setHeapElementE,
nullptr);
315 struct LessThanInternalCell
317 bool operator()(
const CellX *
const a,
const CellX *
const b)
const
319 return lt_(a->data, b->data);
323 LessThanInternal lt_;
327 struct LessThanExternalCell
329 bool operator()(
const CellX *
const a,
const CellX *
const b)
const
331 return lt_(a->data, b->data);
335 LessThanExternal lt_;
345 static void setHeapElementI(
typename internalBHeap::Element *element,
void * )
347 element->data->heapElement =
reinterpret_cast<void *
>(element);
351 static void setHeapElementE(
typename externalBHeap::Element *element,
void * )
353 element->data->heapElement =
reinterpret_cast<void *
>(element);