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();