37 #ifndef OMPL_DATASTRUCTURES_GRID_B_ 38 #define OMPL_DATASTRUCTURES_GRID_B_ 40 #include "ompl/datastructures/GridN.h" 41 #include "ompl/datastructures/BinaryHeap.h" 48 template <
typename _T,
49 class LessThanExternal = std::less<_T>,
50 class LessThanInternal = LessThanExternal >
69 struct CellX :
public Cell
153 (static_cast<CellX*>(cell)->heapElement));
156 (static_cast<CellX*>(cell)->heapElement));
162 std::vector< Cell* > cells;
164 for (
int i = cells.size() - 1 ; i >= 0 ; --i)
171 virtual Cell*
createCell(
const Coord& coord, CellArray *nbh = NULL)
173 CellX* cell =
new CellX();
176 CellArray *list = nbh ? nbh :
new CellArray();
179 for (
typename CellArray::iterator cl = list->begin() ; cl != list->end() ; ++cl)
181 CellX* c =
static_cast<CellX*
>(*cl);
182 bool wasBorder = c->border;
190 external_.
update(reinterpret_cast<typename externalBHeap::Element*>(c->heapElement));
195 external_.
remove(reinterpret_cast<typename externalBHeap::Element*>(c->heapElement));
199 internal_.
update(reinterpret_cast<typename internalBHeap::Element*>(c->heapElement));
205 cell->border =
false;
210 return static_cast<Cell*
>(cell);
214 virtual void add(Cell* cell)
216 CellX* ccell =
static_cast<CellX*
>(cell);
228 virtual bool remove(Cell* cell)
235 for (
typename CellArray::iterator cl = list->begin() ; cl != list->end() ; ++cl)
237 CellX* c =
static_cast<CellX*
>(*cl);
238 bool wasBorder = c->border;
248 external_.
update(reinterpret_cast<typename externalBHeap::Element*>(c->heapElement));
251 internal_.
remove(reinterpret_cast<typename internalBHeap::Element*>(c->heapElement));
256 internal_.
update(reinterpret_cast<typename internalBHeap::Element*>(c->heapElement));
265 CellX* cx =
static_cast<CellX*
>(cell);
267 external_.
remove(reinterpret_cast<typename externalBHeap::Element*>(cx->heapElement));
269 internal_.
remove(reinterpret_cast<typename internalBHeap::Element*>(cx->heapElement));
282 virtual void status(std::ostream &out = std::cout)
const 306 eventCellUpdateData_ = NULL;
321 bool operator()(
const CellX*
const a,
const CellX*
const b)
const 323 return lt_(a->data, b->data);
327 LessThanInternal lt_;
333 bool operator()(
const CellX*
const a,
const CellX*
const b)
const 335 return lt_(a->data, b->data);
338 LessThanExternal lt_;
350 element->data->heapElement =
reinterpret_cast<void*
>(element);
356 element->data->heapElement =
reinterpret_cast<void*
>(element);
Cell * topExternal() const
Return the cell that is at the top of the heap maintaining external cells.
virtual void add(Cell *cell)
Add the cell to the grid.
internalBHeap internal_
The heap of interior cells.
Cell * topInternal() const
Return the cell that is at the top of the heap maintaining internal cells.
void updateAll()
Update all cells and reconstruct the heaps.
GridN< _T >::Coord Coord
Datatype for cell coordinates.
void clear()
Clear the heap.
virtual void clear()
Clear all cells in the grid.
void onCellUpdate(EventCellUpdate event, void *arg)
void rebuild()
Rebuild the heap.
virtual void status(std::ostream &out=std::cout) const
Print information about the data in this grid structure.
void remove(Element *element)
Remove a specific element.
void getCells(CellArray &cells) const
Get the set of instantiated cells in the grid.
virtual void status(std::ostream &out=std::cout) const
Print information about the data in this grid structure.
double fracInternal() const
Return the fraction of internal cells.
GridN< _T >::Cell Cell
Definition of a cell in this grid.
unsigned int countInternal() const
Return the number of internal cells.
void update(Cell *cell)
Update the position in the heaps for a particular cell.
void update(Element *element)
Update an element in the heap.
unsigned int numberOfBoundaryDimensions(const Coord &coord) const
Compute how many sides of a coordinate touch the boundaries of the grid.
Grid< _T >::Coord Coord
Datatype for cell coordinates.
Main namespace. Contains everything in this library.
GridB(unsigned int dimension)
Constructor.
Element * top() const
Return the top element. NULL for an empty heap.
virtual void add(Cell *cell)
Add an instantiated cell to the grid.
virtual void clear()
Clear all cells in the grid.
EventCellUpdate eventCellUpdate_
Pointer to function to be called when a cell needs to be updated.
Define order for internal cells.
Define order for external cells.
BinaryHeap< CellX *, LessThanInternalCell > internalBHeap
Datatype for a heap of cells containing interior cells.
Representation of a grid where cells keep track of how many neighbors they have.
bool empty() const
Check if the heap is empty.
unsigned int countExternal() const
Return the number of external cells.
iterator end() const
Return the end() iterator for the grid.
std::vector< Cell * > CellArray
The datatype for arrays of cells.
static void setHeapElementE(typename externalBHeap::Element *element, void *)
Routine used internally for keeping track of binary heap elements for external cells.
static void setHeapElementI(typename internalBHeap::Element *element, void *)
Routine used internally for keeping track of binary heap elements for internal cells.
void onAfterInsert(EventAfterInsert event, void *arg)
Set the event that gets called after insertion.
void(* EventCellUpdate)(Cell *, void *)
Event to be called when a cell's priority is to be updated.
BinaryHeap< CellX *, LessThanExternalCell > externalBHeap
Datatype for a heap of cells containing exterior cells.
static void noCellUpdate(Cell *, void *)
Default no-op update routine for a cell.
void * eventCellUpdateData_
Data to be passed to function pointer above.
GridN< _T >::CellArray CellArray
The datatype for arrays of cells.
void neighbors(const Cell *cell, CellArray &list) const
Get the list of neighbors for a given cell.
unsigned int size() const
Get the number of elements in the heap.
virtual Cell * createCell(const Coord &coord, CellArray *nbh=NULL)
Create a cell but do not add it to the grid; update neighboring cells however.
void setupHeaps()
Set the update procedure for the heaps of internal and external cells.
externalBHeap external_
The heap of external cells.
This class defines a grid that keeps track of its boundary: it distinguishes between interior and ext...
Definition of a cell in this grid.
void clearHeaps()
Clear the data from both heaps.
Element * insert(const _T &data)
Add a new element.
double fracExternal() const
Return the fraction of external cells.