All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
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 {
45 
48  template < typename _T,
49  class LessThanExternal = std::less<_T>,
50  class LessThanInternal = LessThanExternal >
51  class GridB : public GridN<_T>
52  {
53  public:
54 
56  typedef typename GridN<_T>::Cell Cell;
57 
59  typedef typename GridN<_T>::CellArray CellArray;
60 
62  typedef typename GridN<_T>::Coord Coord;
63 
64  protected:
65 
67  // the type of cell here needs an extra pointer to allow the updatable heap to work fast
68  // however, this stays hidden from the user
69  struct CellX : public Cell
70  {
71  CellX(void) : Cell()
72  {
73  }
74 
75  virtual ~CellX(void)
76  {
77  }
78 
79  void *heapElement;
80  };
81 
83 
84  public:
85 
87  typedef void (*EventCellUpdate)(Cell*, void*);
88 
90  explicit
91  GridB(unsigned int dimension) : GridN<_T>(dimension)
92  {
93  setupHeaps();
94  }
95 
96  virtual ~GridB(void)
97  {
98  clearHeaps();
99  }
100 
103  void onCellUpdate(EventCellUpdate event, void *arg)
104  {
105  eventCellUpdate_ = event;
106  eventCellUpdateData_ = arg;
107  }
108 
110  Cell* topInternal(void) const
111  {
112  Cell* top = static_cast<Cell*>(internal_.top()->data);
113  return top ? top : topExternal();
114  }
115 
117  Cell* topExternal(void) const
118  {
119  Cell* top = static_cast<Cell*>(external_.top()->data);
120  return top ? top : topInternal();
121  }
122 
124  unsigned int countInternal(void) const
125  {
126  return internal_.size();
127  }
128 
130  unsigned int countExternal(void) const
131  {
132  return external_.size();
133  }
134 
136  double fracExternal(void) const
137  {
138  return external_.empty() ? 0.0 : (double)(external_.size()) / (double)(external_.size() + internal_.size());
139  }
140 
142  double fracInternal(void) const
143  {
144  return 1.0 - fracExternal();
145  }
146 
148  void update(Cell* cell)
149  {
151  if (cell->border)
152  external_.update(reinterpret_cast<typename externalBHeap::Element*>
153  (static_cast<CellX*>(cell)->heapElement));
154  else
155  internal_.update(reinterpret_cast<typename internalBHeap::Element*>
156  (static_cast<CellX*>(cell)->heapElement));
157  }
158 
160  void updateAll(void)
161  {
162  std::vector< Cell* > cells;
163  this->getCells(cells);
164  for (int i = cells.size() - 1 ; i >= 0 ; --i)
166  external_.rebuild();
167  internal_.rebuild();
168  }
169 
171  virtual Cell* createCell(const Coord& coord, CellArray *nbh = NULL)
172  {
173  CellX* cell = new CellX();
174  cell->coord = coord;
175 
176  CellArray *list = nbh ? nbh : new CellArray();
177  this->neighbors(cell->coord, *list);
178 
179  for (typename CellArray::iterator cl = list->begin() ; cl != list->end() ; ++cl)
180  {
181  CellX* c = static_cast<CellX*>(*cl);
182  bool wasBorder = c->border;
183  c->neighbors++;
184  if (c->border && c->neighbors >= GridN<_T>::interiorCellNeighborsLimit_)
185  c->border = false;
186 
188 
189  if (c->border)
190  external_.update(reinterpret_cast<typename externalBHeap::Element*>(c->heapElement));
191  else
192  {
193  if (wasBorder)
194  {
195  external_.remove(reinterpret_cast<typename externalBHeap::Element*>(c->heapElement));
196  internal_.insert(c);
197  }
198  else
199  internal_.update(reinterpret_cast<typename internalBHeap::Element*>(c->heapElement));
200  }
201  }
202 
203  cell->neighbors = GridN<_T>::numberOfBoundaryDimensions(cell->coord) + list->size();
204  if (cell->border && cell->neighbors >= GridN<_T>::interiorCellNeighborsLimit_)
205  cell->border = false;
206 
207  if (!nbh)
208  delete list;
209 
210  return static_cast<Cell*>(cell);
211  }
212 
214  virtual void add(Cell* cell)
215  {
216  CellX* ccell = static_cast<CellX*>(cell);
218 
219  GridN<_T>::add(cell);
220 
221  if (cell->border)
222  external_.insert(ccell);
223  else
224  internal_.insert(ccell);
225  }
226 
228  virtual bool remove(Cell* cell)
229  {
230  if (cell)
231  {
232  CellArray *list = new CellArray();
233  this->neighbors(cell->coord, *list);
234 
235  for (typename CellArray::iterator cl = list->begin() ; cl != list->end() ; ++cl)
236  {
237  CellX* c = static_cast<CellX*>(*cl);
238  bool wasBorder = c->border;
239  c->neighbors--;
240  if (!c->border && c->neighbors < GridN<_T>::interiorCellNeighborsLimit_)
241  c->border = true;
242 
244 
245  if (c->border)
246  {
247  if (wasBorder)
248  external_.update(reinterpret_cast<typename externalBHeap::Element*>(c->heapElement));
249  else
250  {
251  internal_.remove(reinterpret_cast<typename internalBHeap::Element*>(c->heapElement));
252  external_.insert(c);
253  }
254  }
255  else
256  internal_.update(reinterpret_cast<typename internalBHeap::Element*>(c->heapElement));
257  }
258 
259  delete list;
260 
261  typename GridN<_T>::CoordHash::iterator pos = GridN<_T>::hash_.find(&cell->coord);
262  if (pos != GridN<_T>::hash_.end())
263  {
264  GridN<_T>::hash_.erase(pos);
265  CellX* cx = static_cast<CellX*>(cell);
266  if (cx->border)
267  external_.remove(reinterpret_cast<typename externalBHeap::Element*>(cx->heapElement));
268  else
269  internal_.remove(reinterpret_cast<typename internalBHeap::Element*>(cx->heapElement));
270  return true;
271  }
272  }
273  return false;
274  }
275 
276  virtual void clear(void)
277  {
279  clearHeaps();
280  }
281 
282  virtual void status(std::ostream &out = std::cout) const
283  {
284  GridN<_T>::status(out);
285  out << countInternal() << " internal cells" << std::endl;
286  out << countExternal() << " external cells" << std::endl;
287  }
288 
289  protected:
290 
293 
296 
298  static void noCellUpdate(Cell*, void*)
299  {
300  }
301 
303  void setupHeaps(void)
304  {
306  eventCellUpdateData_ = NULL;
309  }
310 
312  void clearHeaps(void)
313  {
314  internal_.clear();
315  external_.clear();
316  }
317 
320  {
321  bool operator()(const CellX* const a, const CellX* const b) const
322  {
323  return lt_(a->data, b->data);
324  }
325 
326  private:
327  LessThanInternal lt_;
328  };
329 
332  {
333  bool operator()(const CellX* const a, const CellX* const b) const
334  {
335  return lt_(a->data, b->data);
336  }
337  private:
338  LessThanExternal lt_;
339  };
340 
343 
346 
348  static void setHeapElementI(typename internalBHeap::Element* element, void *)
349  {
350  element->data->heapElement = reinterpret_cast<void*>(element);
351  }
352 
354  static void setHeapElementE(typename externalBHeap::Element* element, void *)
355  {
356  element->data->heapElement = reinterpret_cast<void*>(element);
357  }
358 
361 
364  };
365 
366 }
367 
368 #endif