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
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines