ompl/datastructures/BinaryHeap.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_BINARY_HEAP_
00038 #define OMPL_DATASTRUCTURES_BINARY_HEAP_
00039 
00040 #include <functional>
00041 #include <vector>
00042 #include <cassert>
00043 
00044 namespace ompl
00045 {
00046 
00051     template <typename _T,
00052               class LessThan = std::less<_T> >
00053     class BinaryHeap
00054     {
00055     public:
00056 
00061         class Element
00062         {
00063             friend class BinaryHeap;
00064         private:
00065             Element() { }
00066             ~Element() { }
00068             unsigned int position;
00069         public:
00071             _T           data;
00072         };
00073 
00075         typedef void (*EventAfterInsert) (Element*, void*);
00076 
00078         typedef void (*EventBeforeRemove)(Element*, void*);
00079 
00080         BinaryHeap()
00081         {
00082             eventAfterInsert_  = NULL;
00083             eventBeforeRemove_ = NULL;
00084         }
00085 
00086         ~BinaryHeap()
00087         {
00088             clear();
00089         }
00090 
00092         void onAfterInsert(EventAfterInsert event, void *arg)
00093         {
00094             eventAfterInsert_ = event;
00095             eventAfterInsertData_ = arg;
00096         }
00097 
00099         void onBeforeRemove(EventBeforeRemove event, void *arg)
00100         {
00101             eventBeforeRemove_ = event;
00102             eventBeforeRemoveData_ = arg;
00103         }
00104 
00106         void clear()
00107         {
00108             for (typename std::vector<Element*>::iterator i = vector_.begin() ;
00109                  i != vector_.end() ; ++i)
00110                 delete *i;
00111             vector_.clear();
00112         }
00113 
00115         Element* top() const
00116         {
00117             return vector_.empty() ? NULL : vector_.at(0);
00118         }
00119 
00121         void pop()
00122         {
00123             removePos(0);
00124         }
00125 
00127         void remove(Element *element)
00128         {
00129             if (eventBeforeRemove_)
00130                 eventBeforeRemove_(element, eventBeforeRemoveData_);
00131             removePos(element->position);
00132         }
00133 
00135         Element* insert(const _T& data)
00136         {
00137             Element *element = new Element();
00138             element->data = data;
00139             const unsigned int pos = vector_.size();
00140             element->position = pos;
00141             vector_.push_back(element);
00142             percolateUp(pos);
00143             if (eventAfterInsert_)
00144                 eventAfterInsert_(element, eventAfterInsertData_);
00145             return element;
00146         }
00147 
00149         void insert(const std::vector<_T>& list)
00150         {
00151             const unsigned int n = vector_.size();
00152             const unsigned int m = list.size();
00153             for (unsigned int i = 0 ; i < m ; ++i)
00154             {
00155                 const unsigned int pos = i + n;
00156                 Element *element = newElement(list[i], pos);
00157                 vector_.push_back(element);
00158                 percolateUp(pos);
00159                 if (eventAfterInsert_)
00160                     eventAfterInsert_(element, eventAfterInsertData_);
00161             }
00162         }
00163 
00165         void buildFrom(const std::vector<_T>& list)
00166         {
00167             clear();
00168             const unsigned int m = list.size();
00169             for (unsigned int i = 0 ; i < m ; ++i)
00170                 vector_.push_back(newElement(list[i], i));
00171             build();
00172         }
00173 
00175         void rebuild()
00176         {
00177             build();
00178         }
00179 
00181         void update(Element *element)
00182         {
00183             const unsigned int pos = element->position;
00184             assert(vector_[pos] == element);
00185             percolateUp(pos);
00186             percolateDown(pos);
00187         }
00188 
00190         bool empty() const
00191         {
00192             return vector_.empty();
00193         }
00194 
00196         unsigned int size() const
00197         {
00198             return vector_.size();
00199         }
00200 
00202         void getContent(std::vector<_T> &content) const
00203         {
00204             for (typename std::vector<Element*>::const_iterator i = vector_.begin();
00205                  i != vector_.end() ; ++i)
00206                 content.push_back((*i)->data);
00207         }
00208 
00210         void sort(std::vector<_T>& list)
00211         {
00212             const unsigned int n         = list.size();
00213             std::vector<Element*> backup = vector_;
00214             vector_.clear();
00215             for (unsigned int i = 0 ; i < n ; ++i)
00216                 vector_.push_back(newElement(list[i], i));
00217             build();
00218             list.clear();
00219             list.reserve(n);
00220 
00221             for (unsigned int i = 0 ; i < n ; ++i)
00222             {
00223                 list.push_back(vector_[0]->data);
00224                 removePos(0);
00225             }
00226             vector_ = backup;
00227         }
00228 
00230         LessThan& getComparisonOperator()
00231         {
00232             return lt_;
00233         }
00234 
00235     private:
00236 
00237         LessThan                 lt_;
00238 
00239         std::vector<Element*>    vector_;
00240 
00241         EventAfterInsert         eventAfterInsert_;
00242         void                    *eventAfterInsertData_;
00243         EventBeforeRemove        eventBeforeRemove_;
00244         void                    *eventBeforeRemoveData_;
00245 
00246         void removePos(unsigned int pos)
00247         {
00248             const int n = vector_.size() - 1;
00249             delete vector_[pos];
00250             if ((int)pos < n)
00251             {
00252                 vector_[pos] = vector_.back();
00253                 vector_[pos]->position = pos;
00254                 vector_.pop_back();
00255                 percolateDown(pos);
00256             }
00257             else
00258                 vector_.pop_back();
00259         }
00260 
00261         Element* newElement(_T& data, unsigned int pos) const
00262         {
00263             Element *element = new Element();
00264             element->data = data;
00265             element->position = pos;
00266             return element;
00267         }
00268 
00269         void build()
00270         {
00271             for(int i = vector_.size() / 2 - 1; i >= 0; --i)
00272                 percolateDown(i);
00273         }
00274 
00275         void percolateDown(const unsigned int pos)
00276         {
00277             const unsigned int n      = vector_.size();
00278             Element           *tmp    = vector_[pos];
00279             unsigned int       parent = pos;
00280             unsigned int       child  = (pos + 1) << 1;
00281 
00282             while (child < n)
00283             {
00284                 if (lt_(vector_[child - 1]->data, vector_[child]->data)) --child;
00285                 if (lt_(vector_[child]->data,  tmp->data))
00286                 {
00287                     vector_[parent] = vector_[child];
00288                     vector_[parent]->position = parent;
00289                 }
00290                 else
00291                     break;
00292                 parent = child;
00293                 child  = (child + 1) << 1;
00294             }
00295             if (child == n)
00296             {
00297                 --child;
00298                 if (lt_(vector_[child]->data, tmp->data))
00299                 {
00300                     vector_[parent] = vector_[child];
00301                     vector_[parent]->position = parent;
00302                     parent = child;
00303                 }
00304             }
00305             if (parent != pos)
00306             {
00307                 vector_[parent] = tmp;
00308                 vector_[parent]->position = parent;
00309             }
00310         }
00311 
00312         void percolateUp(const unsigned int pos)
00313         {
00314             Element      *tmp    = vector_[pos];
00315             unsigned int  child  = pos;
00316             unsigned int  parent = (pos - 1) >> 1;
00317 
00318             while (child > 0 && lt_(tmp->data, vector_[parent]->data))
00319             {
00320                 vector_[child] = vector_[parent];
00321                 vector_[child]->position = child;
00322                 child  = parent;
00323                 parent = (parent - 1) >> 1;
00324             }
00325             if (child != pos)
00326             {
00327                 vector_[child] = tmp;
00328                 vector_[child]->position = child;
00329             }
00330         }
00331     };
00332 
00333 }
00334 
00335 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines