Loading...
Searching...
No Matches
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
44OMPL_PUSH_DISABLE_CLANG_WARNING(-Woverloaded-virtual)
45
46namespace 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
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
107 {
108 auto *top = static_cast<Cell *>(internal_.top()->data);
109 return top ? top : topExternal();
110 }
111
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
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 {
281 out << countInternal() << " internal cells" << std::endl;
282 out << countExternal() << " external cells" << std::endl;
283 }
284
285 protected:
288
291
293 static void noCellUpdate(Cell * /*unused*/, void * /*unused*/)
294 {
295 }
296
299 {
300 eventCellUpdate_ = &noCellUpdate;
301 eventCellUpdateData_ = nullptr;
302 internal_.onAfterInsert(&setHeapElementI, nullptr);
303 external_.onAfterInsert(&setHeapElementE, nullptr);
304 }
305
308 {
309 internal_.clear();
310 external_.clear();
311 }
312
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
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
357
360 };
361}
362
363OMPL_POP_CLANG
364
365#endif
This class defines a grid that keeps track of its boundary: it distinguishes between interior and ext...
Definition: GridB.h:52
internalBHeap internal_
The heap of interior cells.
Definition: GridB.h:356
double fracInternal() const
Return the fraction of internal cells.
Definition: GridB.h:138
void updateAll()
Update all cells and reconstruct the heaps.
Definition: GridB.h:156
void status(std::ostream &out=std::cout) const override
Print information about the data in this grid structure.
Definition: GridB.h:278
EventCellUpdate eventCellUpdate_
Pointer to function to be called when a cell needs to be updated.
Definition: GridB.h:287
static void noCellUpdate(Cell *, void *)
Default no-op update routine for a cell.
Definition: GridB.h:293
virtual void add(Cell *cell)
Add the cell to the grid.
Definition: GridB.h:210
virtual bool remove(Cell *cell)
Remove a cell from the grid.
Definition: GridB.h:224
void(*)(Cell *, void *) EventCellUpdate
Event to be called when a cell's priority is to be updated.
Definition: GridB.h:84
static void setHeapElementI(typename internalBHeap::Element *element, void *)
Routine used internally for keeping track of binary heap elements for internal cells.
Definition: GridB.h:344
Cell * topExternal() const
Return the cell that is at the top of the heap maintaining external cells.
Definition: GridB.h:113
GridB(unsigned int dimension)
Constructor.
Definition: GridB.h:87
unsigned int countExternal() const
Return the number of external cells.
Definition: GridB.h:126
typename GridN< _T >::CellArray CellArray
The datatype for arrays of cells.
Definition: GridB.h:58
void setupHeaps()
Set the update procedure for the heaps of internal and external cells.
Definition: GridB.h:298
typename GridN< _T >::Coord Coord
Datatype for cell coordinates.
Definition: GridB.h:61
void onCellUpdate(EventCellUpdate event, void *arg)
Definition: GridB.h:99
typename GridN< _T >::Cell Cell
Definition of a cell in this grid.
Definition: GridB.h:55
void clear() override
Clear all cells in the grid.
Definition: GridB.h:272
unsigned int countInternal() const
Return the number of internal cells.
Definition: GridB.h:120
void * eventCellUpdateData_
Data to be passed to function pointer above.
Definition: GridB.h:290
void clearHeaps()
Clear the data from both heaps.
Definition: GridB.h:307
double fracExternal() const
Return the fraction of external cells.
Definition: GridB.h:132
Cell * topInternal() const
Return the cell that is at the top of the heap maintaining internal cells.
Definition: GridB.h:106
void update(Cell *cell)
Update the position in the heaps for a particular cell.
Definition: GridB.h:144
static void setHeapElementE(typename externalBHeap::Element *element, void *)
Routine used internally for keeping track of binary heap elements for external cells.
Definition: GridB.h:350
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:167
externalBHeap external_
The heap of external cells.
Definition: GridB.h:359
Representation of a grid where cells keep track of how many neighbors they have.
Definition: GridN.h:47
typename Grid< _T >::Coord Coord
Datatype for cell coordinates.
Definition: GridN.h:56
std::vector< Cell * > CellArray
The datatype for arrays of cells.
Definition: GridN.h:75
bool remove(BaseCell *cell) override
Definition: GridN.h:185
unsigned int size() const
Check the size of the grid.
Definition: Grid.h:294
Main namespace. Contains everything in this library.
Define order for external cells.
Definition: GridB.h:327
Define order for internal cells.
Definition: GridB.h:315
Definition of a cell in this grid.
Definition: GridN.h:60