source: lost-perception/astar/BinaryHeap.java

package astar;

import java.util.ArrayList;

public class BinaryHeap
{
    public ArrayList<AStarNode> arr;
    
    public BinaryHeap() {
        this.arr = new ArrayList<AStarNode>();
    }
    
    private int index(final int i) {
        return i - 1;
    }
    
    public boolean isEmpty() {
        return this.arr.size() == 0;
    }
    
    public void add(final AStarNode c) {
        this.arr.add(c);
        int pos = this.arr.size();
        c.openListPos = pos;
        while (this.index(pos) > 0 && this.arr.get(this.index(pos)).compareTo((AStarNode)this.arr.get(this.index(pos / 2))) < 0) {
            final AStarNode temp = this.arr.get(this.index(pos / 2));
            this.arr.set(this.index(pos / 2), this.arr.get(this.index(pos)));
            this.arr.set(this.index(pos), temp);
            this.arr.get(this.index(pos)).openListPos = pos;
            pos /= 2;
        }
        this.arr.get(this.index(pos)).openListPos = pos;
    }
    
    public AStarNode removeSmallest() {
        final AStarNode smallest = this.arr.get(0);
        this.arr.set(0, this.arr.get(this.arr.size() - 1));
        this.arr.get(0).openListPos = 1;
        this.arr.remove(this.arr.size() - 1);
        int pos = 1;
        boolean done = false;
        while (this.index(pos) < this.arr.size() && !done) {
            int smallestIndex = -1;
            if (this.index(pos * 2) < this.arr.size() && this.arr.get(this.index(pos * 2)).compareTo((AStarNode)this.arr.get(this.index(pos))) < 0) {
                smallestIndex = pos * 2;
            }
            if (this.index(pos * 2 + 1) < this.arr.size() && this.arr.get(this.index(pos * 2 + 1)).compareTo((AStarNode)this.arr.get(this.index(pos))) < 0 && (smallestIndex == -1 || this.arr.get(this.index(pos * 2 + 1)).compareTo((AStarNode)this.arr.get(this.index(pos * 2))) < 0)) {
                smallestIndex = pos * 2 + 1;
            }
            if (smallestIndex == -1) {
                done = true;
            }
            else {
                final AStarNode temp = this.arr.get(this.index(pos));
                this.arr.set(this.index(pos), this.arr.get(this.index(smallestIndex)));
                this.arr.set(this.index(smallestIndex), temp);
                this.arr.get(this.index(pos)).openListPos = pos;
                this.arr.get(this.index(smallestIndex)).openListPos = smallestIndex;
                pos = smallestIndex;
            }
        }
        return smallest;
    }
    
    public void update(final AStarNode o) {
        int pos = o.openListPos;
        for (int parent = pos / 2; parent != 0 && this.arr.get(this.index(pos)).compareTo((AStarNode)this.arr.get(this.index(parent))) < 0; pos = parent, parent /= 2) {
            final AStarNode temp = this.arr.get(this.index(parent));
            this.arr.set(this.index(parent), this.arr.get(this.index(pos)));
            this.arr.set(this.index(pos), temp);
            this.arr.get(this.index(pos)).openListPos = pos;
        }
        this.arr.get(this.index(pos)).openListPos = pos;
    }
}