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;
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);
222 auto *cell =
new Cell();
236 if (pos !=
hash_.end())
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();
Representation of a simple grid.
void setDimension(unsigned int dimension)
void neighbors(const Coord &coord, CellArray &list) const
Get the list of neighbors for a given coordinate.
virtual Cell * createCell(const Coord &coord, CellArray *nbh=nullptr)
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.
Eigen::VectorXi Coord
Definition of a coordinate within this grid.
unsigned int getDimension() const
Return the dimension of the grid.
std::unordered_map< Coord *, Cell *, HashFunCoordPtr, EqualCoordPtr > CoordHash
Define the datatype for the used hash structure.
void getCoordinates(std::vector< Coord * > &coords) const
Get the set of coordinates where there are cells.
virtual bool remove(Cell *cell)
unsigned int size() const
Check the size of the grid.
unsigned int maxNeighbors_
The maximum number of neighbors a cell can have (2 * dimension)
unsigned int dimension_
The dimension of the grid.
void getCells(CellArray &cells) const
Get the set of instantiated cells in the grid.
Grid(unsigned int dimension)
The constructor takes the dimension of the grid as argument.
virtual void clear()
Clear all cells in the grid.
Cell * getCell(const Coord &coord) const
Get the cell at a specified coordinate.
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
We only allow const iterators.
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
The datatype for arrays of cells.
std::vector< std::vector< Cell * > > components() const
Get the connected components formed by the cells in this grid (based on neighboring relation)
void getContent(std::vector< _T > &content) const
Get the data stored in the cells we are aware of.
CoordHash hash_
The hash holding the cells.
Main namespace. Contains everything in this library.
Definition of a cell in this grid.
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.