Simple fixed size binary heap implementation, indented for A* applications.

binheap.h

// Simple fixed size binary heap implementation, intended for A* applications.
// Aidan Dodds 15/2/2016.
// Do whatever you want with this...

#pragma once
#include <stdint.h>
#include <array>
#include <cassert>

// Fixed size binary heap implementation
template <typename type_t, 
          size_t c_size,
          bool compare(const type_t &a, const type_t &b)>
struct bin_heap_t {

    // Note:
    //  To simplify the implementation, heap_ is base 1 (index 0 unused).
    
    bin_heap_t() : index_(1) {}

    // pop the current best node in the heap (root node)
    type_t pop()
    {
        assert(!empty());
        // save top most value for final return
        type_t out = heap_[1];
        // swap last and first and shrink by one
        index_ -= 1;
        std::swap(heap_[1], heap_[index_]);
        // push down to correct position
        bubble_down(1);
        return out;
    }

    // push a new node into the heap and rebalance tree
    void push(type_t node)
    {
        assert(!full());
        // insert node into end of list
        size_t i = index_++;
        heap_[i] = node;
        bubble_up(i);
    }

    // return true if the heap is empty
    bool empty() const
    {
        return index_ <= 1;
    }

    // return true if the heap is full
    bool full() const
    {
        return index_ > c_size;
    }

    // number of nodes currently in the heap
    size_t size() const
    {
        return index_-1;
    }

    // wipe the heap back to its empty state
    void clear()
    {
        index_ = 1;
    }

    // test that we have a valid binary heap
    void validate() const
    {
        for (size_t i = 2; i<index_; ++i)
            assert(compare(heap_[i/2], heap_[i]));
    }

protected:

    std::array<type_t, c_size+1> heap_;
    size_t index_;

    // check an index points to a valid node
    bool valid(size_t i) const
    {
        return i<index_;
    }

    // bubble an item down to its correct place in the tree
    inline void bubble_down(size_t i)
    {
        // while we are not at a leaf
        while (valid(child(i, 0))) {
            // get both children
            const size_t x = child(i, 0);
            const size_t y = child(i, 1);
            // select best child
            const bool select = valid(y) && compare(heap_[y], heap_[x]);
            type_t & best = select ? heap_[y] : heap_[x];
            // quit if children are not better
            if (!compare(best, heap_[i]))
                break;
            // swap current and child
            std::swap(best, heap_[i]);
            // repeat from child node
            i = select ? y : x;
        }
    }

    // bubble and item up to its correct place in the tree
    inline void bubble_up(size_t i)
    {
        // while not at the root node
        while (i>1) {
            const size_t j = parent(i);
            // if node is better then parent
            if (!compare(heap_[i], heap_[j]))
                break;
            std::swap(heap_[i], heap_[j]);
            i = j;
        }
    }

    // given an index, return the parent index
    static inline size_t parent(size_t index)
    {
        return index/2;
    }

    // given an index, return one of the two child nodes (branch 0/1)
    static inline size_t child(size_t index, uint32_t branch)
    {
        assert(!(branch&~1u));
        return index*2+branch;
    }
};

test.cpp

// This is just a simple fuzz tester for the binary heap.

#include "binheap.h"

bool compare(const uint64_t & lhs, const uint64_t & rhs) 
{
    return lhs<rhs;
}

uint64_t rand64(uint64_t & x)
{
    x ^= x>>12;
    x ^= x<<25;
    x ^= x>>27;
    return x * uint64_t(2685821657736338717);
}

int main(int argc, char ** args)
{
    bin_heap_t<uint64_t, 32*32, compare> bh;

    uint64_t s1 = 0x1234;
    uint64_t s2 = 0x2345;

    for (size_t i = 0; i<0x12345678; ++i) {

        bh.validate();

        if (rand64(s1)&0x100000) {
            if (!bh.full()) {
                uint64_t v2 = rand64(s2);
                bh.push(v2 & 0xffff);
            }
        }
        else {
            if (!bh.empty()) {
                bh.pop();
            }
        }
    }

    return 0;
}