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