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>
67 struct CellX :
public Cell
73 ~CellX()
override =
default;
77 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
87 explicit GridB(
unsigned int dimension) :
GridN<_T>(dimension)
101 eventCellUpdate_ = event;
102 eventCellUpdateData_ = arg;
108 auto *top =
static_cast<Cell *
>(internal_.top()->data);
109 return top ? top : topExternal();
115 auto *top =
static_cast<Cell *
>(external_.top()->data);
116 return top ? top : topInternal();
122 return internal_.size();
128 return external_.size();
134 return external_.empty() ? 0.0 : (double)(external_.size()) / (
double)(external_.size() + internal_.size());
140 return 1.0 - fracExternal();
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_);
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));
201 cell->border =
false;
206 return static_cast<Cell *
>(cell);
212 auto *ccell =
static_cast<CellX *
>(cell);
213 eventCellUpdate_(ccell, eventCellUpdateData_);
218 external_.insert(ccell);
220 internal_.insert(ccell);
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));
278 void status(std::ostream &out = std::cout)
const override
281 out << countInternal() <<
" internal cells" << std::endl;
282 out << countExternal() <<
" external cells" << std::endl;
300 eventCellUpdate_ = &noCellUpdate;
301 eventCellUpdateData_ =
nullptr;
302 internal_.onAfterInsert(&setHeapElementI,
nullptr);
303 external_.onAfterInsert(&setHeapElementE,
nullptr);
316 bool operator()(
const CellX *
const a,
const CellX *
const b)
const
318 return lt_(a->data, b->data);
322 LessThanInternal lt_;
328 bool operator()(
const CellX *
const a,
const CellX *
const b)
const
330 return lt_(a->data, b->data);
334 LessThanExternal lt_;
346 element->data->heapElement =
reinterpret_cast<void *
>(element);
352 element->data->heapElement =
reinterpret_cast<void *
>(element);
This class defines a grid that keeps track of its boundary: it distinguishes between interior and ext...
internalBHeap internal_
The heap of interior cells.
double fracInternal() const
Return the fraction of internal cells.
void updateAll()
Update all cells and reconstruct the heaps.
void status(std::ostream &out=std::cout) const override
Print information about the data in this grid structure.
EventCellUpdate eventCellUpdate_
Pointer to function to be called when a cell needs to be updated.
static void noCellUpdate(Cell *, void *)
Default no-op update routine for a cell.
virtual void add(Cell *cell)
Add the cell to the grid.
virtual bool remove(Cell *cell)
Remove a cell from the grid.
void(*)(Cell *, void *) EventCellUpdate
Event to be called when a cell's priority is to be updated.
static void setHeapElementI(typename internalBHeap::Element *element, void *)
Routine used internally for keeping track of binary heap elements for internal cells.
GridB(unsigned int dimension)
Constructor.
unsigned int countExternal() const
Return the number of external cells.
void setupHeaps()
Set the update procedure for the heaps of internal and external cells.
void onCellUpdate(EventCellUpdate event, void *arg)
typename GridN< _T >::Cell Cell
Definition of a cell in this grid.
void clear() override
Clear all cells in the grid.
unsigned int countInternal() const
Return the number of internal cells.
void * eventCellUpdateData_
Data to be passed to function pointer above.
void clearHeaps()
Clear the data from both heaps.
virtual Cell * createCell(const Coord &coord, CellArray *nbh=nullptr)
Create a cell but do not add it to the grid; update neighboring cells however.
double fracExternal() const
Return the fraction of external cells.
void update(Cell *cell)
Update the position in the heaps for a particular cell.
Cell * topInternal() const
Return the cell that is at the top of the heap maintaining internal cells.
Cell * topExternal() const
Return the cell that is at the top of the heap maintaining external cells.
static void setHeapElementE(typename externalBHeap::Element *element, void *)
Routine used internally for keeping track of binary heap elements for external cells.
externalBHeap external_
The heap of external cells.
Representation of a grid where cells keep track of how many neighbors they have.
typename Grid< _T >::Coord Coord
Datatype for cell coordinates.
std::vector< Cell * > CellArray
The datatype for arrays of cells.
bool remove(BaseCell *cell) override
Eigen::VectorXi Coord
Definition of a coordinate within this grid.
unsigned int size() const
Check the size of the grid.
std::vector< Cell * > CellArray
The datatype for arrays of cells.
Main namespace. Contains everything in this library.
Define order for external cells.
Define order for internal cells.
Definition of a cell in this grid.