Loading...
Searching...
No Matches
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
47namespace 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 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:
313 {
314 CellArray content;
315 getCells(content);
316 hash_.clear();
317
318 for (auto &c : content)
319 delete c;
320 }
321
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
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
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
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
void setDimension(unsigned int dimension)
Definition Grid.h:102
void neighbors(const Coord &coord, CellArray &list) const
Get the list of neighbors for a given coordinate.
Definition Grid.h:132
void neighbors(const Cell *cell, CellArray &list) const
Get the list of neighbors for a given cell.
Definition Grid.h:125
bool empty() const
Check if the grid is empty.
Definition Grid.h:288
void printCoord(Coord &coord, std::ostream &out=std::cout) const
Print the value of a coordinate to a stream.
Definition Grid.h:279
bool has(const Coord &coord) const
Check if a cell exists at the specified coordinate.
Definition Grid.h:111
virtual void status(std::ostream &out=std::cout) const
Print information about the data in this grid structure.
Definition Grid.h:300
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:252
Eigen::VectorXi Coord
Definition Grid.h:55
unsigned int getDimension() const
Return the dimension of the grid.
Definition Grid.h:95
std::unordered_map< Coord *, Cell *, HashFunCoordPtr, EqualCoordPtr > CoordHash
Definition Grid.h:352
void getCoordinates(std::vector< Coord * > &coords) const
Get the set of coordinates where there are cells.
Definition Grid.h:265
Cell * getCell(const Coord &coord) const
Get the cell at a specified coordinate.
Definition Grid.h:117
virtual bool remove(Cell *cell)
Definition Grid.h:231
unsigned int size() const
Check the size of the grid.
Definition Grid.h:294
unsigned int maxNeighbors_
Definition Grid.h:385
unsigned int dimension_
Definition Grid.h:382
void getCells(CellArray &cells) const
Get the set of instantiated cells in the grid.
Definition Grid.h:272
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:164
Grid(unsigned int dimension)
The constructor takes the dimension of the grid as argument.
Definition Grid.h:77
virtual void clear()
Clear all cells in the grid.
Definition Grid.h:89
void freeMemory()
Free the allocated memory.
Definition Grid.h:312
iterator end() const
Return the end() iterator for the grid.
Definition Grid.h:375
void neighbors(Coord &coord, CellArray &list) const
Get the list of neighbors for a given coordinate.
Definition Grid.h:139
typename CoordHash::const_iterator iterator
Definition Grid.h:366
virtual void add(Cell *cell)
Add an instantiated cell to the grid.
Definition Grid.h:246
virtual ~Grid()
Destructor.
Definition Grid.h:83
iterator begin() const
Return the begin() iterator for the grid.
Definition Grid.h:369
std::vector< Cell * > CellArray
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:258
virtual Cell * createCell(const Coord &coord, CellArray *nbh=nullptr)
Definition Grid.h:220
CoordHash hash_
Definition Grid.h:388
Main namespace. Contains everything in this library.
Coord coord
The coordinate of the cell.
Definition Grid.h:64
_T data
The data we store in the cell.
Definition Grid.h:61
Equality operator for coordinate pointers.
Definition Grid.h:343
bool operator()(const Coord *const c1, const Coord *const c2) const
Equality operator for coordinate pointers.
Definition Grid.h:345
std::size_t operator()(const Coord *const s) const
Hash function for coordinates.
Definition Grid.h:327
Helper to sort components by size.
Definition Grid.h:356
bool operator()(const std::vector< Cell * > &a, const std::vector< Cell * > &b) const
Helper to sort components by size.
Definition Grid.h:358