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;
63 
65  bool border;
66 
67  Cell() : BaseCell(), neighbors(0), border(true)
68  {
69  }
70 
71  ~Cell() override = default;
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 (unsigned int i = 0; i < baselist.size(); ++i)
149  list.push_back(static_cast<Cell *>(baselist[i]));
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  Cell *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  Cell *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 (auto i = Grid<_T>::hash_.begin(); i != Grid<_T>::hash_.end(); ++i)
213  cells.push_back(static_cast<Cell *>(i->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 }
249 
250 #endif
Representation of a simple grid.
Definition: Grid.h:50
unsigned int neighbors
The number of neighbors.
Definition: GridN.h:62
void setBounds(const Coord &low, const Coord &up)
Definition: GridN.h:106
void setInteriorCellNeighborLimit(unsigned int count)
Definition: GridN.h:115
void neighbors(const Cell *cell, CellArray &list) const
Get the list of neighbors for a given cell.
Definition: GridN.h:129
bool overrideCellNeighborsLimit_
Definition: GridN.h:246
Cell * getCell(const Coord &coord) const
Get the cell at a specified coordinate.
Definition: GridN.h:123
typename Grid< CellData * >::Cell BaseCell
Datatype for cell in base class.
Definition: GridN.h:50
std::vector< int > Coord
Definition of a coordinate within this grid.
Definition: Grid.h:54
void setDimension(unsigned int dimension)
Definition: GridN.h:89
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
GridN(unsigned int dimension)
The constructor takes the dimension of the grid as argument.
Definition: GridN.h:78
void neighbors(Coord &coord, CellArray &list) const
Get the list of neighbors for a given coordinate.
Definition: GridN.h:143
bool border
A flag indicating whether this cell is on the border or not.
Definition: GridN.h:65
Representation of a grid where cells keep track of how many neighbors they have.
Definition: GridN.h:46
BaseCell * createCell(const Coord &coord, BaseCellArray *nbh=nullptr) override
Definition: GridN.h:157
std::vector< Cell * > CellArray
The datatype for arrays of cells.
Definition: Grid.h:71
Coord lowBound_
If bounds are set, this defines the lower corner cell.
Definition: GridN.h:234
typename Grid< CellData * >::CellArray BaseCellArray
Datatype for array of cells in base class.
Definition: GridN.h:53
Definition of a cell in this grid.
Definition: Grid.h:57
bool hasBounds_
Flag indicating whether bounds are in effect for this grid.
Definition: GridN.h:231
unsigned int interiorCellNeighborsLimit_
Definition: GridN.h:242
iterator end() const
Return the end() iterator for the grid.
Definition: Grid.h:372
void neighbors(const Coord &coord, CellArray &list) const
Get the list of neighbors for a given coordinate.
Definition: GridN.h:136
Cell * getCell(const Coord &coord) const
Get the cell at a specified coordinate.
Definition: Grid.h:114
void getCells(CellArray &cells) const
Get the set of instantiated cells in the grid.
Definition: GridN.h:210
void neighbors(const Cell *cell, CellArray &list) const
Get the list of neighbors for a given cell.
Definition: Grid.h:122
Definition of a cell in this grid.
Definition: GridN.h:59
iterator begin() const
Return the begin() iterator for the grid.
Definition: Grid.h:366
unsigned int numberOfBoundaryDimensions(const Coord &coord) const
Compute how many sides of a coordinate touch the boundaries of the grid.
Definition: GridN.h:218
Coord upBound_
If bounds are set, this defines the upper corner cell.
Definition: GridN.h:237