Loading...
Searching...
No Matches
GridN.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_N_
38#define OMPL_DATASTRUCTURES_GRID_N_
39
40#include "ompl/datastructures/Grid.h"
41
42namespace ompl
43{
45 template <typename _T>
46 class GridN : public Grid<_T>
47 {
48 public:
50 using BaseCell = typename Grid<_T>::Cell;
51
54
56 using Coord = typename Grid<_T>::Coord;
57
59 struct Cell : public BaseCell
60 {
62 unsigned int neighbors{0};
63
65 bool border{true};
66
67 Cell() = default;
68
69 ~Cell() override = default;
70
71 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
72 };
73
75 using CellArray = std::vector<Cell *>;
76
78 explicit GridN(unsigned int dimension) : Grid<_T>(dimension)
79 {
80 hasBounds_ = false;
82 setDimension(dimension);
83 }
84
85 ~GridN() override = default;
86
89 void setDimension(unsigned int dimension)
90 {
91 assert(Grid<_T>::empty() == true);
92 Grid<_T>::dimension_ = dimension;
93 Grid<_T>::maxNeighbors_ = 2 * dimension;
96 }
97
106 void setBounds(const Coord &low, const Coord &up)
107 {
108 lowBound_ = low;
109 upBound_ = up;
110 hasBounds_ = true;
111 }
112
115 void setInteriorCellNeighborLimit(unsigned int count)
116 {
118 assert(interiorCellNeighborsLimit_ > 0);
120 }
121
123 Cell *getCell(const Coord &coord) const
124 {
125 return static_cast<Cell *>(Grid<_T>::getCell(coord));
126 }
127
129 void neighbors(const Cell *cell, CellArray &list) const
130 {
131 Coord test = cell->coord;
132 neighbors(test, list);
133 }
134
136 void neighbors(const Coord &coord, CellArray &list) const
137 {
138 Coord test = coord;
139 neighbors(test, list);
140 }
141
143 void neighbors(Coord &coord, CellArray &list) const
144 {
145 BaseCellArray baselist;
146 Grid<_T>::neighbors(coord, baselist);
147 list.reserve(list.size() + baselist.size());
148 for (const auto &c : baselist)
149 list.push_back(static_cast<Cell *>(c));
150 }
151
157 BaseCell *createCell(const Coord &coord, BaseCellArray *nbh = nullptr) override
158 {
159 auto *cell = new Cell();
160 cell->coord = coord;
161
162 BaseCellArray *list = nbh ? nbh : new BaseCellArray();
163 Grid<_T>::neighbors(cell->coord, *list);
164
165 for (auto cl = list->begin(); cl != list->end(); ++cl)
166 {
167 auto *c = static_cast<Cell *>(*cl);
168 c->neighbors++;
169 if (c->border && c->neighbors >= interiorCellNeighborsLimit_)
170 c->border = false;
171 }
172
173 cell->neighbors = numberOfBoundaryDimensions(cell->coord) + list->size();
174 if (cell->border && cell->neighbors >= interiorCellNeighborsLimit_)
175 cell->border = false;
176
177 if (!nbh)
178 delete list;
179
180 return cell;
181 }
182
185 bool remove(BaseCell *cell) override
186 {
187 if (cell)
188 {
189 auto *list = new BaseCellArray();
190 Grid<_T>::neighbors(cell->coord, *list);
191 for (auto cl = list->begin(); cl != list->end(); ++cl)
192 {
193 auto *c = static_cast<Cell *>(*cl);
194 c->neighbors--;
195 if (!c->border && c->neighbors < interiorCellNeighborsLimit_)
196 c->border = true;
197 }
198 delete list;
199 auto pos = Grid<_T>::hash_.find(&cell->coord);
200 if (pos != Grid<_T>::hash_.end())
201 {
202 Grid<_T>::hash_.erase(pos);
203 return true;
204 }
205 }
206 return false;
207 }
208
210 void getCells(CellArray &cells) const
211 {
212 for (const auto &h : Grid<_T>::hash_)
213 cells.push_back(static_cast<Cell *>(h.second));
214 }
215
216 protected:
218 unsigned int numberOfBoundaryDimensions(const Coord &coord) const
219 {
220 unsigned int result = 0;
221 if (hasBounds_)
222 {
223 for (unsigned int i = 0; i < Grid<_T>::dimension_; ++i)
224 if (coord[i] == lowBound_[i] || coord[i] == upBound_[i])
225 result++;
226 }
227 return result;
228 }
229
232
235
238
243
247
248 public:
249 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
250 };
251}
252
253#endif
Representation of a grid where cells keep track of how many neighbors they have.
Definition GridN.h:47
unsigned int interiorCellNeighborsLimit_
Definition GridN.h:242
Cell * getCell(const Coord &coord) const
Get the cell at a specified coordinate.
Definition GridN.h:123
GridN(unsigned int dimension)
The constructor takes the dimension of the grid as argument.
Definition GridN.h:78
bool overrideCellNeighborsLimit_
Definition GridN.h:246
Coord upBound_
If bounds are set, this defines the upper corner cell.
Definition GridN.h:237
void setDimension(unsigned int dimension)
Definition GridN.h:89
typename Grid< _T >::Coord Coord
Datatype for cell coordinates.
Definition GridN.h:56
typename Grid< _T >::CellArray BaseCellArray
Datatype for array of cells in base class.
Definition GridN.h:53
typename Grid< _T >::Cell BaseCell
Datatype for cell in base class.
Definition GridN.h:50
void neighbors(const Cell *cell, CellArray &list) const
Get the list of neighbors for a given cell.
Definition GridN.h:129
void neighbors(Coord &coord, CellArray &list) const
Get the list of neighbors for a given coordinate.
Definition GridN.h:143
std::vector< Cell * > CellArray
The datatype for arrays of cells.
Definition GridN.h:75
BaseCell * createCell(const Coord &coord, BaseCellArray *nbh=nullptr) override
Definition GridN.h:157
void neighbors(const Coord &coord, CellArray &list) const
Get the list of neighbors for a given coordinate.
Definition GridN.h:136
Coord lowBound_
If bounds are set, this defines the lower corner cell.
Definition GridN.h:234
bool hasBounds_
Flag indicating whether bounds are in effect for this grid.
Definition GridN.h:231
unsigned int numberOfBoundaryDimensions(const Coord &coord) const
Compute how many sides of a coordinate touch the boundaries of the grid.
Definition GridN.h:218
bool remove(BaseCell *cell) override
Definition GridN.h:185
void getCells(CellArray &cells) const
Get the set of instantiated cells in the grid.
Definition GridN.h:210
void setInteriorCellNeighborLimit(unsigned int count)
Definition GridN.h:115
void setBounds(const Coord &low, const Coord &up)
Definition GridN.h:106
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
Eigen::VectorXi Coord
Definition of a coordinate within this grid.
Definition Grid.h:55
Cell * getCell(const Coord &coord) const
Get the cell at a specified coordinate.
Definition Grid.h:117
unsigned int maxNeighbors_
The maximum number of neighbors a cell can have (2 * dimension)
Definition Grid.h:385
unsigned int dimension_
The dimension of the grid.
Definition Grid.h:382
Grid(unsigned int dimension)
The constructor takes the dimension of the grid as argument.
Definition Grid.h:77
iterator end() const
Return the end() iterator for the grid.
Definition Grid.h:375
std::vector< Cell * > CellArray
The datatype for arrays of cells.
Definition Grid.h:74
CoordHash hash_
The hash holding the cells.
Definition Grid.h:388
Main namespace. Contains everything in this library.
Definition of a cell in this grid.
Definition GridN.h:60
unsigned int neighbors
The number of neighbors.
Definition GridN.h:62
bool border
A flag indicating whether this cell is on the border or not.
Definition GridN.h:65
Definition of a cell in this grid.
Definition Grid.h:59