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