37 #ifndef OMPL_DATASTRUCTURES_BINARY_HEAP_
38 #define OMPL_DATASTRUCTURES_BINARY_HEAP_
51 template <
typename _T,
class LessThan = std::less<_T>>
61 friend class BinaryHeap;
67 unsigned int position;
82 eventAfterInsert_ =
nullptr;
83 eventBeforeRemove_ =
nullptr;
88 eventAfterInsert_ =
nullptr;
89 eventBeforeRemove_ =
nullptr;
100 eventAfterInsert_ = event;
101 eventAfterInsertData_ = arg;
107 eventBeforeRemove_ = event;
108 eventBeforeRemoveData_ = arg;
114 for (
auto &element : vector_)
122 return vector_.empty() ? nullptr : vector_.at(0);
132 void remove(Element *element)
134 if (eventBeforeRemove_)
135 eventBeforeRemove_(element, eventBeforeRemoveData_);
136 removePos(element->position);
140 Element *
insert(
const _T &data)
142 auto *element =
new Element();
143 element->data = data;
144 const unsigned int pos = vector_.size();
145 element->position = pos;
146 vector_.push_back(element);
148 if (eventAfterInsert_)
149 eventAfterInsert_(element, eventAfterInsertData_);
154 void insert(
const std::vector<_T> &list)
156 const unsigned int n = vector_.size();
157 const unsigned int m = list.size();
158 for (
unsigned int i = 0; i < m; ++i)
160 const unsigned int pos = i + n;
161 Element *element = newElement(list[i], pos);
162 vector_.push_back(element);
164 if (eventAfterInsert_)
165 eventAfterInsert_(element, eventAfterInsertData_);
170 void buildFrom(
const std::vector<_T> &list)
173 const unsigned int m = list.size();
174 for (
unsigned int i = 0; i < m; ++i)
175 vector_.push_back(newElement(list[i], i));
186 void update(Element *element)
188 const unsigned int pos = element->position;
189 assert(vector_[pos] == element);
197 return vector_.empty();
201 unsigned int size()
const
203 return vector_.size();
207 void getContent(std::vector<_T> &content)
const
209 for (
auto &element : vector_)
210 content.push_back(element->data);
214 void sort(std::vector<_T> &list)
216 const unsigned int n = list.size();
217 std::vector<Element *> backup = vector_;
219 for (
unsigned int i = 0; i < n; ++i)
220 vector_.push_back(newElement(list[i], i));
225 for (
unsigned int i = 0; i < n; ++i)
227 list.push_back(vector_[0]->data);
242 std::vector<Element *> vector_;
245 void *eventAfterInsertData_;
247 void *eventBeforeRemoveData_;
249 void removePos(
unsigned int pos)
251 const int n = vector_.size() - 1;
255 vector_[pos] = vector_.back();
256 vector_[pos]->position = pos;
264 Element *newElement(
const _T &data,
unsigned int pos)
const
266 auto *element =
new Element();
267 element->data = data;
268 element->position = pos;
274 for (
int i = vector_.size() / 2 - 1; i >= 0; --i)
278 void percolateDown(
const unsigned int pos)
280 const unsigned int n = vector_.size();
281 Element *tmp = vector_[pos];
282 unsigned int parent = pos;
283 unsigned int child = (pos + 1) << 1;
287 if (lt_(vector_[child - 1]->data, vector_[child]->data))
289 if (lt_(vector_[child]->data, tmp->data))
291 vector_[parent] = vector_[child];
292 vector_[parent]->position = parent;
297 child = (child + 1) << 1;
302 if (lt_(vector_[child]->data, tmp->data))
304 vector_[parent] = vector_[child];
305 vector_[parent]->position = parent;
311 vector_[parent] = tmp;
312 vector_[parent]->position = parent;
316 void percolateUp(
const unsigned int pos)
318 Element *tmp = vector_[pos];
319 unsigned int child = pos;
320 unsigned int parent = (pos - 1) >> 1;
322 while (child > 0 && lt_(tmp->data, vector_[parent]->data))
324 vector_[child] = vector_[parent];
325 vector_[child]->position = child;
327 parent = (parent - 1) >> 1;
331 vector_[child] = tmp;
332 vector_[child]->position = child;