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