37 #ifndef OMPL_DATASTRUCTURES_GRID_
38 #define OMPL_DATASTRUCTURES_GRID_
43 #include <unordered_map>
49 template <
typename _T>
54 using Coord = std::vector<int>;
67 virtual ~Cell() =
default;
74 explicit Grid(
unsigned int dimension)
110 return getCell(coord) !=
nullptr;
116 auto pos =
hash_.find(
const_cast<Coord *
>(&coord));
117 Cell *c = (pos !=
hash_.end()) ? pos->second :
nullptr;
145 Cell *cell = (pos !=
hash_.end()) ? pos->second :
nullptr;
148 list.push_back(cell);
151 pos =
hash_.find(&coord);
152 cell = (pos !=
hash_.end()) ? pos->second :
nullptr;
155 list.push_back(cell);
161 std::vector<std::vector<Cell *>>
components()
const
163 using ComponentHash = std::unordered_map<Coord *, int, HashFunCoordPtr, EqualCoordPtr>;
167 std::vector<std::vector<Cell *>> res;
169 for (
auto i =
hash_.begin(); i !=
hash_.end(); ++i)
171 Cell *c0 = i->second;
172 auto pos = ch.find(&c0->coord);
173 int comp = (pos != ch.end()) ? pos->second : -1;
177 res.resize(res.size() + 1);
178 std::vector<Cell *> &q = res.back();
180 std::size_t index = 0;
181 while (index < q.size())
183 Cell *c = q[index++];
184 pos = ch.find(&c->coord);
185 comp = (pos != ch.end()) ? pos->second : -1;
189 ch.insert(std::make_pair(&c->coord,
components));
190 std::vector<Cell *> nbh;
192 for (
unsigned int j = 0; j < nbh.size(); ++j)
194 pos = ch.find(&nbh[j]->coord);
195 comp = (pos != ch.end()) ? pos->second : -1;
203 q.erase(q.begin() + index);
209 std::sort(res.begin(), res.end(), SortComponents());
219 auto *cell =
new Cell();
228 virtual bool remove(Cell *cell)
232 auto pos =
hash_.find(&cell->coord);
233 if (pos !=
hash_.end())
243 virtual void add(Cell *cell)
245 hash_.insert(std::make_pair(&cell->coord, cell));
255 void getContent(std::vector<_T> &content)
const
257 for (
auto i =
hash_.begin(); i !=
hash_.end(); ++i)
258 content.push_back(i->second->data);
265 coords.push_back(i->first);
271 for (
auto i =
hash_.begin(); i !=
hash_.end(); ++i)
272 cells.push_back(i->second);
280 out << coord[i] <<
" ";
281 out <<
"]" << std::endl;
287 return hash_.empty();
291 unsigned int size()
const
297 virtual void status(std::ostream &out = std::cout)
const
299 out <<
size() <<
" total cells " << std::endl;
300 const std::vector<std::vector<Cell *>> &comp =
components();
301 out << comp.size() <<
" connected components: ";
302 for (std::size_t i = 0; i < comp.size(); ++i)
303 out << comp[i].
size() <<
" ";
315 for (
unsigned int i = 0; i < content.size(); ++i)
321 struct HashFunCoordPtr
327 for (
int i = s->size() - 1; i >= 0; --i)
329 int high = h & 0xf8000000;
331 h = h ^ (high >> 27);
334 return (std::size_t)h;
349 using CoordHash = std::unordered_map<Coord *, Cell *, HashFunCoordPtr, EqualCoordPtr>;
352 struct SortComponents
355 bool operator()(
const std::vector<Cell *> &a,
const std::vector<Cell *> &b)
const
357 return a.size() > b.size();
363 using iterator =
typename CoordHash::const_iterator;
368 return hash_.begin();
bool empty() const
Check if the grid is empty.
iterator begin() const
Return the begin() iterator for the grid.
virtual ~Grid()
Destructor.
virtual Cell * createCell(const Coord &coord, CellArray *nbh=nullptr)
std::vector< std::vector< Cell * > > components() const
Get the connected components formed by the cells in this grid (based on neighboring relation)
virtual bool remove(Cell *cell)
void getCoordinates(std::vector< Coord * > &coords) const
Get the set of coordinates where there are cells.
typename CoordHash::const_iterator iterator
We only allow const iterators.
unsigned int getDimension() const
Return the dimension of the grid.
Grid(unsigned int dimension)
The constructor takes the dimension of the grid as argument.
void getContent(std::vector< _T > &content) const
Get the data stored in the cells we are aware of.
void neighbors(const Cell *cell, CellArray &list) const
Get the list of neighbors for a given cell.
Cell * getCell(const Coord &coord) const
Get the cell at a specified coordinate.
bool operator()(const std::vector< Cell * > &a, const std::vector< Cell * > &b) const
Helper to sort components by size.
std::vector< Cell * > CellArray
The datatype for arrays of cells.
unsigned int dimension_
The dimension of the grid.
std::size_t operator()(const Coord *const s) const
Hash function for coordinates.
std::vector< int > Coord
Definition of a coordinate within this grid.
void freeMemory()
Free the allocated memory.
virtual void destroyCell(Cell *cell) const
Clear the memory occupied by a cell; do not call this function unless remove() was called first.
void printCoord(Coord &coord, std::ostream &out=std::cout) const
Print the value of a coordinate to a stream.
virtual void status(std::ostream &out=std::cout) const
Print information about the data in this grid structure.
Coord coord
The coordinate of the cell.
iterator end() const
Return the end() iterator for the grid.
unsigned int size() const
Check the size of the grid.
virtual void add(Cell *cell)
Add an instantiated cell to the grid.
bool has(const Coord &coord) const
Check if a cell exists at the specified coordinate.
CoordHash hash_
The hash holding the cells.
std::unordered_map< Coord *, Cell *, HashFunCoordPtr, EqualCoordPtr > CoordHash
Define the datatype for the used hash structure.
bool operator()(const Coord *const c1, const Coord *const c2) const
Equality operator for coordinate pointers.
_T data
The data we store in the cell.
unsigned int maxNeighbors_
The maximum number of neighbors a cell can have (2 * dimension)
void getCells(CellArray &cells) const
Get the set of instantiated cells in the grid.
void setDimension(unsigned int dimension)
virtual void clear()
Clear all cells in the grid.
Main namespace. Contains everything in this library.