dynamic_priority_queue.h

#pragma once
 
#include <vector>
 
namespace cgv {
        namespace data {
template <typename T>
class dynamic_priority_queue
{
public:
        typedef T element_type;
protected:
        struct extended_element
        {
                element_type element;
                bool is_empty : 1;
                unsigned int link : 31;
        };
        std::vector<extended_element> elements;
        int last_empty_element;
        std::vector<unsigned int> heap;
        bool valid(unsigned int hp) const                  { return hp < heap.size(); }
        void updateLink(unsigned int hp)                   { elements[heap[hp]].link = hp; }
        bool isBetter(unsigned int hp1, unsigned int hp2) const { 
                return elements[heap[hp1]].element<elements[heap[hp2]].element; 
        }
        bool exchangeIfBetter(unsigned int hp1, unsigned int hp2)
        {
                if (isBetter(hp1,hp2)) {
                        std::swap(heap[hp1], heap[hp2]);
                        updateLink(hp1);
                        updateLink(hp2);
                        return true;
                }
                return false;
        }
        void heapify(unsigned int hp)           { if (!positionUpward(hp)) positionDownward(hp); }
        void move(unsigned int hp1, unsigned int hp2)
        {
                heap[hp2] = heap[hp1];
                updateLink(hp2);
        }
        void removeHeap(unsigned int hp)
        {
                if (hp != heap.size()-1) {
                        move((unsigned int)heap.size()-1,hp);
                        heap.pop_back();
                        heapify(hp);
                }
                else 
                        heap.pop_back();
        }
        void positionDownward(unsigned int& hp)
        {
                unsigned int child;
                while (valid(child = 2*hp+1)) {
                        unsigned int tmp = child+1;
                        if (valid(tmp) && isBetter(tmp,child)) child = tmp;
                        if (!exchangeIfBetter(child, hp)) break;
                        hp = child;
                }
        }
        bool positionUpward(unsigned int& hp)
        {
                bool changed = false;
                while (hp != 0) {
                        unsigned int pa = (hp-1)/2;
                        if (!exchangeIfBetter(hp, pa)) break;
                        hp = pa;
                        changed = true;
                }
                return changed;
        }
public:
        dynamic_priority_queue() : last_empty_element(-1) {}
        void clear()        { elements.clear(); heap.clear(); last_empty_element = -1; }
        bool empty() const { return heap.size() == 0; }
        size_t size() const { return heap.size(); }
        size_t size_of_element_container() const { return elements.size(); }
 
        unsigned int top() const { return heap[0]; }
        void pop() { remove(top()); }
        int extract()
        {
                unsigned int tmp = top();
                pop();
                return tmp;
        }
 
        bool is_empty(unsigned int idx) const { return elements[idx].is_empty; }
        const element_type& operator [] (unsigned int idx) const { return elements[idx].element; }
        element_type& operator [] (unsigned int idx) { return elements[idx].element; }
        unsigned int insert(const element_type& e)
        {
                unsigned int idx;
                if (last_empty_element != -1) {
                        idx = last_empty_element;
                        if (elements[idx].link == idx)
                                last_empty_element = -1;
                        else
                                last_empty_element = elements[idx].link;
                }
                else {
                        idx = (unsigned int) elements.size();
                        elements.resize(idx+1);
                }
                elements[idx].element  = e;
                elements[idx].is_empty = false;
 
                unsigned int hp = (unsigned int) heap.size();
                heap.push_back(idx);
                updateLink(hp);
                positionUpward(hp);
 
                return idx;
        }
        void update(unsigned int idx) 
        {
                if (!is_empty(idx)) 
                        heapify(elements[idx].link); 
        }
        void remove(unsigned int idx) 
        {
                if (is_empty(idx))
                        return;
                removeHeap(elements[idx].link);
                elements[idx].is_empty = true;
                if (last_empty_element == -1)
                        elements[idx].link = idx;
                else
                        elements[idx].link = last_empty_element;
                last_empty_element = idx;
        }
};
 
        }
}