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