All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
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(void)
00068             {
00069             }
00070 
00071             virtual ~Cell(void)
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(void)
00089         {
00090             freeMemory();
00091         }
00092 
00094         virtual void clear(void)
00095         {
00096             freeMemory();
00097         }
00098 
00100         unsigned int getDimension(void) 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(void) 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(void) const
00295         {
00296             return hash_.empty();
00297         }
00298 
00300         unsigned int size(void) 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(void)
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         {
00332             std::size_t operator()(const Coord* const s) const
00333             {
00334                 unsigned long h = 0;
00335                 for (int i = s->size() - 1; i >= 0; --i)
00336                 {
00337                     int high = h & 0xf8000000;
00338                     h = h << 5;
00339                     h = h ^ (high >> 27);
00340                     h = h ^ s->at(i);
00341                 }
00342                 return (std::size_t) h;
00343             }
00344         };
00345 
00346 
00348         struct EqualCoordPtr
00349         {
00350             bool operator()(const Coord* const c1, const Coord* const c2) const
00351             {
00352                 return *c1 == *c2;
00353             }
00354         };
00355 
00357         typedef boost::unordered_map<Coord*, Cell*, HashFunCoordPtr, EqualCoordPtr> CoordHash;
00358 
00360         struct SortComponents
00361         {
00362             bool operator()(const std::vector<Cell*> &a, const std::vector<Cell*> &b) const
00363             {
00364                 return a.size() > b.size();
00365             }
00366         };
00367 
00368     public:
00369 
00371         typedef typename CoordHash::const_iterator iterator;
00372 
00374         iterator begin(void) const
00375         {
00376             return hash_.begin();
00377         }
00378 
00380         iterator end(void) const
00381         {
00382             return hash_.end();
00383         }
00384 
00385     protected:
00386 
00388         unsigned int     dimension_;
00389 
00391         unsigned int     maxNeighbors_;
00392 
00394         CoordHash        hash_;
00395     };
00396 }
00397 
00398 #endif