ompl/datastructures/GridN.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_N_
00038 #define OMPL_DATASTRUCTURES_GRID_N_
00039 
00040 #include "ompl/datastructures/Grid.h"
00041 
00042 namespace ompl
00043 {
00044 
00046     template <typename _T>
00047     class GridN : public Grid<_T>
00048     {
00049     public:
00050 
00052         typedef typename Grid<_T>::Cell      BaseCell;
00053 
00055         typedef typename Grid<_T>::CellArray BaseCellArray;
00056 
00058         typedef typename Grid<_T>::Coord     Coord;
00059 
00061         struct Cell : public BaseCell
00062         {
00064             unsigned int        neighbors;
00065 
00067             bool                border;
00068 
00069             Cell() : BaseCell(), neighbors(0), border(true)
00070             {
00071             }
00072 
00073             virtual ~Cell()
00074             {
00075             }
00076         };
00077 
00079         typedef std::vector<Cell*> CellArray;
00080 
00081 
00083         explicit
00084         GridN(unsigned int dimension) : Grid<_T>(dimension)
00085         {
00086             hasBounds_ = false;
00087             overrideCellNeighborsLimit_ = false;
00088             setDimension(dimension);
00089         }
00090 
00091         virtual ~GridN()
00092         {
00093         }
00094 
00097         void setDimension(unsigned int dimension)
00098         {
00099             assert(Grid<_T>::empty() == true);
00100             Grid<_T>::dimension_ = dimension;
00101             Grid<_T>::maxNeighbors_ = 2 * dimension;
00102             if (!overrideCellNeighborsLimit_)
00103                 interiorCellNeighborsLimit_ = Grid<_T>::maxNeighbors_;
00104         }
00105 
00106 
00115         void setBounds(const Coord &low, const Coord &up)
00116         {
00117             lowBound_  = low;
00118             upBound_   = up;
00119             hasBounds_ = true;
00120         }
00121 
00124         void setInteriorCellNeighborLimit(unsigned int count)
00125         {
00126             interiorCellNeighborsLimit_ = count;
00127             assert(interiorCellNeighborsLimit_ > 0);
00128             overrideCellNeighborsLimit_ = true;
00129         }
00130 
00132         Cell* getCell(const Coord &coord) const
00133         {
00134             return static_cast<Cell*>(Grid<_T>::getCell(coord));
00135         }
00136 
00138         void    neighbors(const Cell* cell, CellArray& list) const
00139         {
00140             Coord test = cell->coord;
00141             neighbors(test, list);
00142         }
00143 
00145         void    neighbors(const Coord& coord, CellArray& list) const
00146         {
00147             Coord test = coord;
00148             neighbors(test, list);
00149         }
00150 
00152         void    neighbors(Coord& coord, CellArray& list) const
00153         {
00154             BaseCellArray baselist;
00155             Grid<_T>::neighbors(coord, baselist);
00156             list.reserve(list.size() + baselist.size());
00157             for (unsigned int i = 0 ;  i < baselist.size() ; ++i)
00158                 list.push_back(static_cast<Cell*>(baselist[i]));
00159         }
00160 
00166         virtual BaseCell* createCell(const Coord& coord, BaseCellArray *nbh = NULL)
00167         {
00168             Cell *cell = new Cell();
00169             cell->coord = coord;
00170 
00171             BaseCellArray *list = nbh ? nbh : new BaseCellArray();
00172             Grid<_T>::neighbors(cell->coord, *list);
00173 
00174             for (typename BaseCellArray::iterator cl = list->begin() ; cl != list->end() ; ++cl)
00175             {
00176                 Cell* c = static_cast<Cell*>(*cl);
00177                 c->neighbors++;
00178                 if (c->border && c->neighbors >= interiorCellNeighborsLimit_)
00179                     c->border = false;
00180             }
00181 
00182             cell->neighbors = numberOfBoundaryDimensions(cell->coord) + list->size();
00183             if (cell->border && cell->neighbors >= interiorCellNeighborsLimit_)
00184                 cell->border = false;
00185 
00186             if (!nbh)
00187                 delete list;
00188 
00189             return cell;
00190         }
00191 
00194         virtual bool remove(BaseCell *cell)
00195         {
00196             if (cell)
00197             {
00198                 BaseCellArray *list = new BaseCellArray();
00199                 Grid<_T>::neighbors(cell->coord, *list);
00200                 for (typename BaseCellArray::iterator cl = list->begin() ; cl != list->end() ; ++cl)
00201                 {
00202                     Cell* c = static_cast<Cell*>(*cl);
00203                     c->neighbors--;
00204                     if (!c->border && c->neighbors < interiorCellNeighborsLimit_)
00205                         c->border = true;
00206                 }
00207                 delete list;
00208                 typename Grid<_T>::CoordHash::iterator pos = Grid<_T>::hash_.find(&cell->coord);
00209                 if (pos != Grid<_T>::hash_.end())
00210                 {
00211                     Grid<_T>::hash_.erase(pos);
00212                     return true;
00213                 }
00214             }
00215             return false;
00216         }
00217 
00219         void getCells(CellArray &cells) const
00220         {
00221             for (typename Grid<_T>::CoordHash::const_iterator i = Grid<_T>::hash_.begin() ;
00222                  i != Grid<_T>::hash_.end() ; ++i)
00223                 cells.push_back(static_cast<Cell*>(i->second));
00224         }
00225 
00226     protected:
00227 
00229         unsigned int numberOfBoundaryDimensions(const Coord &coord) const
00230         {
00231             unsigned int result = 0;
00232             if (hasBounds_)
00233             {
00234                 for (unsigned int i = 0 ; i < Grid<_T>::dimension_ ; ++i)
00235                     if (coord[i] == lowBound_[i] || coord[i] == upBound_[i])
00236                         result++;
00237             }
00238             return result;
00239         }
00240 
00242         bool             hasBounds_;
00243 
00245         Coord            lowBound_;
00246 
00248         Coord            upBound_;
00249 
00253         unsigned int     interiorCellNeighborsLimit_;
00254 
00257         bool             overrideCellNeighborsLimit_;
00258     };
00259 }
00260 
00261 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines