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 
42 namespace 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
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
BaseCell * createCell(const Coord &coord, BaseCellArray *nbh=nullptr) override
Definition: GridN.h:157
Cell * getCell(const Coord &coord) const
Get the cell at a specified coordinate.
Definition: GridN.h:123
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
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
Representation of a simple grid.
Definition: Grid.h:52
void neighbors(const Cell *cell, CellArray &list) const
Get the list of neighbors for a given cell.
Definition: Grid.h:125
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
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
Main namespace. Contains everything in this library.
Definition: AppBase.h:22
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