37 #ifndef OMPL_DATASTRUCTURES_GRID_ 38 #define OMPL_DATASTRUCTURES_GRID_ 43 #include <unordered_map> 49 template <
typename _T>
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;
124 Coord test = cell->coord;
144 auto pos =
hash_.find(&coord);
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);
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));
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();
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)
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>;
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();
Helper to sort components by size.
unsigned int getDimension() const
Return the dimension of the grid.
void neighbors(const Coord &coord, CellArray &list) const
Get the list of neighbors for a given coordinate.
bool empty() const
Check if the grid is empty.
void freeMemory()
Free the allocated memory.
Representation of a simple grid.
virtual void clear()
Clear all cells in the grid.
unsigned int dimension_
The dimension of the grid.
virtual void destroyCell(Cell *cell) const
Clear the memory occupied by a cell; do not call this function unless remove() was called first...
_T data
The data we store in the cell.
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.
virtual Cell * createCell(const Coord &coord, CellArray *nbh=nullptr)
Coord coord
The coordinate of the cell.
std::vector< int > Coord
Definition of a coordinate within this grid.
bool has(const Coord &coord) const
Check if a cell exists at the specified coordinate.
unsigned int maxNeighbors_
The maximum number of neighbors a cell can have (2 * dimension)
std::size_t operator()(const Coord *const s) const
Hash function for coordinates.
Main namespace. Contains everything in this library.
virtual void status(std::ostream &out=std::cout) const
Print information about the data in this grid structure.
void setDimension(unsigned int dimension)
virtual void add(Cell *cell)
Add an instantiated cell to the grid.
bool operator()(const Coord *const c1, const Coord *const c2) const
Equality operator for coordinate pointers.
virtual ~Grid()
Destructor.
std::vector< Cell *> CellArray
The datatype for arrays of cells.
void getCoordinates(std::vector< Coord *> &coords) const
Get the set of coordinates where there are cells.
void getCells(CellArray &cells) const
Get the set of instantiated cells in the grid.
CoordHash hash_
The hash holding the cells.
bool operator()(const std::vector< Cell *> &a, const std::vector< Cell *> &b) const
Helper to sort components by size.
Definition of a cell in this grid.
unsigned int size() const
Check the size of the grid.
std::vector< std::vector< Cell * > > components() const
Get the connected components formed by the cells in this grid (based on neighboring relation) ...
iterator end() const
Return the end() iterator for the grid.
typename CoordHash::const_iterator iterator
We only allow const iterators.
Cell * getCell(const Coord &coord) const
Get the cell at a specified coordinate.
void neighbors(const Cell *cell, CellArray &list) const
Get the list of neighbors for a given cell.
Equality operator for coordinate pointers.
void neighbors(Coord &coord, CellArray &list) const
Get the list of neighbors for a given coordinate.
std::unordered_map< Coord *, Cell *, HashFunCoordPtr, EqualCoordPtr > CoordHash
Define the datatype for the used hash structure.
void printCoord(Coord &coord, std::ostream &out=std::cout) const
Print the value of a coordinate to a stream.
iterator begin() const
Return the begin() iterator for the grid.