37 #ifndef OMPL_DATASTRUCTURES_GRID_B_
38 #define OMPL_DATASTRUCTURES_GRID_B_
40 #include "ompl/datastructures/GridN.h"
41 #include "ompl/datastructures/BinaryHeap.h"
47 template <
typename _T,
class LessThanExternal = std::less<_T>,
class LessThanInternal = LessThanExternal>
48 class GridB :
public GridN<_T>
52 using Cell =
typename GridN<_T>::Cell;
64 struct CellX :
public Cell
70 ~CellX()
override =
default;
82 explicit GridB(
unsigned int dimension) :
GridN<_T>(dimension)
144 reinterpret_cast<typename externalBHeap::Element *
>(
static_cast<CellX *
>(cell)->heapElement));
147 reinterpret_cast<typename internalBHeap::Element *
>(
static_cast<CellX *
>(cell)->heapElement));
153 std::vector<Cell *> cells;
155 for (
int i = cells.size() - 1; i >= 0; --i)
164 auto *cell =
new CellX();
170 for (
auto cl = list->begin(); cl != list->end(); ++cl)
172 auto *c =
static_cast<CellX *
>(*cl);
173 bool wasBorder = c->border;
181 external_.
update(
reinterpret_cast<typename externalBHeap::Element *
>(c->heapElement));
186 external_.
remove(
reinterpret_cast<typename externalBHeap::Element *
>(c->heapElement));
190 internal_.
update(
reinterpret_cast<typename internalBHeap::Element *
>(c->heapElement));
196 cell->border =
false;
201 return static_cast<Cell *
>(cell);
207 auto *ccell =
static_cast<CellX *
>(cell);
226 for (
auto cl = list->begin(); cl != list->end(); ++cl)
228 auto *c =
static_cast<CellX *
>(*cl);
229 bool wasBorder = c->border;
239 external_.
update(
reinterpret_cast<typename externalBHeap::Element *
>(c->heapElement));
242 internal_.
remove(
reinterpret_cast<typename internalBHeap::Element *
>(c->heapElement));
247 internal_.
update(
reinterpret_cast<typename internalBHeap::Element *
>(c->heapElement));
256 auto *cx =
static_cast<CellX *
>(cell);
258 external_.
remove(
reinterpret_cast<typename externalBHeap::Element *
>(cx->heapElement));
260 internal_.
remove(
reinterpret_cast<typename internalBHeap::Element *
>(cx->heapElement));
267 void clear()
override
273 void status(std::ostream &out = std::cout)
const override
309 struct LessThanInternalCell
311 bool operator()(
const CellX *
const a,
const CellX *
const b)
const
313 return lt_(a->data, b->data);
317 LessThanInternal lt_;
321 struct LessThanExternalCell
323 bool operator()(
const CellX *
const a,
const CellX *
const b)
const
325 return lt_(a->data, b->data);
329 LessThanExternal lt_;
339 static void setHeapElementI(
typename internalBHeap::Element *element,
void * )
341 element->data->heapElement =
reinterpret_cast<void *
>(element);
345 static void setHeapElementE(
typename externalBHeap::Element *element,
void * )
347 element->data->heapElement =
reinterpret_cast<void *
>(element);
static void setHeapElementI(typename internalBHeap::Element *element, void *)
Routine used internally for keeping track of binary heap elements for internal cells.
std::vector< Cell * > CellArray
The datatype for arrays of cells.
void clear() override
Clear all cells in the grid.
void updateAll()
Update all cells and reconstruct the heaps.
void * eventCellUpdateData_
Data to be passed to function pointer above.
void status(std::ostream &out=std::cout) const override
Print information about the data in this grid structure.
double fracExternal() const
Return the fraction of external cells.
void setupHeaps()
Set the update procedure for the heaps of internal and external cells.
static void noCellUpdate(Cell *, void *)
Default no-op update routine for a cell.
Element * top() const
Return the top element. nullptr for an empty heap.
externalBHeap external_
The heap of external cells.
void onCellUpdate(EventCellUpdate event, void *arg)
EventCellUpdate eventCellUpdate_
Pointer to function to be called when a cell needs to be updated.
virtual bool remove(Cell *cell)
Remove a cell from the grid.
This class defines a grid that keeps track of its boundary: it distinguishes between interior and ext...
void rebuild()
Rebuild the heap.
void clear()
Clear the heap.
bool empty() const
Check if the heap is empty.
void getCells(CellArray &cells) const
Get the set of instantiated cells in the grid.
static void setHeapElementE(typename externalBHeap::Element *element, void *)
Routine used internally for keeping track of binary heap elements for external cells.
virtual Cell * createCell(const Coord &coord, CellArray *nbh=nullptr)
Create a cell but do not add it to the grid; update neighboring cells however.
typename Grid< _T >::Coord Coord
Datatype for cell coordinates.
std::vector< Cell * > CellArray
The datatype for arrays of cells.
typename GridN< _T >::CellArray CellArray
The datatype for arrays of cells.
unsigned int numberOfBoundaryDimensions(const Coord &coord) const
Compute how many sides of a coordinate touch the boundaries of the grid.
virtual void add(Cell *cell)
Add the cell to the grid.
Cell * topInternal() const
Return the cell that is at the top of the heap maintaining internal cells.
unsigned int countInternal() const
Return the number of internal cells.
GridB(unsigned int dimension)
Constructor.
typename GridN< _T >::Cell Cell
Definition of a cell in this grid.
typename GridN< _T >::Coord Coord
Datatype for cell coordinates.
double fracInternal() const
Return the fraction of internal cells.
void(*)(Cell *, void *) EventCellUpdate
Event to be called when a cell's priority is to be updated.
void neighbors(const Cell *cell, CellArray &list) const
Get the list of neighbors for a given cell.
unsigned int interiorCellNeighborsLimit_
virtual void status(std::ostream &out=std::cout) const
Print information about the data in this grid structure.
iterator end() const
Return the end() iterator for the grid.
void update(Cell *cell)
Update the position in the heaps for a particular cell.
virtual void add(Cell *cell)
Add an instantiated cell to the grid.
Cell * topExternal() const
Return the cell that is at the top of the heap maintaining external cells.
Element * insert(const _T &data)
Add a new element.
void remove(Element *element)
Remove a specific element.
void clearHeaps()
Clear the data from both heaps.
internalBHeap internal_
The heap of interior cells.
unsigned int countExternal() const
Return the number of external cells.
void update(Element *element)
Update an element in the heap.
void onAfterInsert(EventAfterInsert event, void *arg)
Set the event that gets called after insertion.
Representation of a grid where cells keep track of how many neighbors they have.
unsigned int size() const
Get the number of elements in the heap.
virtual void clear()
Clear all cells in the grid.
Main namespace. Contains everything in this library.