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 
73  using CellArray = std::vector<Cell *>;
74 
76  explicit GridN(unsigned int dimension) : Grid<_T>(dimension)
77  {
78  hasBounds_ = false;
80  setDimension(dimension);
81  }
82 
83  ~GridN() override = default;
84 
87  void setDimension(unsigned int dimension)
88  {
89  assert(Grid<_T>::empty() == true);
90  Grid<_T>::dimension_ = dimension;
91  Grid<_T>::maxNeighbors_ = 2 * dimension;
94  }
95 
104  void setBounds(const Coord &low, const Coord &up)
105  {
106  lowBound_ = low;
107  upBound_ = up;
108  hasBounds_ = true;
109  }
110 
113  void setInteriorCellNeighborLimit(unsigned int count)
114  {
116  assert(interiorCellNeighborsLimit_ > 0);
118  }
119 
121  Cell *getCell(const Coord &coord) const
122  {
123  return static_cast<Cell *>(Grid<_T>::getCell(coord));
124  }
125 
127  void neighbors(const Cell *cell, CellArray &list) const
128  {
129  Coord test = cell->coord;
130  neighbors(test, list);
131  }
132 
134  void neighbors(const Coord &coord, CellArray &list) const
135  {
136  Coord test = coord;
137  neighbors(test, list);
138  }
139 
141  void neighbors(Coord &coord, CellArray &list) const
142  {
143  BaseCellArray baselist;
144  Grid<_T>::neighbors(coord, baselist);
145  list.reserve(list.size() + baselist.size());
146  for (unsigned int i = 0; i < baselist.size(); ++i)
147  list.push_back(static_cast<Cell *>(baselist[i]));
148  }
149 
155  BaseCell *createCell(const Coord &coord, BaseCellArray *nbh = nullptr) override
156  {
157  auto *cell = new Cell();
158  cell->coord = coord;
159 
160  BaseCellArray *list = nbh ? nbh : new BaseCellArray();
161  Grid<_T>::neighbors(cell->coord, *list);
162 
163  for (auto cl = list->begin(); cl != list->end(); ++cl)
164  {
165  auto *c = static_cast<Cell *>(*cl);
166  c->neighbors++;
167  if (c->border && c->neighbors >= interiorCellNeighborsLimit_)
168  c->border = false;
169  }
170 
171  cell->neighbors = numberOfBoundaryDimensions(cell->coord) + list->size();
172  if (cell->border && cell->neighbors >= interiorCellNeighborsLimit_)
173  cell->border = false;
174 
175  if (!nbh)
176  delete list;
177 
178  return cell;
179  }
180 
183  bool remove(BaseCell *cell) override
184  {
185  if (cell)
186  {
187  auto *list = new BaseCellArray();
188  Grid<_T>::neighbors(cell->coord, *list);
189  for (auto cl = list->begin(); cl != list->end(); ++cl)
190  {
191  auto *c = static_cast<Cell *>(*cl);
192  c->neighbors--;
193  if (!c->border && c->neighbors < interiorCellNeighborsLimit_)
194  c->border = true;
195  }
196  delete list;
197  auto pos = Grid<_T>::hash_.find(&cell->coord);
198  if (pos != Grid<_T>::hash_.end())
199  {
200  Grid<_T>::hash_.erase(pos);
201  return true;
202  }
203  }
204  return false;
205  }
206 
208  void getCells(CellArray &cells) const
209  {
210  for (auto i = Grid<_T>::hash_.begin(); i != Grid<_T>::hash_.end(); ++i)
211  cells.push_back(static_cast<Cell *>(i->second));
212  }
213 
214  protected:
216  unsigned int numberOfBoundaryDimensions(const Coord &coord) const
217  {
218  unsigned int result = 0;
219  if (hasBounds_)
220  {
221  for (unsigned int i = 0; i < Grid<_T>::dimension_; ++i)
222  if (coord[i] == lowBound_[i] || coord[i] == upBound_[i])
223  result++;
224  }
225  return result;
226  }
227 
230 
233 
236 
241 
245  };
246 }
247 
248 #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:104
void setInteriorCellNeighborLimit(unsigned int count)
Definition: GridN.h:113
void neighbors(const Cell *cell, CellArray &list) const
Get the list of neighbors for a given cell.
Definition: GridN.h:127
bool overrideCellNeighborsLimit_
Definition: GridN.h:244
Cell * getCell(const Coord &coord) const
Get the cell at a specified coordinate.
Definition: GridN.h:121
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:87
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:76
void neighbors(Coord &coord, CellArray &list) const
Get the list of neighbors for a given coordinate.
Definition: GridN.h:141
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:155
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:232
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:229
unsigned int interiorCellNeighborsLimit_
Definition: GridN.h:240
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:134
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:208
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:216
Coord upBound_
If bounds are set, this defines the upper corner cell.
Definition: GridN.h:235