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 {
47  template <typename _T, class LessThanExternal = std::less<_T>, class LessThanInternal = LessThanExternal>
48  class GridB : public GridN<_T>
49  {
50  public:
52  using Cell = typename GridN<_T>::Cell;
53 
55  using CellArray = typename GridN<_T>::CellArray;
56 
58  using Coord = typename GridN<_T>::Coord;
59 
60  protected:
62  // the type of cell here needs an extra pointer to allow the updatable heap to work fast
63  // however, this stays hidden from the user
64  struct CellX : public Cell
65  {
66  CellX() : Cell()
67  {
68  }
69 
70  ~CellX() override = default;
71 
72  void *heapElement;
73  };
74 
76 
77  public:
79  using EventCellUpdate = void (*)(Cell *, void *);
80 
82  explicit GridB(unsigned int dimension) : GridN<_T>(dimension)
83  {
84  setupHeaps();
85  }
86 
87  ~GridB() override
88  {
89  clearHeaps();
90  }
91 
94  void onCellUpdate(EventCellUpdate event, void *arg)
95  {
96  eventCellUpdate_ = event;
98  }
99 
101  Cell *topInternal() const
102  {
103  auto *top = static_cast<Cell *>(internal_.top()->data);
104  return top ? top : topExternal();
105  }
106 
108  Cell *topExternal() const
109  {
110  auto *top = static_cast<Cell *>(external_.top()->data);
111  return top ? top : topInternal();
112  }
113 
115  unsigned int countInternal() const
116  {
117  return internal_.size();
118  }
119 
121  unsigned int countExternal() const
122  {
123  return external_.size();
124  }
125 
127  double fracExternal() const
128  {
129  return external_.empty() ? 0.0 : (double)(external_.size()) / (double)(external_.size() + internal_.size());
130  }
131 
133  double fracInternal() const
134  {
135  return 1.0 - fracExternal();
136  }
137 
139  void update(Cell *cell)
140  {
142  if (cell->border)
144  reinterpret_cast<typename externalBHeap::Element *>(static_cast<CellX *>(cell)->heapElement));
145  else
147  reinterpret_cast<typename internalBHeap::Element *>(static_cast<CellX *>(cell)->heapElement));
148  }
149 
151  void updateAll()
152  {
153  std::vector<Cell *> cells;
154  this->getCells(cells);
155  for (int i = cells.size() - 1; i >= 0; --i)
157  external_.rebuild();
158  internal_.rebuild();
159  }
160 
162  virtual Cell *createCell(const Coord &coord, CellArray *nbh = nullptr)
163  {
164  auto *cell = new CellX();
165  cell->coord = coord;
166 
167  CellArray *list = nbh ? nbh : new CellArray();
168  this->neighbors(cell->coord, *list);
169 
170  for (auto cl = list->begin(); cl != list->end(); ++cl)
171  {
172  auto *c = static_cast<CellX *>(*cl);
173  bool wasBorder = c->border;
174  c->neighbors++;
175  if (c->border && c->neighbors >= GridN<_T>::interiorCellNeighborsLimit_)
176  c->border = false;
177 
179 
180  if (c->border)
181  external_.update(reinterpret_cast<typename externalBHeap::Element *>(c->heapElement));
182  else
183  {
184  if (wasBorder)
185  {
186  external_.remove(reinterpret_cast<typename externalBHeap::Element *>(c->heapElement));
187  internal_.insert(c);
188  }
189  else
190  internal_.update(reinterpret_cast<typename internalBHeap::Element *>(c->heapElement));
191  }
192  }
193 
194  cell->neighbors = GridN<_T>::numberOfBoundaryDimensions(cell->coord) + list->size();
195  if (cell->border && cell->neighbors >= GridN<_T>::interiorCellNeighborsLimit_)
196  cell->border = false;
197 
198  if (!nbh)
199  delete list;
200 
201  return static_cast<Cell *>(cell);
202  }
203 
205  virtual void add(Cell *cell)
206  {
207  auto *ccell = static_cast<CellX *>(cell);
209 
210  GridN<_T>::add(cell);
211 
212  if (cell->border)
213  external_.insert(ccell);
214  else
215  internal_.insert(ccell);
216  }
217 
219  virtual bool remove(Cell *cell)
220  {
221  if (cell)
222  {
223  auto *list = new CellArray();
224  this->neighbors(cell->coord, *list);
225 
226  for (auto cl = list->begin(); cl != list->end(); ++cl)
227  {
228  auto *c = static_cast<CellX *>(*cl);
229  bool wasBorder = c->border;
230  c->neighbors--;
231  if (!c->border && c->neighbors < GridN<_T>::interiorCellNeighborsLimit_)
232  c->border = true;
233 
235 
236  if (c->border)
237  {
238  if (wasBorder)
239  external_.update(reinterpret_cast<typename externalBHeap::Element *>(c->heapElement));
240  else
241  {
242  internal_.remove(reinterpret_cast<typename internalBHeap::Element *>(c->heapElement));
243  external_.insert(c);
244  }
245  }
246  else
247  internal_.update(reinterpret_cast<typename internalBHeap::Element *>(c->heapElement));
248  }
249 
250  delete list;
251 
252  auto pos = GridN<_T>::hash_.find(&cell->coord);
253  if (pos != GridN<_T>::hash_.end())
254  {
255  GridN<_T>::hash_.erase(pos);
256  auto *cx = static_cast<CellX *>(cell);
257  if (cx->border)
258  external_.remove(reinterpret_cast<typename externalBHeap::Element *>(cx->heapElement));
259  else
260  internal_.remove(reinterpret_cast<typename internalBHeap::Element *>(cx->heapElement));
261  return true;
262  }
263  }
264  return false;
265  }
266 
267  void clear() override
268  {
270  clearHeaps();
271  }
272 
273  void status(std::ostream &out = std::cout) const override
274  {
275  GridN<_T>::status(out);
276  out << countInternal() << " internal cells" << std::endl;
277  out << countExternal() << " external cells" << std::endl;
278  }
279 
280  protected:
283 
286 
288  static void noCellUpdate(Cell * /*unused*/, void * /*unused*/)
289  {
290  }
291 
293  void setupHeaps()
294  {
295  eventCellUpdate_ = &noCellUpdate;
296  eventCellUpdateData_ = nullptr;
299  }
300 
302  void clearHeaps()
303  {
304  internal_.clear();
305  external_.clear();
306  }
307 
310  {
311  bool operator()(const CellX *const a, const CellX *const b) const
312  {
313  return lt_(a->data, b->data);
314  }
315 
316  private:
317  LessThanInternal lt_;
318  };
319 
322  {
323  bool operator()(const CellX *const a, const CellX *const b) const
324  {
325  return lt_(a->data, b->data);
326  }
327 
328  private:
329  LessThanExternal lt_;
330  };
331 
334 
337 
339  static void setHeapElementI(typename internalBHeap::Element *element, void * /*unused*/)
340  {
341  element->data->heapElement = reinterpret_cast<void *>(element);
342  }
343 
345  static void setHeapElementE(typename externalBHeap::Element *element, void * /*unused*/)
346  {
347  element->data->heapElement = reinterpret_cast<void *>(element);
348  }
349 
352 
355  };
356 }
357 
358 #endif
virtual void add(Cell *cell)
Add the cell to the grid.
Definition: GridB.h:205
internalBHeap internal_
The heap of interior cells.
Definition: GridB.h:351
typename GridN< _T >::CellArray CellArray
The datatype for arrays of cells.
Definition: GridB.h:55
unsigned int countInternal() const
Return the number of internal cells.
Definition: GridB.h:115
void updateAll()
Update all cells and reconstruct the heaps.
Definition: GridB.h:151
double fracExternal() const
Return the fraction of external cells.
Definition: GridB.h:127
void clear()
Clear the heap.
Definition: BinaryHeap.h:112
virtual void clear()
Clear all cells in the grid.
Definition: Grid.h:86
void onCellUpdate(EventCellUpdate event, void *arg)
Definition: GridB.h:94
void rebuild()
Rebuild the heap.
Definition: BinaryHeap.h:180
typename GridN< _T >::Cell Cell
Definition of a cell in this grid.
Definition: GridB.h:52
void neighbors(const Cell *cell, CellArray &list) const
Get the list of neighbors for a given cell.
Definition: GridN.h:127
void remove(Element *element)
Remove a specific element.
Definition: BinaryHeap.h:132
std::vector< int > Coord
Definition of a coordinate within this grid.
Definition: Grid.h:54
void update(Cell *cell)
Update the position in the heaps for a particular cell.
Definition: GridB.h:139
bool empty() const
Check if the heap is empty.
Definition: BinaryHeap.h:195
void update(Element *element)
Update an element in the heap.
Definition: BinaryHeap.h:186
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
void(*)(Cell *, void *) EventCellUpdate
Event to be called when a cell&#39;s priority is to be updated.
Definition: GridB.h:79
virtual void status(std::ostream &out=std::cout) const
Print information about the data in this grid structure.
Definition: Grid.h:297
GridB(unsigned int dimension)
Constructor.
Definition: GridB.h:82
virtual void add(Cell *cell)
Add an instantiated cell to the grid.
Definition: Grid.h:243
EventCellUpdate eventCellUpdate_
Pointer to function to be called when a cell needs to be updated.
Definition: GridB.h:282
Define order for internal cells.
Definition: GridB.h:309
Define order for external cells.
Definition: GridB.h:321
Representation of a grid where cells keep track of how many neighbors they have.
Definition: GridN.h:46
Cell * topExternal() const
Return the cell that is at the top of the heap maintaining external cells.
Definition: GridB.h:108
std::vector< Cell *> CellArray
The datatype for arrays of cells.
Definition: Grid.h:71
static void setHeapElementE(typename externalBHeap::Element *element, void *)
Routine used internally for keeping track of binary heap elements for external cells.
Definition: GridB.h:345
double fracInternal() const
Return the fraction of internal cells.
Definition: GridB.h:133
static void setHeapElementI(typename internalBHeap::Element *element, void *)
Routine used internally for keeping track of binary heap elements for internal cells.
Definition: GridB.h:339
typename Grid< _T >::Coord Coord
Datatype for cell coordinates.
Definition: GridN.h:56
void onAfterInsert(EventAfterInsert event, void *arg)
Set the event that gets called after insertion.
Definition: BinaryHeap.h:98
static void noCellUpdate(Cell *, void *)
Default no-op update routine for a cell.
Definition: GridB.h:288
void * eventCellUpdateData_
Data to be passed to function pointer above.
Definition: GridB.h:285
Element * top() const
Return the top element. nullptr for an empty heap.
Definition: BinaryHeap.h:120
iterator end() const
Return the end() iterator for the grid.
Definition: Grid.h:372
unsigned int countExternal() const
Return the number of external cells.
Definition: GridB.h:121
void getCells(CellArray &cells) const
Get the set of instantiated cells in the grid.
Definition: GridN.h:208
void setupHeaps()
Set the update procedure for the heaps of internal and external cells.
Definition: GridB.h:293
externalBHeap external_
The heap of external cells.
Definition: GridB.h:354
Cell * topInternal() const
Return the cell that is at the top of the heap maintaining internal cells.
Definition: GridB.h:101
This class defines a grid that keeps track of its boundary: it distinguishes between interior and ext...
Definition: GridB.h:48
void clear() override
Clear all cells in the grid.
Definition: GridB.h:267
Definition of a cell in this grid.
Definition: GridN.h:59
void status(std::ostream &out=std::cout) const override
Print information about the data in this grid structure.
Definition: GridB.h:273
unsigned int size() const
Get the number of elements in the heap.
Definition: BinaryHeap.h:201
unsigned int numberOfBoundaryDimensions(const Coord &coord) const
Compute how many sides of a coordinate touch the boundaries of the grid.
Definition: GridN.h:216
void clearHeaps()
Clear the data from both heaps.
Definition: GridB.h:302
Element * insert(const _T &data)
Add a new element.
Definition: BinaryHeap.h:140
std::vector< Cell * > CellArray
The datatype for arrays of cells.
Definition: GridN.h:73
virtual Cell * createCell(const Coord &coord, CellArray *nbh=nullptr)
Create a cell but do not add it to the grid; update neighboring cells however.
Definition: GridB.h:162