ompl/datastructures/GridB.h
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2008, Willow Garage, Inc. 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 Willow Garage 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_B_ 00038 #define OMPL_DATASTRUCTURES_GRID_B_ 00039 00040 #include "ompl/datastructures/GridN.h" 00041 #include "ompl/datastructures/BinaryHeap.h" 00042 00043 namespace ompl 00044 { 00045 00048 template < typename _T, 00049 class LessThanExternal = std::less<_T>, 00050 class LessThanInternal = LessThanExternal > 00051 class GridB : public GridN<_T> 00052 { 00053 public: 00054 00056 typedef typename GridN<_T>::Cell Cell; 00057 00059 typedef typename GridN<_T>::CellArray CellArray; 00060 00062 typedef typename GridN<_T>::Coord Coord; 00063 00064 protected: 00065 00067 // the type of cell here needs an extra pointer to allow the updatable heap to work fast 00068 // however, this stays hidden from the user 00069 struct CellX : public Cell 00070 { 00071 CellX() : Cell() 00072 { 00073 } 00074 00075 virtual ~CellX() 00076 { 00077 } 00078 00079 void *heapElement; 00080 }; 00081 00083 00084 public: 00085 00087 typedef void (*EventCellUpdate)(Cell*, void*); 00088 00090 explicit 00091 GridB(unsigned int dimension) : GridN<_T>(dimension) 00092 { 00093 setupHeaps(); 00094 } 00095 00096 virtual ~GridB() 00097 { 00098 clearHeaps(); 00099 } 00100 00103 void onCellUpdate(EventCellUpdate event, void *arg) 00104 { 00105 eventCellUpdate_ = event; 00106 eventCellUpdateData_ = arg; 00107 } 00108 00110 Cell* topInternal() const 00111 { 00112 Cell* top = static_cast<Cell*>(internal_.top()->data); 00113 return top ? top : topExternal(); 00114 } 00115 00117 Cell* topExternal() const 00118 { 00119 Cell* top = static_cast<Cell*>(external_.top()->data); 00120 return top ? top : topInternal(); 00121 } 00122 00124 unsigned int countInternal() const 00125 { 00126 return internal_.size(); 00127 } 00128 00130 unsigned int countExternal() const 00131 { 00132 return external_.size(); 00133 } 00134 00136 double fracExternal() const 00137 { 00138 return external_.empty() ? 0.0 : (double)(external_.size()) / (double)(external_.size() + internal_.size()); 00139 } 00140 00142 double fracInternal() const 00143 { 00144 return 1.0 - fracExternal(); 00145 } 00146 00148 void update(Cell* cell) 00149 { 00150 eventCellUpdate_(cell, eventCellUpdateData_); 00151 if (cell->border) 00152 external_.update(reinterpret_cast<typename externalBHeap::Element*> 00153 (static_cast<CellX*>(cell)->heapElement)); 00154 else 00155 internal_.update(reinterpret_cast<typename internalBHeap::Element*> 00156 (static_cast<CellX*>(cell)->heapElement)); 00157 } 00158 00160 void updateAll() 00161 { 00162 std::vector< Cell* > cells; 00163 this->getCells(cells); 00164 for (int i = cells.size() - 1 ; i >= 0 ; --i) 00165 eventCellUpdate_(cells[i], eventCellUpdateData_); 00166 external_.rebuild(); 00167 internal_.rebuild(); 00168 } 00169 00171 virtual Cell* createCell(const Coord& coord, CellArray *nbh = NULL) 00172 { 00173 CellX* cell = new CellX(); 00174 cell->coord = coord; 00175 00176 CellArray *list = nbh ? nbh : new CellArray(); 00177 this->neighbors(cell->coord, *list); 00178 00179 for (typename CellArray::iterator cl = list->begin() ; cl != list->end() ; ++cl) 00180 { 00181 CellX* c = static_cast<CellX*>(*cl); 00182 bool wasBorder = c->border; 00183 c->neighbors++; 00184 if (c->border && c->neighbors >= GridN<_T>::interiorCellNeighborsLimit_) 00185 c->border = false; 00186 00187 eventCellUpdate_(c, eventCellUpdateData_); 00188 00189 if (c->border) 00190 external_.update(reinterpret_cast<typename externalBHeap::Element*>(c->heapElement)); 00191 else 00192 { 00193 if (wasBorder) 00194 { 00195 external_.remove(reinterpret_cast<typename externalBHeap::Element*>(c->heapElement)); 00196 internal_.insert(c); 00197 } 00198 else 00199 internal_.update(reinterpret_cast<typename internalBHeap::Element*>(c->heapElement)); 00200 } 00201 } 00202 00203 cell->neighbors = GridN<_T>::numberOfBoundaryDimensions(cell->coord) + list->size(); 00204 if (cell->border && cell->neighbors >= GridN<_T>::interiorCellNeighborsLimit_) 00205 cell->border = false; 00206 00207 if (!nbh) 00208 delete list; 00209 00210 return static_cast<Cell*>(cell); 00211 } 00212 00214 virtual void add(Cell* cell) 00215 { 00216 CellX* ccell = static_cast<CellX*>(cell); 00217 eventCellUpdate_(ccell, eventCellUpdateData_); 00218 00219 GridN<_T>::add(cell); 00220 00221 if (cell->border) 00222 external_.insert(ccell); 00223 else 00224 internal_.insert(ccell); 00225 } 00226 00228 virtual bool remove(Cell* cell) 00229 { 00230 if (cell) 00231 { 00232 CellArray *list = new CellArray(); 00233 this->neighbors(cell->coord, *list); 00234 00235 for (typename CellArray::iterator cl = list->begin() ; cl != list->end() ; ++cl) 00236 { 00237 CellX* c = static_cast<CellX*>(*cl); 00238 bool wasBorder = c->border; 00239 c->neighbors--; 00240 if (!c->border && c->neighbors < GridN<_T>::interiorCellNeighborsLimit_) 00241 c->border = true; 00242 00243 eventCellUpdate_(c, eventCellUpdateData_); 00244 00245 if (c->border) 00246 { 00247 if (wasBorder) 00248 external_.update(reinterpret_cast<typename externalBHeap::Element*>(c->heapElement)); 00249 else 00250 { 00251 internal_.remove(reinterpret_cast<typename internalBHeap::Element*>(c->heapElement)); 00252 external_.insert(c); 00253 } 00254 } 00255 else 00256 internal_.update(reinterpret_cast<typename internalBHeap::Element*>(c->heapElement)); 00257 } 00258 00259 delete list; 00260 00261 typename GridN<_T>::CoordHash::iterator pos = GridN<_T>::hash_.find(&cell->coord); 00262 if (pos != GridN<_T>::hash_.end()) 00263 { 00264 GridN<_T>::hash_.erase(pos); 00265 CellX* cx = static_cast<CellX*>(cell); 00266 if (cx->border) 00267 external_.remove(reinterpret_cast<typename externalBHeap::Element*>(cx->heapElement)); 00268 else 00269 internal_.remove(reinterpret_cast<typename internalBHeap::Element*>(cx->heapElement)); 00270 return true; 00271 } 00272 } 00273 return false; 00274 } 00275 00276 virtual void clear() 00277 { 00278 GridN<_T>::clear(); 00279 clearHeaps(); 00280 } 00281 00282 virtual void status(std::ostream &out = std::cout) const 00283 { 00284 GridN<_T>::status(out); 00285 out << countInternal() << " internal cells" << std::endl; 00286 out << countExternal() << " external cells" << std::endl; 00287 } 00288 00289 protected: 00290 00292 EventCellUpdate eventCellUpdate_; 00293 00295 void *eventCellUpdateData_; 00296 00298 static void noCellUpdate(Cell*, void*) 00299 { 00300 } 00301 00303 void setupHeaps() 00304 { 00305 eventCellUpdate_ = &noCellUpdate; 00306 eventCellUpdateData_ = NULL; 00307 internal_.onAfterInsert(&setHeapElementI, NULL); 00308 external_.onAfterInsert(&setHeapElementE, NULL); 00309 } 00310 00312 void clearHeaps() 00313 { 00314 internal_.clear(); 00315 external_.clear(); 00316 } 00317 00319 struct LessThanInternalCell 00320 { 00321 bool operator()(const CellX* const a, const CellX* const b) const 00322 { 00323 return lt_(a->data, b->data); 00324 } 00325 00326 private: 00327 LessThanInternal lt_; 00328 }; 00329 00331 struct LessThanExternalCell 00332 { 00333 bool operator()(const CellX* const a, const CellX* const b) const 00334 { 00335 return lt_(a->data, b->data); 00336 } 00337 private: 00338 LessThanExternal lt_; 00339 }; 00340 00342 typedef BinaryHeap< CellX*, LessThanInternalCell > internalBHeap; 00343 00345 typedef BinaryHeap< CellX*, LessThanExternalCell > externalBHeap; 00346 00348 static void setHeapElementI(typename internalBHeap::Element *element, void*) 00349 { 00350 element->data->heapElement = reinterpret_cast<void*>(element); 00351 } 00352 00354 static void setHeapElementE(typename externalBHeap::Element *element, void*) 00355 { 00356 element->data->heapElement = reinterpret_cast<void*>(element); 00357 } 00358 00360 internalBHeap internal_; 00361 00363 externalBHeap external_; 00364 }; 00365 00366 } 00367 00368 #endif