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