GridB.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 #ifndef OMPL_DATASTRUCTURES_GRID_B_
38 #define OMPL_DATASTRUCTURES_GRID_B_
39 
40 #include "ompl/datastructures/GridN.h"
41 #include "ompl/datastructures/BinaryHeap.h"
42 
43 namespace ompl
44 {
45 
48  template < typename _T,
49  class LessThanExternal = std::less<_T>,
50  class LessThanInternal = LessThanExternal >
51  class GridB : public GridN<_T>
52  {
53  public:
54 
56  typedef typename GridN<_T>::Cell Cell;
57 
59  typedef typename GridN<_T>::CellArray CellArray;
60 
62  typedef typename GridN<_T>::Coord Coord;
63 
64  protected:
65 
67  // the type of cell here needs an extra pointer to allow the updatable heap to work fast
68  // however, this stays hidden from the user
69  struct CellX : public Cell
70  {
71  CellX() : Cell()
72  {
73  }
74 
75  virtual ~CellX()
76  {
77  }
78 
79  void *heapElement;
80  };
81 
83 
84  public:
85 
87  typedef void (*EventCellUpdate)(Cell*, void*);
88 
90  explicit
91  GridB(unsigned int dimension) : GridN<_T>(dimension)
92  {
93  setupHeaps();
94  }
95 
96  virtual ~GridB()
97  {
98  clearHeaps();
99  }
100 
103  void onCellUpdate(EventCellUpdate event, void *arg)
104  {
105  eventCellUpdate_ = event;
106  eventCellUpdateData_ = arg;
107  }
108 
110  Cell* topInternal() const
111  {
112  Cell* top = static_cast<Cell*>(internal_.top()->data);
113  return top ? top : topExternal();
114  }
115 
117  Cell* topExternal() const
118  {
119  Cell* top = static_cast<Cell*>(external_.top()->data);
120  return top ? top : topInternal();
121  }
122 
124  unsigned int countInternal() const
125  {
126  return internal_.size();
127  }
128 
130  unsigned int countExternal() const
131  {
132  return external_.size();
133  }
134 
136  double fracExternal() const
137  {
138  return external_.empty() ? 0.0 : (double)(external_.size()) / (double)(external_.size() + internal_.size());
139  }
140 
142  double fracInternal() const
143  {
144  return 1.0 - fracExternal();
145  }
146 
148  void update(Cell* cell)
149  {
151  if (cell->border)
152  external_.update(reinterpret_cast<typename externalBHeap::Element*>
153  (static_cast<CellX*>(cell)->heapElement));
154  else
155  internal_.update(reinterpret_cast<typename internalBHeap::Element*>
156  (static_cast<CellX*>(cell)->heapElement));
157  }
158 
160  void updateAll()
161  {
162  std::vector< Cell* > cells;
163  this->getCells(cells);
164  for (int i = cells.size() - 1 ; i >= 0 ; --i)
166  external_.rebuild();
167  internal_.rebuild();
168  }
169 
171  virtual Cell* createCell(const Coord& coord, CellArray *nbh = NULL)
172  {
173  CellX* cell = new CellX();
174  cell->coord = coord;
175 
176  CellArray *list = nbh ? nbh : new CellArray();
177  this->neighbors(cell->coord, *list);
178 
179  for (typename CellArray::iterator cl = list->begin() ; cl != list->end() ; ++cl)
180  {
181  CellX* c = static_cast<CellX*>(*cl);
182  bool wasBorder = c->border;
183  c->neighbors++;
184  if (c->border && c->neighbors >= GridN<_T>::interiorCellNeighborsLimit_)
185  c->border = false;
186 
188 
189  if (c->border)
190  external_.update(reinterpret_cast<typename externalBHeap::Element*>(c->heapElement));
191  else
192  {
193  if (wasBorder)
194  {
195  external_.remove(reinterpret_cast<typename externalBHeap::Element*>(c->heapElement));
196  internal_.insert(c);
197  }
198  else
199  internal_.update(reinterpret_cast<typename internalBHeap::Element*>(c->heapElement));
200  }
201  }
202 
203  cell->neighbors = GridN<_T>::numberOfBoundaryDimensions(cell->coord) + list->size();
204  if (cell->border && cell->neighbors >= GridN<_T>::interiorCellNeighborsLimit_)
205  cell->border = false;
206 
207  if (!nbh)
208  delete list;
209 
210  return static_cast<Cell*>(cell);
211  }
212 
214  virtual void add(Cell* cell)
215  {
216  CellX* ccell = static_cast<CellX*>(cell);
218 
219  GridN<_T>::add(cell);
220 
221  if (cell->border)
222  external_.insert(ccell);
223  else
224  internal_.insert(ccell);
225  }
226 
228  virtual bool remove(Cell* cell)
229  {
230  if (cell)
231  {
232  CellArray *list = new CellArray();
233  this->neighbors(cell->coord, *list);
234 
235  for (typename CellArray::iterator cl = list->begin() ; cl != list->end() ; ++cl)
236  {
237  CellX* c = static_cast<CellX*>(*cl);
238  bool wasBorder = c->border;
239  c->neighbors--;
240  if (!c->border && c->neighbors < GridN<_T>::interiorCellNeighborsLimit_)
241  c->border = true;
242 
244 
245  if (c->border)
246  {
247  if (wasBorder)
248  external_.update(reinterpret_cast<typename externalBHeap::Element*>(c->heapElement));
249  else
250  {
251  internal_.remove(reinterpret_cast<typename internalBHeap::Element*>(c->heapElement));
252  external_.insert(c);
253  }
254  }
255  else
256  internal_.update(reinterpret_cast<typename internalBHeap::Element*>(c->heapElement));
257  }
258 
259  delete list;
260 
261  typename GridN<_T>::CoordHash::iterator pos = GridN<_T>::hash_.find(&cell->coord);
262  if (pos != GridN<_T>::hash_.end())
263  {
264  GridN<_T>::hash_.erase(pos);
265  CellX* cx = static_cast<CellX*>(cell);
266  if (cx->border)
267  external_.remove(reinterpret_cast<typename externalBHeap::Element*>(cx->heapElement));
268  else
269  internal_.remove(reinterpret_cast<typename internalBHeap::Element*>(cx->heapElement));
270  return true;
271  }
272  }
273  return false;
274  }
275 
276  virtual void clear()
277  {
279  clearHeaps();
280  }
281 
282  virtual void status(std::ostream &out = std::cout) const
283  {
284  GridN<_T>::status(out);
285  out << countInternal() << " internal cells" << std::endl;
286  out << countExternal() << " external cells" << std::endl;
287  }
288 
289  protected:
290 
293 
296 
298  static void noCellUpdate(Cell*, void*)
299  {
300  }
301 
303  void setupHeaps()
304  {
305  eventCellUpdate_ = &noCellUpdate;
306  eventCellUpdateData_ = NULL;
309  }
310 
312  void clearHeaps()
313  {
314  internal_.clear();
315  external_.clear();
316  }
317 
320  {
321  bool operator()(const CellX* const a, const CellX* const b) const
322  {
323  return lt_(a->data, b->data);
324  }
325 
326  private:
327  LessThanInternal lt_;
328  };
329 
332  {
333  bool operator()(const CellX* const a, const CellX* const b) const
334  {
335  return lt_(a->data, b->data);
336  }
337  private:
338  LessThanExternal lt_;
339  };
340 
343 
346 
348  static void setHeapElementI(typename internalBHeap::Element *element, void*)
349  {
350  element->data->heapElement = reinterpret_cast<void*>(element);
351  }
352 
354  static void setHeapElementE(typename externalBHeap::Element *element, void*)
355  {
356  element->data->heapElement = reinterpret_cast<void*>(element);
357  }
358 
360  internalBHeap internal_;
361 
363  externalBHeap external_;
364  };
365 
366 }
367 
368 #endif
Cell * topExternal() const
Return the cell that is at the top of the heap maintaining external cells.
Definition: GridB.h:117
virtual void add(Cell *cell)
Add the cell to the grid.
Definition: GridB.h:214
internalBHeap internal_
The heap of interior cells.
Definition: GridB.h:360
Cell * topInternal() const
Return the cell that is at the top of the heap maintaining internal cells.
Definition: GridB.h:110
void updateAll()
Update all cells and reconstruct the heaps.
Definition: GridB.h:160
GridN< _T >::Coord Coord
Datatype for cell coordinates.
Definition: GridB.h:62
void clear()
Clear the heap.
Definition: BinaryHeap.h:106
virtual void clear()
Clear all cells in the grid.
Definition: Grid.h:94
void onCellUpdate(EventCellUpdate event, void *arg)
Definition: GridB.h:103
void rebuild()
Rebuild the heap.
Definition: BinaryHeap.h:175
virtual void status(std::ostream &out=std::cout) const
Print information about the data in this grid structure.
Definition: Grid.h:306
void remove(Element *element)
Remove a specific element.
Definition: BinaryHeap.h:127
void getCells(CellArray &cells) const
Get the set of instantiated cells in the grid.
Definition: GridN.h:219
virtual void status(std::ostream &out=std::cout) const
Print information about the data in this grid structure.
Definition: GridB.h:282
double fracInternal() const
Return the fraction of internal cells.
Definition: GridB.h:142
GridN< _T >::Cell Cell
Definition of a cell in this grid.
Definition: GridB.h:56
unsigned int countInternal() const
Return the number of internal cells.
Definition: GridB.h:124
void update(Cell *cell)
Update the position in the heaps for a particular cell.
Definition: GridB.h:148
void update(Element *element)
Update an element in the heap.
Definition: BinaryHeap.h:181
unsigned int numberOfBoundaryDimensions(const Coord &coord) const
Compute how many sides of a coordinate touch the boundaries of the grid.
Definition: GridN.h:229
Grid< _T >::Coord Coord
Datatype for cell coordinates.
Definition: GridN.h:58
Main namespace. Contains everything in this library.
Definition: Cost.h:42
GridB(unsigned int dimension)
Constructor.
Definition: GridB.h:91
Element * top() const
Return the top element. NULL for an empty heap.
Definition: BinaryHeap.h:115
virtual void add(Cell *cell)
Add an instantiated cell to the grid.
Definition: Grid.h:252
virtual void clear()
Clear all cells in the grid.
Definition: GridB.h:276
EventCellUpdate eventCellUpdate_
Pointer to function to be called when a cell needs to be updated.
Definition: GridB.h:292
Define order for internal cells.
Definition: GridB.h:319
Define order for external cells.
Definition: GridB.h:331
BinaryHeap< CellX *, LessThanInternalCell > internalBHeap
Datatype for a heap of cells containing interior cells.
Definition: GridB.h:342
Representation of a grid where cells keep track of how many neighbors they have.
Definition: GridN.h:47
bool empty() const
Check if the heap is empty.
Definition: BinaryHeap.h:190
unsigned int countExternal() const
Return the number of external cells.
Definition: GridB.h:130
iterator end() const
Return the end() iterator for the grid.
Definition: Grid.h:383
std::vector< Cell * > CellArray
The datatype for arrays of cells.
Definition: GridN.h:79
static void setHeapElementE(typename externalBHeap::Element *element, void *)
Routine used internally for keeping track of binary heap elements for external cells.
Definition: GridB.h:354
static void setHeapElementI(typename internalBHeap::Element *element, void *)
Routine used internally for keeping track of binary heap elements for internal cells.
Definition: GridB.h:348
void onAfterInsert(EventAfterInsert event, void *arg)
Set the event that gets called after insertion.
Definition: BinaryHeap.h:92
void(* EventCellUpdate)(Cell *, void *)
Event to be called when a cell's priority is to be updated.
Definition: GridB.h:87
BinaryHeap< CellX *, LessThanExternalCell > externalBHeap
Datatype for a heap of cells containing exterior cells.
Definition: GridB.h:345
static void noCellUpdate(Cell *, void *)
Default no-op update routine for a cell.
Definition: GridB.h:298
void * eventCellUpdateData_
Data to be passed to function pointer above.
Definition: GridB.h:295
GridN< _T >::CellArray CellArray
The datatype for arrays of cells.
Definition: GridB.h:59
void neighbors(const Cell *cell, CellArray &list) const
Get the list of neighbors for a given cell.
Definition: GridN.h:138
unsigned int size() const
Get the number of elements in the heap.
Definition: BinaryHeap.h:196
virtual Cell * createCell(const Coord &coord, CellArray *nbh=NULL)
Create a cell but do not add it to the grid; update neighboring cells however.
Definition: GridB.h:171
void setupHeaps()
Set the update procedure for the heaps of internal and external cells.
Definition: GridB.h:303
externalBHeap external_
The heap of external cells.
Definition: GridB.h:363
This class defines a grid that keeps track of its boundary: it distinguishes between interior and ext...
Definition: GridB.h:51
Definition of a cell in this grid.
Definition: GridN.h:61
void clearHeaps()
Clear the data from both heaps.
Definition: GridB.h:312
Element * insert(const _T &data)
Add a new element.
Definition: BinaryHeap.h:135
double fracExternal() const
Return the fraction of external cells.
Definition: GridB.h:136