37#ifndef OMPL_DATASTRUCTURES_GRID_
38#define OMPL_DATASTRUCTURES_GRID_
44#include <unordered_map>
50 template <
typename _T>
68 virtual ~Cell() =
default;
70 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
77 explicit Grid(
unsigned int dimension)
113 return getCell(coord) !=
nullptr;
119 auto pos =
hash_.find(
const_cast<Coord *
>(&coord));
120 Cell *c = (pos !=
hash_.end()) ? pos->second :
nullptr;
127 Coord test = cell->coord;
147 auto pos =
hash_.find(&coord);
148 Cell *cell = (pos !=
hash_.end()) ? pos->second :
nullptr;
151 list.push_back(cell);
154 pos =
hash_.find(&coord);
155 cell = (pos !=
hash_.end()) ? pos->second :
nullptr;
158 list.push_back(cell);
166 using ComponentHash = std::unordered_map<Coord *, int, HashFunCoordPtr, EqualCoordPtr>;
170 std::vector<std::vector<Cell *>> res;
172 for (
auto & i:
hash_)
175 auto pos = ch.find(&c0->coord);
176 int comp = (pos != ch.end()) ? pos->second : -1;
180 res.resize(res.size() + 1);
181 std::vector<Cell *> &q = res.back();
183 std::size_t index = 0;
184 while (index < q.size())
186 Cell *c = q[index++];
187 pos = ch.find(&c->coord);
188 comp = (pos != ch.end()) ? pos->second : -1;
192 ch.insert(std::make_pair(&c->coord,
components));
193 std::vector<Cell *> nbh;
195 for (
const auto &n : nbh)
197 pos = ch.find(&n->coord);
198 comp = (pos != ch.end()) ? pos->second : -1;
206 q.erase(q.begin() + index);
212 std::sort(res.begin(), res.end(), SortComponents());
222 auto *cell =
new Cell();
235 auto pos =
hash_.find(&cell->coord);
236 if (pos !=
hash_.end())
246 virtual void add(Cell *cell)
248 hash_.insert(std::make_pair(&cell->coord, cell));
260 for (
const auto &h :
hash_)
261 content.push_back(h.second->data);
267 for (
const auto &h :
hash_)
268 coords.push_back(h.first);
274 for (
const auto &h :
hash_)
275 cells.push_back(h.second);
283 out << coord[i] <<
" ";
284 out <<
"]" << std::endl;
290 return hash_.empty();
300 virtual void status(std::ostream &out = std::cout)
const
302 out <<
size() <<
" total cells " << std::endl;
303 const std::vector<std::vector<Cell *>> &comp =
components();
304 out << comp.size() <<
" connected components: ";
305 for (
const auto &c : comp)
306 out << c.size() <<
" ";
318 for (
auto &c : content)
330 for (
int i = s->size() - 1; i >= 0; --i)
332 int high = h & 0xf8000000;
334 h = h ^ (high >> 27);
337 return (std::size_t)h;
352 using CoordHash = std::unordered_map<Coord *, Cell *, HashFunCoordPtr, EqualCoordPtr>;
358 bool operator()(
const std::vector<Cell *> &a,
const std::vector<Cell *> &b)
const
360 return a.size() > b.size();
366 using iterator =
typename CoordHash::const_iterator;
371 return hash_.begin();
void setDimension(unsigned int dimension)
void neighbors(const Coord &coord, CellArray &list) const
Get the list of neighbors for a given coordinate.
void neighbors(const Cell *cell, CellArray &list) const
Get the list of neighbors for a given cell.
bool empty() const
Check if the grid is empty.
void printCoord(Coord &coord, std::ostream &out=std::cout) const
Print the value of a coordinate to a stream.
bool has(const Coord &coord) const
Check if a cell exists at the specified coordinate.
virtual void status(std::ostream &out=std::cout) const
Print information about the data in this grid structure.
virtual void destroyCell(Cell *cell) const
Clear the memory occupied by a cell; do not call this function unless remove() was called first.
unsigned int getDimension() const
Return the dimension of the grid.
std::unordered_map< Coord *, Cell *, HashFunCoordPtr, EqualCoordPtr > CoordHash
void getCoordinates(std::vector< Coord * > &coords) const
Get the set of coordinates where there are cells.
Cell * getCell(const Coord &coord) const
Get the cell at a specified coordinate.
virtual bool remove(Cell *cell)
unsigned int size() const
Check the size of the grid.
unsigned int maxNeighbors_
void getCells(CellArray &cells) const
Get the set of instantiated cells in the grid.
std::vector< std::vector< Cell * > > components() const
Get the connected components formed by the cells in this grid (based on neighboring relation)
Grid(unsigned int dimension)
The constructor takes the dimension of the grid as argument.
virtual void clear()
Clear all cells in the grid.
void freeMemory()
Free the allocated memory.
iterator end() const
Return the end() iterator for the grid.
void neighbors(Coord &coord, CellArray &list) const
Get the list of neighbors for a given coordinate.
typename CoordHash::const_iterator iterator
virtual void add(Cell *cell)
Add an instantiated cell to the grid.
virtual ~Grid()
Destructor.
iterator begin() const
Return the begin() iterator for the grid.
std::vector< Cell * > CellArray
void getContent(std::vector< _T > &content) const
Get the data stored in the cells we are aware of.
virtual Cell * createCell(const Coord &coord, CellArray *nbh=nullptr)
Main namespace. Contains everything in this library.
Coord coord
The coordinate of the cell.
_T data
The data we store in the cell.
Equality operator for coordinate pointers.
bool operator()(const Coord *const c1, const Coord *const c2) const
Equality operator for coordinate pointers.
std::size_t operator()(const Coord *const s) const
Hash function for coordinates.
Helper to sort components by size.
bool operator()(const std::vector< Cell * > &a, const std::vector< Cell * > &b) const
Helper to sort components by size.