37 #ifndef OMPL_DATASTRUCTURES_GRID_
38 #define OMPL_DATASTRUCTURES_GRID_
44 #include <unordered_map>
50 template <
typename _T>
55 using Coord = Eigen::VectorXi;
68 virtual ~Cell() =
default;
70 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
77 explicit Grid(
unsigned int dimension)
113 return getCell(coord) !=
nullptr;
120 Cell *c = (pos !=
hash_.end()) ? pos->second :
nullptr;
127 Coord test = cell->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);
164 std::vector<std::vector<Cell *>>
components()
const
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();
231 virtual bool remove(Cell *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));
258 void getContent(std::vector<_T> &content)
const
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();
294 unsigned int size()
const
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)
324 struct HashFunCoordPtr
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>;
355 struct SortComponents
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();
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.
void freeMemory()
Free the allocated memory.
Eigen::VectorXi Coord
Definition of a coordinate within this grid.
virtual void destroyCell(Cell *cell) const
Clear the memory occupied by a cell; do not call this function unless remove() was called first.
Definition of a cell in this grid.
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.