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 <unordered_map>
44 #include <algorithm>
45 
46 namespace ompl
47 {
49  template <typename _T>
50  class Grid
51  {
52  public:
54  using Coord = std::vector<int>;
55 
57  struct Cell
58  {
60  _T data;
61 
64 
65  Cell() = default;
66 
67  virtual ~Cell() = default;
68  };
69 
71  using CellArray = std::vector<Cell *>;
72 
74  explicit Grid(unsigned int dimension)
75  {
76  setDimension(dimension);
77  }
78 
80  virtual ~Grid()
81  {
82  freeMemory();
83  }
84 
86  virtual void clear()
87  {
88  freeMemory();
89  }
90 
92  unsigned int getDimension() const
93  {
94  return dimension_;
95  }
96 
99  void setDimension(unsigned int dimension)
100  {
101  if (!empty())
102  throw;
103  dimension_ = dimension;
105  }
106 
108  bool has(const Coord &coord) const
109  {
110  return getCell(coord) != nullptr;
111  }
112 
114  Cell *getCell(const Coord &coord) const
115  {
116  auto pos = hash_.find(const_cast<Coord *>(&coord));
117  Cell *c = (pos != hash_.end()) ? pos->second : nullptr;
118  return c;
119  }
120 
122  void neighbors(const Cell *cell, CellArray &list) const
123  {
124  Coord test = cell->coord;
125  neighbors(test, list);
126  }
127 
129  void neighbors(const Coord &coord, CellArray &list) const
130  {
131  Coord test = coord;
132  neighbors(test, list);
133  }
134 
136  void neighbors(Coord &coord, CellArray &list) const
137  {
138  list.reserve(list.size() + maxNeighbors_);
139 
140  for (int i = dimension_ - 1; i >= 0; --i)
141  {
142  coord[i]--;
143 
144  auto pos = hash_.find(&coord);
145  Cell *cell = (pos != hash_.end()) ? pos->second : nullptr;
146 
147  if (cell)
148  list.push_back(cell);
149  coord[i] += 2;
150 
151  pos = hash_.find(&coord);
152  cell = (pos != hash_.end()) ? pos->second : nullptr;
153 
154  if (cell)
155  list.push_back(cell);
156  coord[i]--;
157  }
158  }
159 
161  std::vector<std::vector<Cell *>> components() const
162  {
163  using ComponentHash = std::unordered_map<Coord *, int, HashFunCoordPtr, EqualCoordPtr>;
164 
165  int components = 0;
166  ComponentHash ch;
167  std::vector<std::vector<Cell *>> res;
168 
169  for (auto i = hash_.begin(); i != hash_.end(); ++i)
170  {
171  Cell *c0 = i->second;
172  auto pos = ch.find(&c0->coord);
173  int comp = (pos != ch.end()) ? pos->second : -1;
174 
175  if (comp < 0)
176  {
177  res.resize(res.size() + 1);
178  std::vector<Cell *> &q = res.back();
179  q.push_back(c0);
180  std::size_t index = 0;
181  while (index < q.size())
182  {
183  Cell *c = q[index++];
184  pos = ch.find(&c->coord);
185  comp = (pos != ch.end()) ? pos->second : -1;
186 
187  if (comp < 0)
188  {
189  ch.insert(std::make_pair(&c->coord, components));
190  std::vector<Cell *> nbh;
191  neighbors(c, nbh);
192  for (unsigned int j = 0; j < nbh.size(); ++j)
193  {
194  pos = ch.find(&nbh[j]->coord);
195  comp = (pos != ch.end()) ? pos->second : -1;
196  if (comp < 0)
197  q.push_back(nbh[j]);
198  }
199  }
200  else
201  {
202  --index;
203  q.erase(q.begin() + index);
204  }
205  }
206  ++components;
207  }
208  }
209  std::sort(res.begin(), res.end(), SortComponents());
210  return res;
211  }
212 
217  virtual Cell *createCell(const Coord &coord, CellArray *nbh = nullptr)
218  {
219  auto *cell = new Cell();
220  cell->coord = coord;
221  if (nbh)
222  neighbors(cell->coord, *nbh);
223  return cell;
224  }
225 
228  virtual bool remove(Cell *cell)
229  {
230  if (cell)
231  {
232  auto pos = hash_.find(&cell->coord);
233  if (pos != hash_.end())
234  {
235  hash_.erase(pos);
236  return true;
237  }
238  }
239  return false;
240  }
241 
243  virtual void add(Cell *cell)
244  {
245  hash_.insert(std::make_pair(&cell->coord, cell));
246  }
247 
249  virtual void destroyCell(Cell *cell) const
250  {
251  delete cell;
252  }
253 
255  void getContent(std::vector<_T> &content) const
256  {
257  for (auto i = hash_.begin(); i != hash_.end(); ++i)
258  content.push_back(i->second->data);
259  }
260 
262  void getCoordinates(std::vector<Coord *> &coords) const
263  {
264  for (iterator i = hash_.begin(); i != hash_.end(); ++i)
265  coords.push_back(i->first);
266  }
267 
269  void getCells(CellArray &cells) const
270  {
271  for (auto i = hash_.begin(); i != hash_.end(); ++i)
272  cells.push_back(i->second);
273  }
274 
276  void printCoord(Coord &coord, std::ostream &out = std::cout) const
277  {
278  out << "[ ";
279  for (unsigned int i = 0; i < dimension_; ++i)
280  out << coord[i] << " ";
281  out << "]" << std::endl;
282  }
283 
285  bool empty() const
286  {
287  return hash_.empty();
288  }
289 
291  unsigned int size() const
292  {
293  return hash_.size();
294  }
295 
297  virtual void status(std::ostream &out = std::cout) const
298  {
299  out << size() << " total cells " << std::endl;
300  const std::vector<std::vector<Cell *>> &comp = components();
301  out << comp.size() << " connected components: ";
302  for (std::size_t i = 0; i < comp.size(); ++i)
303  out << comp[i].size() << " ";
304  out << std::endl;
305  }
306 
307  protected:
309  void freeMemory()
310  {
311  CellArray content;
312  getCells(content);
313  hash_.clear();
314 
315  for (unsigned int i = 0; i < content.size(); ++i)
316  delete content[i];
317  }
318 
322  {
324  std::size_t operator()(const Coord *const s) const
325  {
326  unsigned long h = 0;
327  for (int i = s->size() - 1; i >= 0; --i)
328  {
329  int high = h & 0xf8000000;
330  h = h << 5;
331  h = h ^ (high >> 27);
332  h = h ^ s->at(i);
333  }
334  return (std::size_t)h;
335  }
336  };
337 
340  {
342  bool operator()(const Coord *const c1, const Coord *const c2) const
343  {
344  return *c1 == *c2;
345  }
346  };
347 
349  using CoordHash = std::unordered_map<Coord *, Cell *, HashFunCoordPtr, EqualCoordPtr>;
350 
353  {
355  bool operator()(const std::vector<Cell *> &a, const std::vector<Cell *> &b) const
356  {
357  return a.size() > b.size();
358  }
359  };
360 
361  public:
363  using iterator = typename CoordHash::const_iterator;
364 
366  iterator begin() const
367  {
368  return hash_.begin();
369  }
370 
372  iterator end() const
373  {
374  return hash_.end();
375  }
376 
377  protected:
379  unsigned int dimension_;
380 
382  unsigned int maxNeighbors_;
383 
386  };
387 }
388 
389 #endif
Helper to sort components by size.
Definition: Grid.h:352
unsigned int getDimension() const
Return the dimension of the grid.
Definition: Grid.h:92
void neighbors(const Coord &coord, CellArray &list) const
Get the list of neighbors for a given coordinate.
Definition: Grid.h:129
bool empty() const
Check if the grid is empty.
Definition: Grid.h:285
void freeMemory()
Free the allocated memory.
Definition: Grid.h:309
Representation of a simple grid.
Definition: Grid.h:50
virtual void clear()
Clear all cells in the grid.
Definition: Grid.h:86
unsigned int dimension_
The dimension of the grid.
Definition: Grid.h:379
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:249
_T data
The data we store in the cell.
Definition: Grid.h:60
Grid(unsigned int dimension)
The constructor takes the dimension of the grid as argument.
Definition: Grid.h:74
void getContent(std::vector< _T > &content) const
Get the data stored in the cells we are aware of.
Definition: Grid.h:255
virtual Cell * createCell(const Coord &coord, CellArray *nbh=nullptr)
Definition: Grid.h:217
Coord coord
The coordinate of the cell.
Definition: Grid.h:63
std::vector< int > Coord
Definition of a coordinate within this grid.
Definition: Grid.h:54
bool has(const Coord &coord) const
Check if a cell exists at the specified coordinate.
Definition: Grid.h:108
unsigned int maxNeighbors_
The maximum number of neighbors a cell can have (2 * dimension)
Definition: Grid.h:382
std::size_t operator()(const Coord *const s) const
Hash function for coordinates.
Definition: Grid.h:324
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
virtual void status(std::ostream &out=std::cout) const
Print information about the data in this grid structure.
Definition: Grid.h:297
void setDimension(unsigned int dimension)
Definition: Grid.h:99
virtual void add(Cell *cell)
Add an instantiated cell to the grid.
Definition: Grid.h:243
bool operator()(const Coord *const c1, const Coord *const c2) const
Equality operator for coordinate pointers.
Definition: Grid.h:342
virtual ~Grid()
Destructor.
Definition: Grid.h:80
std::vector< Cell *> CellArray
The datatype for arrays of cells.
Definition: Grid.h:71
void getCoordinates(std::vector< Coord *> &coords) const
Get the set of coordinates where there are cells.
Definition: Grid.h:262
void getCells(CellArray &cells) const
Get the set of instantiated cells in the grid.
Definition: Grid.h:269
CoordHash hash_
The hash holding the cells.
Definition: Grid.h:385
bool operator()(const std::vector< Cell *> &a, const std::vector< Cell *> &b) const
Helper to sort components by size.
Definition: Grid.h:355
Definition of a cell in this grid.
Definition: Grid.h:57
unsigned int size() const
Check the size of the grid.
Definition: Grid.h:291
std::vector< std::vector< Cell * > > components() const
Get the connected components formed by the cells in this grid (based on neighboring relation) ...
Definition: Grid.h:161
iterator end() const
Return the end() iterator for the grid.
Definition: Grid.h:372
typename CoordHash::const_iterator iterator
We only allow const iterators.
Definition: Grid.h:363
Cell * getCell(const Coord &coord) const
Get the cell at a specified coordinate.
Definition: Grid.h:114
void neighbors(const Cell *cell, CellArray &list) const
Get the list of neighbors for a given cell.
Definition: Grid.h:122
Equality operator for coordinate pointers.
Definition: Grid.h:339
void neighbors(Coord &coord, CellArray &list) const
Get the list of neighbors for a given coordinate.
Definition: Grid.h:136
std::unordered_map< Coord *, Cell *, HashFunCoordPtr, EqualCoordPtr > CoordHash
Define the datatype for the used hash structure.
Definition: Grid.h:349
void printCoord(Coord &coord, std::ostream &out=std::cout) const
Print the value of a coordinate to a stream.
Definition: Grid.h:276
iterator begin() const
Return the begin() iterator for the grid.
Definition: Grid.h:366