ompl/datastructures/Grid.h
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, Rice University 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Rice University nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Author: Ioan Sucan */ 00036 00037 #ifndef OMPL_DATASTRUCTURES_GRID_ 00038 #define OMPL_DATASTRUCTURES_GRID_ 00039 00040 #include <vector> 00041 #include <iostream> 00042 #include <cstdlib> 00043 #include <boost/unordered_map.hpp> 00044 #include <algorithm> 00045 00046 namespace ompl 00047 { 00048 00050 template <typename _T> 00051 class Grid 00052 { 00053 public: 00054 00056 typedef std::vector<int> Coord; 00057 00059 struct Cell 00060 { 00062 _T data; 00063 00065 Coord coord; 00066 00067 Cell() 00068 { 00069 } 00070 00071 virtual ~Cell() 00072 { 00073 } 00074 }; 00075 00077 typedef std::vector<Cell*> CellArray; 00078 00079 00081 explicit 00082 Grid(unsigned int dimension) 00083 { 00084 setDimension(dimension); 00085 } 00086 00088 virtual ~Grid() 00089 { 00090 freeMemory(); 00091 } 00092 00094 virtual void clear() 00095 { 00096 freeMemory(); 00097 } 00098 00100 unsigned int getDimension() const 00101 { 00102 return dimension_; 00103 } 00104 00107 void setDimension(unsigned int dimension) 00108 { 00109 if (!empty()) 00110 throw; 00111 dimension_ = dimension; 00112 maxNeighbors_ = 2 * dimension_; 00113 } 00114 00116 bool has(const Coord &coord) const 00117 { 00118 return getCell(coord) != NULL; 00119 } 00120 00122 Cell* getCell(const Coord &coord) const 00123 { 00124 iterator pos = hash_.find(const_cast<Coord*>(&coord)); 00125 Cell *c = (pos != hash_.end()) ? pos->second : NULL; 00126 return c; 00127 } 00128 00130 void neighbors(const Cell* cell, CellArray& list) const 00131 { 00132 Coord test = cell->coord; 00133 neighbors(test, list); 00134 } 00135 00137 void neighbors(const Coord& coord, CellArray& list) const 00138 { 00139 Coord test = coord; 00140 neighbors(test, list); 00141 } 00142 00144 void neighbors(Coord& coord, CellArray& list) const 00145 { 00146 list.reserve(list.size() + maxNeighbors_); 00147 00148 for (int i = dimension_ - 1 ; i >= 0 ; --i) 00149 { 00150 coord[i]--; 00151 00152 iterator pos = hash_.find(&coord); 00153 Cell *cell = (pos != hash_.end()) ? pos->second : NULL; 00154 00155 if (cell) 00156 list.push_back(cell); 00157 coord[i] += 2; 00158 00159 pos = hash_.find(&coord); 00160 cell = (pos != hash_.end()) ? pos->second : NULL; 00161 00162 if (cell) 00163 list.push_back(cell); 00164 coord[i]--; 00165 } 00166 } 00167 00169 std::vector< std::vector<Cell*> > components() const 00170 { 00171 typedef boost::unordered_map<Coord*, int, HashFunCoordPtr, EqualCoordPtr> ComponentHash; 00172 typedef typename ComponentHash::iterator CHit; 00173 00174 int components = 0; 00175 ComponentHash ch; 00176 std::vector< std::vector<Cell*> > res; 00177 00178 for (iterator i = hash_.begin() ; i != hash_.end() ; ++i) 00179 { 00180 Cell *c0 = i->second; 00181 CHit pos = ch.find(&c0->coord); 00182 int comp = (pos != ch.end()) ? pos->second : -1; 00183 00184 if (comp < 0) 00185 { 00186 res.resize(res.size() + 1); 00187 std::vector<Cell*> &q = res.back(); 00188 q.push_back(c0); 00189 std::size_t index = 0; 00190 while (index < q.size()) 00191 { 00192 Cell *c = q[index++]; 00193 pos = ch.find(&c->coord); 00194 comp = (pos != ch.end()) ? pos->second : -1; 00195 00196 if (comp < 0) 00197 { 00198 ch.insert(std::make_pair(&c->coord, components)); 00199 std::vector< Cell* > nbh; 00200 neighbors(c, nbh); 00201 for (unsigned int j = 0 ; j < nbh.size() ; ++j) 00202 { 00203 pos = ch.find(&nbh[j]->coord); 00204 comp = (pos != ch.end()) ? pos->second : -1; 00205 if (comp < 0) 00206 q.push_back(nbh[j]); 00207 } 00208 } 00209 else 00210 { 00211 --index; 00212 q.erase(q.begin() + index); 00213 } 00214 } 00215 ++components; 00216 } 00217 } 00218 std::sort(res.begin(), res.end(), SortComponents()); 00219 return res; 00220 } 00221 00226 virtual Cell* createCell(const Coord& coord, CellArray *nbh = NULL) 00227 { 00228 Cell *cell = new Cell(); 00229 cell->coord = coord; 00230 if (nbh) 00231 neighbors(cell->coord, *nbh); 00232 return cell; 00233 } 00234 00237 virtual bool remove(Cell *cell) 00238 { 00239 if (cell) 00240 { 00241 typename CoordHash::iterator pos = hash_.find(&cell->coord); 00242 if (pos != hash_.end()) 00243 { 00244 hash_.erase(pos); 00245 return true; 00246 } 00247 } 00248 return false; 00249 } 00250 00252 virtual void add(Cell *cell) 00253 { 00254 hash_.insert(std::make_pair(&cell->coord, cell)); 00255 } 00256 00258 virtual void destroyCell(Cell *cell) const 00259 { 00260 delete cell; 00261 } 00262 00264 void getContent(std::vector<_T> &content) const 00265 { 00266 for (iterator i = hash_.begin() ; i != hash_.end() ; ++i) 00267 content.push_back(i->second->data); 00268 } 00269 00271 void getCoordinates(std::vector<Coord*> &coords) const 00272 { 00273 for (iterator i = hash_.begin() ; i != hash_.end() ; ++i) 00274 coords.push_back(i->first); 00275 } 00276 00278 void getCells(CellArray &cells) const 00279 { 00280 for (iterator i = hash_.begin() ; i != hash_.end() ; ++i) 00281 cells.push_back(i->second); 00282 } 00283 00285 void printCoord(Coord& coord, std::ostream &out = std::cout) const 00286 { 00287 out << "[ "; 00288 for (unsigned int i = 0 ; i < dimension_ ; ++i) 00289 out << coord[i] << " "; 00290 out << "]" << std::endl; 00291 } 00292 00294 bool empty() const 00295 { 00296 return hash_.empty(); 00297 } 00298 00300 unsigned int size() const 00301 { 00302 return hash_.size(); 00303 } 00304 00306 virtual void status(std::ostream &out = std::cout) const 00307 { 00308 out << size() << " total cells " << std::endl; 00309 const std::vector< std::vector<Cell*> > &comp = components(); 00310 out << comp.size() << " connected components: "; 00311 for (std::size_t i = 0 ; i < comp.size() ; ++i) 00312 out << comp[i].size() << " "; 00313 out << std::endl; 00314 } 00315 00316 protected: 00317 00319 void freeMemory() 00320 { 00321 CellArray content; 00322 getCells(content); 00323 hash_.clear(); 00324 00325 for (unsigned int i = 0 ; i < content.size() ; ++i) 00326 delete content[i]; 00327 } 00328 00330 struct HashFunCoordPtr 00331 { 00333 std::size_t operator()(const Coord* const s) const 00334 { 00335 unsigned long h = 0; 00336 for (int i = s->size() - 1; i >= 0; --i) 00337 { 00338 int high = h & 0xf8000000; 00339 h = h << 5; 00340 h = h ^ (high >> 27); 00341 h = h ^ s->at(i); 00342 } 00343 return (std::size_t) h; 00344 } 00345 }; 00346 00347 00349 struct EqualCoordPtr 00350 { 00352 bool operator()(const Coord* const c1, const Coord* const c2) const 00353 { 00354 return *c1 == *c2; 00355 } 00356 }; 00357 00359 typedef boost::unordered_map<Coord*, Cell*, HashFunCoordPtr, EqualCoordPtr> CoordHash; 00360 00362 struct SortComponents 00363 { 00365 bool operator()(const std::vector<Cell*> &a, const std::vector<Cell*> &b) const 00366 { 00367 return a.size() > b.size(); 00368 } 00369 }; 00370 00371 public: 00372 00374 typedef typename CoordHash::const_iterator iterator; 00375 00377 iterator begin() const 00378 { 00379 return hash_.begin(); 00380 } 00381 00383 iterator end() const 00384 { 00385 return hash_.end(); 00386 } 00387 00388 protected: 00389 00391 unsigned int dimension_; 00392 00394 unsigned int maxNeighbors_; 00395 00397 CoordHash hash_; 00398 }; 00399 } 00400 00401 #endif