37 #ifndef OMPL_DATASTRUCTURES_GRID_
38 #define OMPL_DATASTRUCTURES_GRID_
43 #include <boost/unordered_map.hpp>
50 template <
typename _T>
82 Grid(
unsigned int dimension)
125 Cell *c = (pos !=
hash_.end()) ? pos->second : NULL;
132 Coord test = cell->coord;
153 Cell *cell = (pos !=
hash_.end()) ? pos->second : NULL;
156 list.push_back(cell);
159 pos =
hash_.find(&coord);
160 cell = (pos !=
hash_.end()) ? pos->second : NULL;
163 list.push_back(cell);
171 typedef boost::unordered_map<Coord*, int, HashFunCoordPtr, EqualCoordPtr> ComponentHash;
172 typedef typename ComponentHash::iterator CHit;
176 std::vector< std::vector<Cell*> > res;
180 Cell *c0 = i->second;
181 CHit pos = ch.find(&c0->coord);
182 int comp = (pos != ch.end()) ? pos->second : -1;
186 res.resize(res.size() + 1);
187 std::vector<Cell*> &q = res.back();
189 std::size_t index = 0;
190 while (index < q.size())
192 Cell *c = q[index++];
193 pos = ch.find(&c->coord);
194 comp = (pos != ch.end()) ? pos->second : -1;
198 ch.insert(std::make_pair(&c->coord, components));
199 std::vector< Cell* > nbh;
201 for (
unsigned int j = 0 ; j < nbh.size() ; ++j)
203 pos = ch.find(&nbh[j]->coord);
204 comp = (pos != ch.end()) ? pos->second : -1;
212 q.erase(q.begin() + index);
218 std::sort(res.begin(), res.end(), SortComponents());
228 Cell *cell =
new Cell();
237 virtual bool remove(Cell *cell)
241 typename CoordHash::iterator pos =
hash_.find(&cell->coord);
242 if (pos !=
hash_.end())
252 virtual void add(Cell *cell)
254 hash_.insert(std::make_pair(&cell->coord, cell));
267 content.push_back(i->second->data);
274 coords.push_back(i->first);
281 cells.push_back(i->second);
288 for (
unsigned int i = 0 ; i <
dimension_ ; ++i)
289 out << coord[i] <<
" ";
290 out <<
"]" << std::endl;
296 return hash_.empty();
306 virtual void status(std::ostream &out = std::cout)
const
308 out <<
size() <<
" total cells " << std::endl;
309 const std::vector< std::vector<Cell*> > &comp =
components();
310 out << comp.size() <<
" connected components: ";
311 for (std::size_t i = 0 ; i < comp.size() ; ++i)
312 out << comp[i].
size() <<
" ";
325 for (
unsigned int i = 0 ; i < content.size() ; ++i)
336 for (
int i = s->size() - 1; i >= 0; --i)
338 int high = h & 0xf8000000;
340 h = h ^ (high >> 27);
343 return (std::size_t) h;
359 typedef boost::unordered_map<Coord*, Cell*, HashFunCoordPtr, EqualCoordPtr>
CoordHash;
365 bool operator()(
const std::vector<Cell*> &a,
const std::vector<Cell*> &b)
const
367 return a.size() > b.size();
374 typedef typename CoordHash::const_iterator
iterator;
379 return hash_.begin();
void printCoord(Coord &coord, std::ostream &out=std::cout) const
Print the value of a coordinate to a stream.
Helper to sort components by size.
virtual void destroyCell(Cell *cell) const
Clear the memory occupied by a cell; do not call this function unless remove() was called first...
Representation of a simple grid.
unsigned int dimension_
The dimension of the grid.
std::vector< int > Coord
Definition of a coordinate within this grid.
virtual void status(std::ostream &out=std::cout) const
Print information about the data in this grid structure.
virtual Cell * createCell(const Coord &coord, CellArray *nbh=NULL)
bool operator()(const std::vector< Cell * > &a, const std::vector< Cell * > &b) const
Helper to sort components by size.
_T data
The data we store in the cell.
Grid(unsigned int dimension)
The constructor takes the dimension of the grid as argument.
Coord coord
The coordinate of the cell.
void neighbors(const Coord &coord, CellArray &list) const
Get the list of neighbors for a given coordinate.
std::vector< Cell * > CellArray
The datatype for arrays of cells.
unsigned int maxNeighbors_
The maximum number of neighbors a cell can have (2 * dimension)
void neighbors(Coord &coord, CellArray &list) const
Get the list of neighbors for a given coordinate.
Cell * getCell(const Coord &coord) const
Get the cell at a specified coordinate.
iterator begin(void) const
Return the begin() iterator for the grid.
void setDimension(unsigned int dimension)
void getCells(CellArray &cells) const
Get the set of instantiated cells in the grid.
std::size_t operator()(const Coord *const s) const
Hash function for coordinates.
virtual void add(Cell *cell)
Add an instantiated cell to the grid.
void freeMemory(void)
Free the allocated memory.
bool empty(void) const
Check if the grid is empty.
bool operator()(const Coord *const c1, const Coord *const c2) const
Equality operator for coordinate pointers.
CoordHash hash_
The hash holding the cells.
unsigned int getDimension(void) const
Return the dimension of the grid.
Definition of a cell in this grid.
std::vector< std::vector< Cell * > > components(void) const
Get the connected components formed by the cells in this grid (based on neighboring relation) ...
void neighbors(const Cell *cell, CellArray &list) const
Get the list of neighbors for a given cell.
virtual ~Grid(void)
Destructor.
Hash function for coordinates; see http://www.cs.hmc.edu/~geoff/classes/hmc.cs070.200101/homework10/hashfuncs.html.
bool has(const Coord &coord) const
Check if a cell exists at the specified coordinate.
virtual void clear(void)
Clear all cells in the grid.
Equality operator for coordinate pointers.
void getContent(std::vector< _T > &content) const
Get the data stored in the cells we are aware of.
iterator end(void) const
Return the end() iterator for the grid.
boost::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.
CoordHash::const_iterator iterator
We only allow const iterators.
unsigned int size(void) const
Check the size of the grid.