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);
virtual void add(Cell *cell)
Add the cell to the grid.
internalBHeap internal_
The heap of interior cells.
unsigned int countInternal() const
Return the number of internal cells.
void updateAll()
Update all cells and reconstruct the heaps.
double fracExternal() const
Return the fraction of external cells.
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.
void neighbors(const Cell *cell, CellArray &list) const
Get the list of neighbors for a given cell.
void remove(Element *element)
Remove a specific element.
GridN< _T >::Cell Cell
Definition of a cell in this grid.
void update(Cell *cell)
Update the position in the heaps for a particular cell.
bool empty() const
Check if the heap is empty.
void update(Element *element)
Update an element in the heap.
Grid< _T >::Coord Coord
Datatype for cell coordinates.
Main namespace. Contains everything in this library.
virtual void status(std::ostream &out=std::cout) const
Print information about the data in this grid structure.
GridB(unsigned int dimension)
Constructor.
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.
bool border
A flag indicating whether this cell is on the border or not.
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.
Cell * topExternal() const
Return the cell that is at the top of the heap maintaining external cells.
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.
double fracInternal() const
Return the fraction of internal 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.
virtual void status(std::ostream &out=std::cout) const
Print information about the data in this grid structure.
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.
Element * top() const
Return the top element. NULL for an empty heap.
GridN< _T >::CellArray CellArray
The datatype for arrays of cells.
iterator end() const
Return the end() iterator for the grid.
virtual Cell * createCell(const Coord &coord, CellArray *nbh=NULL)
Create a cell but do not add it to the grid; update neighboring cells however.
unsigned int countExternal() const
Return the number of external cells.
void getCells(CellArray &cells) const
Get the set of instantiated cells in the grid.
void setupHeaps()
Set the update procedure for the heaps of internal and external cells.
externalBHeap external_
The heap of external cells.
Cell * topInternal() const
Return the cell that is at the top of the heap maintaining internal 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.
unsigned int size() const
Get the number of elements in the heap.
unsigned int numberOfBoundaryDimensions(const Coord &coord) const
Compute how many sides of a coordinate touch the boundaries of the grid.
void clearHeaps()
Clear the data from both heaps.
Element * insert(const _T &data)
Add a new element.