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  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
78  };
79 
81 
82  public:
84  using EventCellUpdate = void (*)(Cell *, void *);
85 
87  explicit GridB(unsigned int dimension) : GridN<_T>(dimension)
88  {
89  setupHeaps();
90  }
91 
92  ~GridB() override
93  {
94  clearHeaps();
95  }
96 
99  void onCellUpdate(EventCellUpdate event, void *arg)
100  {
101  eventCellUpdate_ = event;
102  eventCellUpdateData_ = arg;
103  }
104 
106  Cell *topInternal() const
107  {
108  auto *top = static_cast<Cell *>(internal_.top()->data);
109  return top ? top : topExternal();
110  }
111 
113  Cell *topExternal() const
114  {
115  auto *top = static_cast<Cell *>(external_.top()->data);
116  return top ? top : topInternal();
117  }
118 
120  unsigned int countInternal() const
121  {
122  return internal_.size();
123  }
124 
126  unsigned int countExternal() const
127  {
128  return external_.size();
129  }
130 
132  double fracExternal() const
133  {
134  return external_.empty() ? 0.0 : (double)(external_.size()) / (double)(external_.size() + internal_.size());
135  }
136 
138  double fracInternal() const
139  {
140  return 1.0 - fracExternal();
141  }
142 
144  void update(Cell *cell)
145  {
146  eventCellUpdate_(cell, eventCellUpdateData_);
147  if (cell->border)
148  external_.update(
149  reinterpret_cast<typename externalBHeap::Element *>(static_cast<CellX *>(cell)->heapElement));
150  else
151  internal_.update(
152  reinterpret_cast<typename internalBHeap::Element *>(static_cast<CellX *>(cell)->heapElement));
153  }
154 
156  void updateAll()
157  {
158  std::vector<Cell *> cells;
159  this->getCells(cells);
160  for (int i = cells.size() - 1; i >= 0; --i)
161  eventCellUpdate_(cells[i], eventCellUpdateData_);
162  external_.rebuild();
163  internal_.rebuild();
164  }
165 
167  virtual Cell *createCell(const Coord &coord, CellArray *nbh = nullptr)
168  {
169  auto *cell = new CellX();
170  cell->coord = coord;
171 
172  CellArray *list = nbh ? nbh : new CellArray();
173  this->neighbors(cell->coord, *list);
174 
175  for (auto cl = list->begin(); cl != list->end(); ++cl)
176  {
177  auto *c = static_cast<CellX *>(*cl);
178  bool wasBorder = c->border;
179  c->neighbors++;
180  if (c->border && c->neighbors >= GridN<_T>::interiorCellNeighborsLimit_)
181  c->border = false;
182 
183  eventCellUpdate_(c, eventCellUpdateData_);
184 
185  if (c->border)
186  external_.update(reinterpret_cast<typename externalBHeap::Element *>(c->heapElement));
187  else
188  {
189  if (wasBorder)
190  {
191  external_.remove(reinterpret_cast<typename externalBHeap::Element *>(c->heapElement));
192  internal_.insert(c);
193  }
194  else
195  internal_.update(reinterpret_cast<typename internalBHeap::Element *>(c->heapElement));
196  }
197  }
198 
199  cell->neighbors = GridN<_T>::numberOfBoundaryDimensions(cell->coord) + list->size();
200  if (cell->border && cell->neighbors >= GridN<_T>::interiorCellNeighborsLimit_)
201  cell->border = false;
202 
203  if (!nbh)
204  delete list;
205 
206  return static_cast<Cell *>(cell);
207  }
208 
210  virtual void add(Cell *cell)
211  {
212  auto *ccell = static_cast<CellX *>(cell);
213  eventCellUpdate_(ccell, eventCellUpdateData_);
214 
215  GridN<_T>::add(cell);
216 
217  if (cell->border)
218  external_.insert(ccell);
219  else
220  internal_.insert(ccell);
221  }
222 
224  virtual bool remove(Cell *cell)
225  {
226  if (cell)
227  {
228  auto *list = new CellArray();
229  this->neighbors(cell->coord, *list);
230 
231  for (auto cl = list->begin(); cl != list->end(); ++cl)
232  {
233  auto *c = static_cast<CellX *>(*cl);
234  bool wasBorder = c->border;
235  c->neighbors--;
236  if (!c->border && c->neighbors < GridN<_T>::interiorCellNeighborsLimit_)
237  c->border = true;
238 
239  eventCellUpdate_(c, eventCellUpdateData_);
240 
241  if (c->border)
242  {
243  if (wasBorder)
244  external_.update(reinterpret_cast<typename externalBHeap::Element *>(c->heapElement));
245  else
246  {
247  internal_.remove(reinterpret_cast<typename internalBHeap::Element *>(c->heapElement));
248  external_.insert(c);
249  }
250  }
251  else
252  internal_.update(reinterpret_cast<typename internalBHeap::Element *>(c->heapElement));
253  }
254 
255  delete list;
256 
257  auto pos = GridN<_T>::hash_.find(&cell->coord);
258  if (pos != GridN<_T>::hash_.end())
259  {
260  GridN<_T>::hash_.erase(pos);
261  auto *cx = static_cast<CellX *>(cell);
262  if (cx->border)
263  external_.remove(reinterpret_cast<typename externalBHeap::Element *>(cx->heapElement));
264  else
265  internal_.remove(reinterpret_cast<typename internalBHeap::Element *>(cx->heapElement));
266  return true;
267  }
268  }
269  return false;
270  }
271 
272  void clear() override
273  {
275  clearHeaps();
276  }
277 
278  void status(std::ostream &out = std::cout) const override
279  {
280  GridN<_T>::status(out);
281  out << countInternal() << " internal cells" << std::endl;
282  out << countExternal() << " external cells" << std::endl;
283  }
284 
285  protected:
287  EventCellUpdate eventCellUpdate_;
288 
290  void *eventCellUpdateData_;
291 
293  static void noCellUpdate(Cell * /*unused*/, void * /*unused*/)
294  {
295  }
296 
298  void setupHeaps()
299  {
300  eventCellUpdate_ = &noCellUpdate;
301  eventCellUpdateData_ = nullptr;
302  internal_.onAfterInsert(&setHeapElementI, nullptr);
303  external_.onAfterInsert(&setHeapElementE, nullptr);
304  }
305 
307  void clearHeaps()
308  {
309  internal_.clear();
310  external_.clear();
311  }
312 
314  struct LessThanInternalCell
315  {
316  bool operator()(const CellX *const a, const CellX *const b) const
317  {
318  return lt_(a->data, b->data);
319  }
320 
321  private:
322  LessThanInternal lt_;
323  };
324 
326  struct LessThanExternalCell
327  {
328  bool operator()(const CellX *const a, const CellX *const b) const
329  {
330  return lt_(a->data, b->data);
331  }
332 
333  private:
334  LessThanExternal lt_;
335  };
336 
339 
342 
344  static void setHeapElementI(typename internalBHeap::Element *element, void * /*unused*/)
345  {
346  element->data->heapElement = reinterpret_cast<void *>(element);
347  }
348 
350  static void setHeapElementE(typename externalBHeap::Element *element, void * /*unused*/)
351  {
352  element->data->heapElement = reinterpret_cast<void *>(element);
353  }
354 
356  internalBHeap internal_;
357 
359  externalBHeap external_;
360  };
361 }
362 
363 OMPL_POP_CLANG
364 
365 #endif
bool remove(BaseCell *cell) override
Definition: GridN.h:249
std::vector< Cell * > CellArray
The datatype for arrays of cells.
Definition: Grid.h:138
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.