00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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(void) { }
00066 ~Element(void) { }
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(void)
00081 {
00082 eventAfterInsert_ = NULL;
00083 eventBeforeRemove_ = NULL;
00084 }
00085
00086 ~BinaryHeap(void)
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(void)
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(void) const
00116 {
00117 return vector_.empty() ? NULL : vector_.at(0);
00118 }
00119
00121 void pop(void)
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(void)
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(void) const
00191 {
00192 return vector_.empty();
00193 }
00194
00196 unsigned int size(void) 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
00229 private:
00230
00231 LessThan lt_;
00232
00233 std::vector<Element*> vector_;
00234
00235 EventAfterInsert eventAfterInsert_;
00236 void *eventAfterInsertData_;
00237 EventBeforeRemove eventBeforeRemove_;
00238 void *eventBeforeRemoveData_;
00239
00240 void removePos(unsigned int pos)
00241 {
00242 const int n = vector_.size() - 1;
00243 delete vector_[pos];
00244 if ((int)pos < n)
00245 {
00246 vector_[pos] = vector_.back();
00247 vector_[pos]->position = pos;
00248 vector_.pop_back();
00249 percolateDown(pos);
00250 }
00251 else
00252 vector_.pop_back();
00253 }
00254
00255 Element* newElement(_T& data, unsigned int pos) const
00256 {
00257 Element* element = new Element();
00258 element->data = data;
00259 element->position = pos;
00260 return element;
00261 }
00262
00263 void build(void)
00264 {
00265 for(int i = vector_.size() / 2 - 1; i >= 0; --i)
00266 percolateDown(i);
00267 }
00268
00269 void percolateDown(const unsigned int pos)
00270 {
00271 const unsigned int n = vector_.size();
00272 Element* tmp = vector_[pos];
00273 unsigned int parent = pos;
00274 unsigned int child = (pos + 1) << 1;
00275
00276 while (child < n)
00277 {
00278 if (lt_(vector_[child - 1]->data, vector_[child]->data)) --child;
00279 if (lt_(vector_[child]->data, tmp->data))
00280 {
00281 vector_[parent] = vector_[child];
00282 vector_[parent]->position = parent;
00283 }
00284 else
00285 break;
00286 parent = child;
00287 child = (child + 1) << 1;
00288 }
00289 if (child == n)
00290 {
00291 --child;
00292 if (lt_(vector_[child]->data, tmp->data))
00293 {
00294 vector_[parent] = vector_[child];
00295 vector_[parent]->position = parent;
00296 parent = child;
00297 }
00298 }
00299 if (parent != pos)
00300 {
00301 vector_[parent] = tmp;
00302 vector_[parent]->position = parent;
00303 }
00304 }
00305
00306 void percolateUp(const unsigned int pos)
00307 {
00308 Element* tmp = vector_[pos];
00309 unsigned int child = pos;
00310 unsigned int parent = (pos - 1) >> 1;
00311
00312 while (child > 0 && lt_(tmp->data, vector_[parent]->data))
00313 {
00314 vector_[child] = vector_[parent];
00315 vector_[child]->position = child;
00316 child = parent;
00317 parent = (parent - 1) >> 1;
00318 }
00319 if (child != pos)
00320 {
00321 vector_[child] = tmp;
00322 vector_[child]->position = child;
00323 }
00324 }
00325 };
00326
00327 }
00328
00329 #endif