37 #ifndef OMPL_DATASTRUCTURES_BINARY_HEAP_
38 #define OMPL_DATASTRUCTURES_BINARY_HEAP_
51 template <
typename _T,
52 class LessThan = std::less<_T> >
68 unsigned int position;
82 eventAfterInsert_ = NULL;
83 eventBeforeRemove_ = NULL;
94 eventAfterInsert_ = event;
95 eventAfterInsertData_ = arg;
101 eventBeforeRemove_ = event;
102 eventBeforeRemoveData_ = arg;
108 for (
typename std::vector<Element*>::iterator i = vector_.begin() ;
109 i != vector_.end() ; ++i)
117 return vector_.empty() ? NULL : vector_.at(0);
127 void remove(Element* element)
129 if (eventBeforeRemove_)
130 eventBeforeRemove_(element, eventBeforeRemoveData_);
131 removePos(element->position);
137 Element* element =
new Element();
138 element->data = data;
139 const unsigned int pos = vector_.size();
140 element->position = pos;
141 vector_.push_back(element);
143 if (eventAfterInsert_)
144 eventAfterInsert_(element, eventAfterInsertData_);
151 const unsigned int n = vector_.size();
152 const unsigned int m = list.size();
153 for (
unsigned int i = 0 ; i < m ; ++i)
155 const unsigned int pos = i + n;
156 Element* element = newElement(list[i], pos);
157 vector_.push_back(element);
159 if (eventAfterInsert_)
160 eventAfterInsert_(element, eventAfterInsertData_);
168 const unsigned int m = list.size();
169 for (
unsigned int i = 0 ; i < m ; ++i)
170 vector_.push_back(newElement(list[i], i));
183 const unsigned int pos = element->position;
184 assert(vector_[pos] == element);
192 return vector_.empty();
198 return vector_.size();
204 for (
typename std::vector<Element*>::const_iterator i = vector_.begin();
205 i != vector_.end() ; ++i)
206 content.push_back((*i)->data);
210 void sort(std::vector<_T>& list)
212 const unsigned int n = list.size();
213 std::vector<Element*> backup = vector_;
215 for (
unsigned int i = 0 ; i < n ; ++i)
216 vector_.push_back(newElement(list[i], i));
221 for (
unsigned int i = 0 ; i < n ; ++i)
223 list.push_back(vector_[0]->data);
233 std::vector<Element*> vector_;
236 void *eventAfterInsertData_;
238 void *eventBeforeRemoveData_;
240 void removePos(
unsigned int pos)
242 const int n = vector_.size() - 1;
246 vector_[pos] = vector_.back();
247 vector_[pos]->position = pos;
255 Element* newElement(_T& data,
unsigned int pos)
const
257 Element* element =
new Element();
258 element->data = data;
259 element->position = pos;
265 for(
int i = vector_.size() / 2 - 1; i >= 0; --i)
269 void percolateDown(
const unsigned int pos)
271 const unsigned int n = vector_.size();
272 Element* tmp = vector_[pos];
273 unsigned int parent = pos;
274 unsigned int child = (pos + 1) << 1;
278 if (lt_(vector_[child - 1]->data, vector_[child]->data)) --child;
279 if (lt_(vector_[child]->data, tmp->data))
281 vector_[parent] = vector_[child];
282 vector_[parent]->position = parent;
287 child = (child + 1) << 1;
292 if (lt_(vector_[child]->data, tmp->data))
294 vector_[parent] = vector_[child];
295 vector_[parent]->position = parent;
301 vector_[parent] = tmp;
302 vector_[parent]->position = parent;
306 void percolateUp(
const unsigned int pos)
308 Element* tmp = vector_[pos];
309 unsigned int child = pos;
310 unsigned int parent = (pos - 1) >> 1;
312 while (child > 0 && lt_(tmp->data, vector_[parent]->data))
314 vector_[child] = vector_[parent];
315 vector_[child]->position = child;
317 parent = (parent - 1) >> 1;
321 vector_[child] = tmp;
322 vector_[child]->position = child;