.. _program_listing_file_src_ompl_datastructures_BinaryHeap.h:

Program Listing for File BinaryHeap.h
=====================================

|exhale_lsh| :ref:`Return to documentation for file <file_src_ompl_datastructures_BinaryHeap.h>` (``src/ompl/datastructures/BinaryHeap.h``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   /*********************************************************************
   * Software License Agreement (BSD License)
   *
   *  Copyright (c) 2008, Willow Garage, Inc.
   *  All rights reserved.
   *
   *  Redistribution and use in source and binary forms, with or without
   *  modification, are permitted provided that the following conditions
   *  are met:
   *
   *   * Redistributions of source code must retain the above copyright
   *     notice, this list of conditions and the following disclaimer.
   *   * Redistributions in binary form must reproduce the above
   *     copyright notice, this list of conditions and the following
   *     disclaimer in the documentation and/or other materials provided
   *     with the distribution.
   *   * Neither the name of the Willow Garage nor the names of its
   *     contributors may be used to endorse or promote products derived
   *     from this software without specific prior written permission.
   *
   *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
   *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
   *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
   *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
   *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
   *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
   *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
   *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
   *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
   *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
   *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
   *  POSSIBILITY OF SUCH DAMAGE.
   *********************************************************************/
   
   /* Author: Ioan Sucan */
   
   #ifndef OMPL_DATASTRUCTURES_BINARY_HEAP_
   #define OMPL_DATASTRUCTURES_BINARY_HEAP_
   
   #include <functional>
   #include <utility>
   #include <vector>
   #include <cassert>
   
   namespace ompl
   {
       template <typename _T, class LessThan = std::less<_T>>
       class BinaryHeap
       {
       public:
           class Element
           {
               friend class BinaryHeap;
   
           private:
               Element() = default;
               ~Element() = default;
               unsigned int position;
   
           public:
               _T data;
           };
   
           using EventAfterInsert = void (*)(Element *, void *);
   
           using EventBeforeRemove = void (*)(Element *, void *);
   
           BinaryHeap()
           {
               eventAfterInsert_ = nullptr;
               eventBeforeRemove_ = nullptr;
           }
   
           BinaryHeap(LessThan lt) : lt_(std::move(lt))
           {
               eventAfterInsert_ = nullptr;
               eventBeforeRemove_ = nullptr;
           }
   
           ~BinaryHeap()
           {
               clear();
           }
   
           void onAfterInsert(EventAfterInsert event, void *arg)
           {
               eventAfterInsert_ = event;
               eventAfterInsertData_ = arg;
           }
   
           void onBeforeRemove(EventBeforeRemove event, void *arg)
           {
               eventBeforeRemove_ = event;
               eventBeforeRemoveData_ = arg;
           }
   
           void clear()
           {
               for (auto &element : vector_)
                   delete element;
               vector_.clear();
           }
   
           Element *top() const
           {
               return vector_.empty() ? nullptr : vector_.at(0);
           }
   
           void pop()
           {
               removePos(0);
           }
   
           void remove(Element *element)
           {
               if (eventBeforeRemove_)
                   eventBeforeRemove_(element, eventBeforeRemoveData_);
               removePos(element->position);
           }
   
           Element *insert(const _T &data)
           {
               auto *element = new Element();
               element->data = data;
               const unsigned int pos = vector_.size();
               element->position = pos;
               vector_.push_back(element);
               percolateUp(pos);
               if (eventAfterInsert_)
                   eventAfterInsert_(element, eventAfterInsertData_);
               return element;
           }
   
           void insert(const std::vector<_T> &list)
           {
               const unsigned int n = vector_.size();
               const unsigned int m = list.size();
               for (unsigned int i = 0; i < m; ++i)
               {
                   const unsigned int pos = i + n;
                   Element *element = newElement(list[i], pos);
                   vector_.push_back(element);
                   percolateUp(pos);
                   if (eventAfterInsert_)
                       eventAfterInsert_(element, eventAfterInsertData_);
               }
           }
   
           void buildFrom(const std::vector<_T> &list)
           {
               clear();
               const unsigned int m = list.size();
               for (unsigned int i = 0; i < m; ++i)
                   vector_.push_back(newElement(list[i], i));
               build();
           }
   
           void rebuild()
           {
               build();
           }
   
           void update(Element *element)
           {
               const unsigned int pos = element->position;
               assert(vector_[pos] == element);
               percolateUp(pos);
               percolateDown(pos);
           }
   
           bool empty() const
           {
               return vector_.empty();
           }
   
           unsigned int size() const
           {
               return vector_.size();
           }
   
           void getContent(std::vector<_T> &content) const
           {
               for (auto &element : vector_)
                   content.push_back(element->data);
           }
   
           void sort(std::vector<_T> &list)
           {
               const unsigned int n = list.size();
               std::vector<Element *> backup = vector_;
               vector_.clear();
               for (unsigned int i = 0; i < n; ++i)
                   vector_.push_back(newElement(list[i], i));
               build();
               list.clear();
               list.reserve(n);
   
               for (unsigned int i = 0; i < n; ++i)
               {
                   list.push_back(vector_[0]->data);
                   removePos(0);
               }
               vector_ = backup;
           }
   
           LessThan &getComparisonOperator()
           {
               return lt_;
           }
   
       private:
           LessThan lt_;
   
           std::vector<Element *> vector_;
   
           EventAfterInsert eventAfterInsert_;
           void *eventAfterInsertData_;
           EventBeforeRemove eventBeforeRemove_;
           void *eventBeforeRemoveData_;
   
           void removePos(unsigned int pos)
           {
               const int n = vector_.size() - 1;
               delete vector_[pos];
               if ((int)pos < n)
               {
                   vector_[pos] = vector_.back();
                   vector_[pos]->position = pos;
                   vector_.pop_back();
                   percolateDown(pos);
               }
               else
                   vector_.pop_back();
           }
   
           Element *newElement(const _T &data, unsigned int pos) const
           {
               auto *element = new Element();
               element->data = data;
               element->position = pos;
               return element;
           }
   
           void build()
           {
               for (int i = vector_.size() / 2 - 1; i >= 0; --i)
                   percolateDown(i);
           }
   
           void percolateDown(const unsigned int pos)
           {
               const unsigned int n = vector_.size();
               Element *tmp = vector_[pos];
               unsigned int parent = pos;
               unsigned int child = (pos + 1) << 1;
   
               while (child < n)
               {
                   if (lt_(vector_[child - 1]->data, vector_[child]->data))
                       --child;
                   if (lt_(vector_[child]->data, tmp->data))
                   {
                       vector_[parent] = vector_[child];
                       vector_[parent]->position = parent;
                   }
                   else
                       break;
                   parent = child;
                   child = (child + 1) << 1;
               }
               if (child == n)
               {
                   --child;
                   if (lt_(vector_[child]->data, tmp->data))
                   {
                       vector_[parent] = vector_[child];
                       vector_[parent]->position = parent;
                       parent = child;
                   }
               }
               if (parent != pos)
               {
                   vector_[parent] = tmp;
                   vector_[parent]->position = parent;
               }
           }
   
           void percolateUp(const unsigned int pos)
           {
               Element *tmp = vector_[pos];
               unsigned int child = pos;
               unsigned int parent = (pos - 1) >> 1;
   
               while (child > 0 && lt_(tmp->data, vector_[parent]->data))
               {
                   vector_[child] = vector_[parent];
                   vector_[child]->position = child;
                   child = parent;
                   parent = (parent - 1) >> 1;
               }
               if (child != pos)
               {
                   vector_[child] = tmp;
                   vector_[child]->position = child;
               }
           }
       };
   }
   
   #endif