All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
Grid.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2010, Rice University
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 Rice University 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_
38 #define OMPL_DATASTRUCTURES_GRID_
39 
40 #include <vector>
41 #include <iostream>
42 #include <cstdlib>
43 #include <boost/unordered_map.hpp>
44 #include <algorithm>
45 
46 namespace ompl
47 {
48 
50  template <typename _T>
51  class Grid
52  {
53  public:
54 
56  typedef std::vector<int> Coord;
57 
59  struct Cell
60  {
62  _T data;
63 
66 
67  Cell(void)
68  {
69  }
70 
71  virtual ~Cell(void)
72  {
73  }
74  };
75 
77  typedef std::vector<Cell*> CellArray;
78 
79 
81  explicit
82  Grid(unsigned int dimension)
83  {
84  setDimension(dimension);
85  }
86 
88  virtual ~Grid(void)
89  {
90  freeMemory();
91  }
92 
94  virtual void clear(void)
95  {
96  freeMemory();
97  }
98 
100  unsigned int getDimension(void) const
101  {
102  return dimension_;
103  }
104 
107  void setDimension(unsigned int dimension)
108  {
109  if (!empty())
110  throw;
111  dimension_ = dimension;
113  }
114 
116  bool has(const Coord &coord) const
117  {
118  return getCell(coord) != NULL;
119  }
120 
122  Cell* getCell(const Coord &coord) const
123  {
124  iterator pos = hash_.find(const_cast<Coord*>(&coord));
125  Cell *c = (pos != hash_.end()) ? pos->second : NULL;
126  return c;
127  }
128 
130  void neighbors(const Cell* cell, CellArray& list) const
131  {
132  Coord test = cell->coord;
133  neighbors(test, list);
134  }
135 
137  void neighbors(const Coord& coord, CellArray& list) const
138  {
139  Coord test = coord;
140  neighbors(test, list);
141  }
142 
144  void neighbors(Coord& coord, CellArray& list) const
145  {
146  list.reserve(list.size() + maxNeighbors_);
147 
148  for (int i = dimension_ - 1 ; i >= 0 ; --i)
149  {
150  coord[i]--;
151 
152  iterator pos = hash_.find(&coord);
153  Cell *cell = (pos != hash_.end()) ? pos->second : NULL;
154 
155  if (cell)
156  list.push_back(cell);
157  coord[i] += 2;
158 
159  pos = hash_.find(&coord);
160  cell = (pos != hash_.end()) ? pos->second : NULL;
161 
162  if (cell)
163  list.push_back(cell);
164  coord[i]--;
165  }
166  }
167 
169  std::vector< std::vector<Cell*> > components(void) const
170  {
171  typedef boost::unordered_map<Coord*, int, HashFunCoordPtr, EqualCoordPtr> ComponentHash;
172  typedef typename ComponentHash::iterator CHit;
173 
174  int components = 0;
175  ComponentHash ch;
176  std::vector< std::vector<Cell*> > res;
177 
178  for (iterator i = hash_.begin() ; i != hash_.end() ; ++i)
179  {
180  Cell *c0 = i->second;
181  CHit pos = ch.find(&c0->coord);
182  int comp = (pos != ch.end()) ? pos->second : -1;
183 
184  if (comp < 0)
185  {
186  res.resize(res.size() + 1);
187  std::vector<Cell*> &q = res.back();
188  q.push_back(c0);
189  std::size_t index = 0;
190  while (index < q.size())
191  {
192  Cell *c = q[index++];
193  pos = ch.find(&c->coord);
194  comp = (pos != ch.end()) ? pos->second : -1;
195 
196  if (comp < 0)
197  {
198  ch.insert(std::make_pair(&c->coord, components));
199  std::vector< Cell* > nbh;
200  neighbors(c, nbh);
201  for (unsigned int j = 0 ; j < nbh.size() ; ++j)
202  {
203  pos = ch.find(&nbh[j]->coord);
204  comp = (pos != ch.end()) ? pos->second : -1;
205  if (comp < 0)
206  q.push_back(nbh[j]);
207  }
208  }
209  else
210  {
211  --index;
212  q.erase(q.begin() + index);
213  }
214  }
215  ++components;
216  }
217  }
218  std::sort(res.begin(), res.end(), SortComponents());
219  return res;
220  }
221 
226  virtual Cell* createCell(const Coord& coord, CellArray *nbh = NULL)
227  {
228  Cell *cell = new Cell();
229  cell->coord = coord;
230  if (nbh)
231  neighbors(cell->coord, *nbh);
232  return cell;
233  }
234 
237  virtual bool remove(Cell *cell)
238  {
239  if (cell)
240  {
241  typename CoordHash::iterator pos = hash_.find(&cell->coord);
242  if (pos != hash_.end())
243  {
244  hash_.erase(pos);
245  return true;
246  }
247  }
248  return false;
249  }
250 
252  virtual void add(Cell *cell)
253  {
254  hash_.insert(std::make_pair(&cell->coord, cell));
255  }
256 
258  virtual void destroyCell(Cell *cell) const
259  {
260  delete cell;
261  }
262 
264  void getContent(std::vector<_T> &content) const
265  {
266  for (iterator i = hash_.begin() ; i != hash_.end() ; ++i)
267  content.push_back(i->second->data);
268  }
269 
271  void getCoordinates(std::vector<Coord*> &coords) const
272  {
273  for (iterator i = hash_.begin() ; i != hash_.end() ; ++i)
274  coords.push_back(i->first);
275  }
276 
278  void getCells(CellArray &cells) const
279  {
280  for (iterator i = hash_.begin() ; i != hash_.end() ; ++i)
281  cells.push_back(i->second);
282  }
283 
285  void printCoord(Coord& coord, std::ostream &out = std::cout) const
286  {
287  out << "[ ";
288  for (unsigned int i = 0 ; i < dimension_ ; ++i)
289  out << coord[i] << " ";
290  out << "]" << std::endl;
291  }
292 
294  bool empty(void) const
295  {
296  return hash_.empty();
297  }
298 
300  unsigned int size(void) const
301  {
302  return hash_.size();
303  }
304 
306  virtual void status(std::ostream &out = std::cout) const
307  {
308  out << size() << " total cells " << std::endl;
309  const std::vector< std::vector<Cell*> > &comp = components();
310  out << comp.size() << " connected components: ";
311  for (std::size_t i = 0 ; i < comp.size() ; ++i)
312  out << comp[i].size() << " ";
313  out << std::endl;
314  }
315 
316  protected:
317 
319  void freeMemory(void)
320  {
321  CellArray content;
322  getCells(content);
323  hash_.clear();
324 
325  for (unsigned int i = 0 ; i < content.size() ; ++i)
326  delete content[i];
327  }
328 
331  {
333  std::size_t operator()(const Coord* const s) const
334  {
335  unsigned long h = 0;
336  for (int i = s->size() - 1; i >= 0; --i)
337  {
338  int high = h & 0xf8000000;
339  h = h << 5;
340  h = h ^ (high >> 27);
341  h = h ^ s->at(i);
342  }
343  return (std::size_t) h;
344  }
345  };
346 
347 
350  {
352  bool operator()(const Coord* const c1, const Coord* const c2) const
353  {
354  return *c1 == *c2;
355  }
356  };
357 
359  typedef boost::unordered_map<Coord*, Cell*, HashFunCoordPtr, EqualCoordPtr> CoordHash;
360 
363  {
365  bool operator()(const std::vector<Cell*> &a, const std::vector<Cell*> &b) const
366  {
367  return a.size() > b.size();
368  }
369  };
370 
371  public:
372 
374  typedef typename CoordHash::const_iterator iterator;
375 
377  iterator begin(void) const
378  {
379  return hash_.begin();
380  }
381 
383  iterator end(void) const
384  {
385  return hash_.end();
386  }
387 
388  protected:
389 
391  unsigned int dimension_;
392 
394  unsigned int maxNeighbors_;
395 
398  };
399 }
400 
401 #endif
void printCoord(Coord &coord, std::ostream &out=std::cout) const
Print the value of a coordinate to a stream.
Definition: Grid.h:285
Helper to sort components by size.
Definition: Grid.h:362
virtual void destroyCell(Cell *cell) const
Clear the memory occupied by a cell; do not call this function unless remove() was called first...
Definition: Grid.h:258
Representation of a simple grid.
Definition: Grid.h:51
unsigned int dimension_
The dimension of the grid.
Definition: Grid.h:391
std::vector< int > Coord
Definition of a coordinate within this grid.
Definition: Grid.h:56
virtual void status(std::ostream &out=std::cout) const
Print information about the data in this grid structure.
Definition: Grid.h:306
virtual Cell * createCell(const Coord &coord, CellArray *nbh=NULL)
Definition: Grid.h:226
bool operator()(const std::vector< Cell * > &a, const std::vector< Cell * > &b) const
Helper to sort components by size.
Definition: Grid.h:365
_T data
The data we store in the cell.
Definition: Grid.h:62
Grid(unsigned int dimension)
The constructor takes the dimension of the grid as argument.
Definition: Grid.h:82
Coord coord
The coordinate of the cell.
Definition: Grid.h:65
void neighbors(const Coord &coord, CellArray &list) const
Get the list of neighbors for a given coordinate.
Definition: Grid.h:137
std::vector< Cell * > CellArray
The datatype for arrays of cells.
Definition: Grid.h:77
unsigned int maxNeighbors_
The maximum number of neighbors a cell can have (2 * dimension)
Definition: Grid.h:394
void neighbors(Coord &coord, CellArray &list) const
Get the list of neighbors for a given coordinate.
Definition: Grid.h:144
Cell * getCell(const Coord &coord) const
Get the cell at a specified coordinate.
Definition: Grid.h:122
iterator begin(void) const
Return the begin() iterator for the grid.
Definition: Grid.h:377
void setDimension(unsigned int dimension)
Definition: Grid.h:107
void getCells(CellArray &cells) const
Get the set of instantiated cells in the grid.
Definition: Grid.h:278
std::size_t operator()(const Coord *const s) const
Hash function for coordinates.
Definition: Grid.h:333
virtual void add(Cell *cell)
Add an instantiated cell to the grid.
Definition: Grid.h:252
void freeMemory(void)
Free the allocated memory.
Definition: Grid.h:319
bool empty(void) const
Check if the grid is empty.
Definition: Grid.h:294
bool operator()(const Coord *const c1, const Coord *const c2) const
Equality operator for coordinate pointers.
Definition: Grid.h:352
CoordHash hash_
The hash holding the cells.
Definition: Grid.h:397
unsigned int getDimension(void) const
Return the dimension of the grid.
Definition: Grid.h:100
Definition of a cell in this grid.
Definition: Grid.h:59
std::vector< std::vector< Cell * > > components(void) const
Get the connected components formed by the cells in this grid (based on neighboring relation) ...
Definition: Grid.h:169
void neighbors(const Cell *cell, CellArray &list) const
Get the list of neighbors for a given cell.
Definition: Grid.h:130
virtual ~Grid(void)
Destructor.
Definition: Grid.h:88
Hash function for coordinates; see http://www.cs.hmc.edu/~geoff/classes/hmc.cs070.200101/homework10/hashfuncs.html.
Definition: Grid.h:330
bool has(const Coord &coord) const
Check if a cell exists at the specified coordinate.
Definition: Grid.h:116
virtual void clear(void)
Clear all cells in the grid.
Definition: Grid.h:94
Equality operator for coordinate pointers.
Definition: Grid.h:349
void getContent(std::vector< _T > &content) const
Get the data stored in the cells we are aware of.
Definition: Grid.h:264
iterator end(void) const
Return the end() iterator for the grid.
Definition: Grid.h:383
boost::unordered_map< Coord *, Cell *, HashFunCoordPtr, EqualCoordPtr > CoordHash
Define the datatype for the used hash structure.
Definition: Grid.h:359
void getCoordinates(std::vector< Coord * > &coords) const
Get the set of coordinates where there are cells.
Definition: Grid.h:271
CoordHash::const_iterator iterator
We only allow const iterators.
Definition: Grid.h:374
unsigned int size(void) const
Check the size of the grid.
Definition: Grid.h:300