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