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 #include "ompl/util/DisableCompilerWarning.h"
43 
44 OMPL_PUSH_DISABLE_CLANG_WARNING(-Woverloaded-virtual)
45 
46 namespace ompl
47 {
50  template <typename _T, class LessThanExternal = std::less<_T>, class LessThanInternal = LessThanExternal>
51  class GridB : public GridN<_T>
52  {
53  public:
55  using Cell = typename GridN<_T>::Cell;
56  using BaseCell = typename GridN<_T>::BaseCell;
57 
59  using CellArray = typename GridN<_T>::CellArray;
60 
62  using Coord = typename GridN<_T>::Coord;
63 
64  protected:
66  // the type of cell here needs an extra pointer to allow the updatable heap to work fast
67  // however, this stays hidden from the user
68  struct CellX : public Cell
69  {
70  CellX() : Cell()
71  {
72  }
73 
74  ~CellX() override = default;
75 
76  void *heapElement;
77 
78  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
79  };
80 
82 
83  public:
85  using EventCellUpdate = void (*)(Cell *, void *);
86 
88  explicit GridB(unsigned int dimension) : GridN<_T>(dimension)
89  {
90  setupHeaps();
91  }
92 
93  ~GridB() override
94  {
95  clearHeaps();
96  }
97 
100  void onCellUpdate(EventCellUpdate event, void *arg)
101  {
102  eventCellUpdate_ = event;
103  eventCellUpdateData_ = arg;
104  }
105 
107  Cell *topInternal() const
108  {
109  auto *top = static_cast<Cell *>(internal_.top()->data);
110  return top ? top : topExternal();
111  }
112 
114  Cell *topExternal() const
115  {
116  auto *top = static_cast<Cell *>(external_.top()->data);
117  return top ? top : topInternal();
118  }
119 
121  unsigned int countInternal() const
122  {
123  return internal_.size();
124  }
125 
127  unsigned int countExternal() const
128  {
129  return external_.size();
130  }
131 
133  double fracExternal() const
134  {
135  return external_.empty() ? 0.0 : (double)(external_.size()) / (double)(external_.size() + internal_.size());
136  }
137 
139  double fracInternal() const
140  {
141  return 1.0 - fracExternal();
142  }
143 
145  void update(Cell *cell)
146  {
147  eventCellUpdate_(cell, eventCellUpdateData_);
148  if (cell->border)
149  external_.update(
150  reinterpret_cast<typename externalBHeap::Element *>(static_cast<CellX *>(cell)->heapElement));
151  else
152  internal_.update(
153  reinterpret_cast<typename internalBHeap::Element *>(static_cast<CellX *>(cell)->heapElement));
154  }
155 
157  void updateAll()
158  {
159  std::vector<Cell *> cells;
160  this->getCells(cells);
161  for (int i = cells.size() - 1; i >= 0; --i)
162  eventCellUpdate_(cells[i], eventCellUpdateData_);
163  external_.rebuild();
164  internal_.rebuild();
165  }
166 
168  virtual Cell *createCell(const Coord &coord, CellArray *nbh = nullptr)
169  {
170  auto *cell = new CellX();
171  cell->coord = coord;
172 
173  CellArray *list = nbh ? nbh : new CellArray();
174  this->neighbors(cell->coord, *list);
175 
176  for (auto cl = list->begin(); cl != list->end(); ++cl)
177  {
178  auto *c = static_cast<CellX *>(*cl);
179  bool wasBorder = c->border;
180  c->neighbors++;
181  if (c->border && c->neighbors >= GridN<_T>::interiorCellNeighborsLimit_)
182  c->border = false;
183 
184  eventCellUpdate_(c, eventCellUpdateData_);
185 
186  if (c->border)
187  external_.update(reinterpret_cast<typename externalBHeap::Element *>(c->heapElement));
188  else
189  {
190  if (wasBorder)
191  {
192  external_.remove(reinterpret_cast<typename externalBHeap::Element *>(c->heapElement));
193  internal_.insert(c);
194  }
195  else
196  internal_.update(reinterpret_cast<typename internalBHeap::Element *>(c->heapElement));
197  }
198  }
199 
200  cell->neighbors = GridN<_T>::numberOfBoundaryDimensions(cell->coord) + list->size();
201  if (cell->border && cell->neighbors >= GridN<_T>::interiorCellNeighborsLimit_)
202  cell->border = false;
203 
204  if (!nbh)
205  delete list;
206 
207  return static_cast<Cell *>(cell);
208  }
209 
211  virtual void add(Cell *cell)
212  {
213  auto *ccell = static_cast<CellX *>(cell);
214  eventCellUpdate_(ccell, eventCellUpdateData_);
215 
216  GridN<_T>::add(cell);
217 
218  if (cell->border)
219  external_.insert(ccell);
220  else
221  internal_.insert(ccell);
222  }
223 
225  bool remove(BaseCell *cell) override
226  {
227  if (cell)
228  {
229  auto *list = new CellArray();
230  this->neighbors(cell->coord, *list);
231 
232  for (auto cl = list->begin(); cl != list->end(); ++cl)
233  {
234  auto *c = static_cast<CellX *>(*cl);
235  bool wasBorder = c->border;
236  c->neighbors--;
237  if (!c->border && c->neighbors < GridN<_T>::interiorCellNeighborsLimit_)
238  c->border = true;
239 
240  eventCellUpdate_(c, eventCellUpdateData_);
241 
242  if (c->border)
243  {
244  if (wasBorder)
245  external_.update(reinterpret_cast<typename externalBHeap::Element *>(c->heapElement));
246  else
247  {
248  internal_.remove(reinterpret_cast<typename internalBHeap::Element *>(c->heapElement));
249  external_.insert(c);
250  }
251  }
252  else
253  internal_.update(reinterpret_cast<typename internalBHeap::Element *>(c->heapElement));
254  }
255 
256  delete list;
257 
258  auto pos = GridN<_T>::hash_.find(&cell->coord);
259  if (pos != GridN<_T>::hash_.end())
260  {
261  GridN<_T>::hash_.erase(pos);
262  auto *cx = static_cast<CellX *>(cell);
263  if (cx->border)
264  external_.remove(reinterpret_cast<typename externalBHeap::Element *>(cx->heapElement));
265  else
266  internal_.remove(reinterpret_cast<typename internalBHeap::Element *>(cx->heapElement));
267  return true;
268  }
269  }
270  return false;
271  }
272 
273  void clear() override
274  {
276  clearHeaps();
277  }
278 
279  void status(std::ostream &out = std::cout) const override
280  {
281  GridN<_T>::status(out);
282  out << countInternal() << " internal cells" << std::endl;
283  out << countExternal() << " external cells" << std::endl;
284  }
285 
286  protected:
288  EventCellUpdate eventCellUpdate_;
289 
291  void *eventCellUpdateData_;
292 
294  static void noCellUpdate(Cell * /*unused*/, void * /*unused*/)
295  {
296  }
297 
299  void setupHeaps()
300  {
301  eventCellUpdate_ = &noCellUpdate;
302  eventCellUpdateData_ = nullptr;
303  internal_.onAfterInsert(&setHeapElementI, nullptr);
304  external_.onAfterInsert(&setHeapElementE, nullptr);
305  }
306 
308  void clearHeaps()
309  {
310  internal_.clear();
311  external_.clear();
312  }
313 
315  struct LessThanInternalCell
316  {
317  bool operator()(const CellX *const a, const CellX *const b) const
318  {
319  return lt_(a->data, b->data);
320  }
321 
322  private:
323  LessThanInternal lt_;
324  };
325 
327  struct LessThanExternalCell
328  {
329  bool operator()(const CellX *const a, const CellX *const b) const
330  {
331  return lt_(a->data, b->data);
332  }
333 
334  private:
335  LessThanExternal lt_;
336  };
337 
340 
343 
345  static void setHeapElementI(typename internalBHeap::Element *element, void * /*unused*/)
346  {
347  element->data->heapElement = reinterpret_cast<void *>(element);
348  }
349 
351  static void setHeapElementE(typename externalBHeap::Element *element, void * /*unused*/)
352  {
353  element->data->heapElement = reinterpret_cast<void *>(element);
354  }
355 
357  internalBHeap internal_;
358 
360  externalBHeap external_;
361  };
362 }
363 
364 OMPL_POP_CLANG
365 
366 #endif
This class defines a grid that keeps track of its boundary: it distinguishes between interior and ext...
Definition: GridB.h:83
bool remove(BaseCell *cell) override
Definition: GridN.h:249
std::vector< Cell * > CellArray
The datatype for arrays of cells.
Definition: Grid.h:138
typename GridN< _T >::Cell Cell
Definition of a cell in this grid.
Definition: GridB.h:119
void(*)(Cell *, void *) EventCellUpdate
Event to be called when a cell's priority is to be updated.
Definition: GridB.h:149
Representation of a grid where cells keep track of how many neighbors they have.
Definition: GridN.h:78
Main namespace. Contains everything in this library.
Definition: AppBase.h:21