friendship ended with linked list. now binary heap is my best friend.

.gitignore

@@ -12,3 +12,4 @@ classes/
 # IntelliJ Files
 .idea/
 *.iml
+/logs/

src/main/java/baritone/bot/pathing/calc/AStarPathFinder.java

@@ -1,7 +1,8 @@
 package baritone.bot.pathing.calc;
 
 
-import baritone.Baritone;
+//import baritone.Baritone;
+
 import baritone.bot.pathing.goals.Goal;
 import baritone.bot.pathing.movement.ActionCosts;
 import baritone.bot.pathing.movement.Movement;
@@ -11,8 +12,6 @@ import net.minecraft.util.math.BlockPos;
 import net.minecraft.world.chunk.EmptyChunk;
 
 import java.util.Random;
-import java.util.logging.Level;
-import java.util.logging.Logger;
 
 /**
  * The actual A* pathfinding
@@ -28,7 +27,8 @@ public class AStarPathFinder extends AbstractNodeCostSearch {
     protected IPath calculate0() {
         startNode = getNodeAtPosition(start);
         startNode.cost = 0;
-        IOpenSet openSet = new LinkedListOpenSet();
+        startNode.combinedCost = startNode.estimatedCostToGoal;
+        IOpenSet openSet = new BinaryHeapOpenSet();
         startNode.isOpen = true;
         openSet.insert(startNode);
         bestSoFar = new PathNode[COEFFICIENTS.length];//keep track of the best node by the metric of (estimatedCostToGoal + cost / COEFFICIENTS[i])
@@ -38,19 +38,19 @@ public class AStarPathFinder extends AbstractNodeCostSearch {
         }
         currentlyRunning = this;
         long startTime = System.currentTimeMillis();
-        long timeoutTime = startTime + (Baritone.slowPath ? 40000 : 4000);
+        long timeoutTime = startTime + /*(Baritone.slowPath ? 40000 : */4000/*)*/;
         long lastPrintout = 0;
         int numNodes = 0;
         ToolSet ts = new ToolSet();
         int numEmptyChunk = 0;
         while (!openSet.isEmpty() && numEmptyChunk < 50 && System.currentTimeMillis() < timeoutTime) {
-            if (Baritone.slowPath) {
+            /*if (Baritone.slowPath) {
                 try {
                     Thread.sleep(100);
                 } catch (InterruptedException ex) {
                     Logger.getLogger(AStarPathFinder.class.getName()).log(Level.SEVERE, null, ex);
                 }
-            }
+            }*/
             PathNode currentNode = openSet.removeLowest();
             mostRecentConsidered = currentNode;
             currentNode.isOpen = false;
@@ -89,6 +89,7 @@ public class AStarPathFinder extends AbstractNodeCostSearch {
                     neighbor.previous = currentNode;
                     neighbor.previousMovement = movementToGetToNeighbor;
                     neighbor.cost = tentativeCost;
+                    neighbor.combinedCost = tentativeCost + neighbor.estimatedCostToGoal;
                     if (!neighbor.isOpen) {
                         openSet.insert(neighbor);//dont double count, dont insert into open set if it's already there
                         neighbor.isOpen = true;

src/main/java/baritone/bot/pathing/calc/BinaryHeapOpenSet.java

@@ -0,0 +1,75 @@
+package baritone.bot.pathing.calc;
+
+import java.util.Arrays;
+
+public class BinaryHeapOpenSet implements IOpenSet {
+    private static final int INITIAL_CAPACITY = 1024;
+    private PathNode[] array;
+    private int size;
+
+    public BinaryHeapOpenSet() {
+        this(INITIAL_CAPACITY);
+    }
+
+    public BinaryHeapOpenSet(int size) {
+        this.size = 0;
+        this.array = new PathNode[size];
+    }
+
+    public void insert(PathNode value) {
+        if (size >= array.length - 1) {
+            array = Arrays.copyOf(array, array.length * 2);
+        }
+        size++;
+        int index = size;
+        array[index] = value;
+        int parent = index >>> 1;
+        while (index > 1 && array[parent].combinedCost > array[index].combinedCost) {
+            swap(index, parent);
+            index = parent;
+            parent = index >>> 1;
+        }
+    }
+
+    /**
+     * Returns true if the heap has no elements; false otherwise.
+     */
+    public boolean isEmpty() {
+        return size == 0;
+    }
+
+    /**
+     * Removes and returns the minimum element in the heap.
+     */
+    public PathNode removeLowest() {
+        if (size == 0) {
+            throw new IllegalStateException();
+        }
+        PathNode result = array[1];
+        array[1] = array[size];
+        array[size] = null;
+        size--;
+        int index = 1;
+        int smallerChild = 2;
+        while (smallerChild <= size) {
+            int right = smallerChild + 1;
+            if (right <= size && array[smallerChild].combinedCost > array[right].combinedCost) {
+                smallerChild = right;
+            }
+            if (array[index].combinedCost > array[smallerChild].combinedCost) {
+                swap(index, smallerChild);
+            } else {
+                break;
+            }
+            index = smallerChild;
+            smallerChild = index << 1;
+        }
+        return result;
+    }
+
+    protected void swap(int index1, int index2) {
+        PathNode tmp = array[index1];
+        array[index1] = array[index2];
+        array[index2] = tmp;
+    }
+}

src/main/java/baritone/bot/pathing/calc/FibonacciHeapOpenSet.java

@@ -9,7 +9,7 @@ public class FibonacciHeapOpenSet extends FibonacciHeap implements IOpenSet {
     //isEmpty is already defined in FibonacciHeap
     @Override
     public void insert(PathNode node) {
-        super.insert(node, node.estimatedCostToGoal + node.cost);
+        super.insert(node, node.combinedCost);
     }
 
     @Override

src/main/java/baritone/bot/pathing/calc/LinkedListOpenSet.java

@@ -1,7 +1,8 @@
 package baritone.bot.pathing.calc;
 
 /**
- *
+ * A linked list implementation of an open set. This is the original implementation from MineBot.
+ * It has incredbly fast insert performance, at the cost of O(n) removeLowest.
  */
 public class LinkedListOpenSet implements IOpenSet {
     private PathNode first = null;
@@ -26,11 +27,11 @@ public class LinkedListOpenSet implements IOpenSet {
             return n;
         }
         PathNode previous = first;
-        double bestValue = first.estimatedCostToGoal + first.cost;
+        double bestValue = first.combinedCost;
         PathNode bestNode = first;
         PathNode beforeBest = null;
         while (current != null) {
-            double comp = current.estimatedCostToGoal + current.cost;
+            double comp = current.combinedCost;
             if (comp < bestValue) {
                 bestValue = comp;
                 bestNode = current;

src/main/java/baritone/bot/pathing/calc/PathNode.java

@@ -22,6 +22,8 @@ class PathNode {
     // These three fields are mutable and are changed by PathFinder
     double cost;
 
+    public double combinedCost;
+
     PathNode previous;
 
     Movement previousMovement;

src/main/java/baritone/bot/pathing/movement/movements/MovementAscend.java

@@ -17,6 +17,11 @@ public class MovementAscend extends Movement {
         throw new UnsupportedOperationException();
     }
 
+    @Override
+    public void onFinish() {
+
+    }
+
     @Override
     public MovementState updateState() {
         MovementState latestState = currentState.setInput(InputOverrideHandler.Input.JUMP, true).setInput(InputOverrideHandler.Input.MOVE_FORWARD, true);

src/test/java/baritone/bot/pathing/calc/OpenSetsTest.java

@@ -0,0 +1,66 @@
+package baritone.bot.pathing.calc;
+
+import baritone.bot.pathing.goals.GoalBlock;
+import net.minecraft.util.math.BlockPos;
+import org.junit.Test;
+
+import static org.junit.Assert.*;
+
+public class OpenSetsTest {
+
+    @Test
+    public void testOpenSets() {
+        for (int size = 1; size < 100; size++) {
+            testSize(size);
+        }
+        for (int size = 100; size < 10000; size += 100) {
+            testSize(size);
+        }
+    }
+
+    public void testSize(int size) {
+        System.out.println("Testing size " + size);
+        // Include LinkedListOpenSet even though it's not performant because I absolutely trust that it behaves properly
+        // I'm really testing the heap implementations against it as the ground truth
+        IOpenSet[] test = new IOpenSet[]{new BinaryHeapOpenSet(), new LinkedListOpenSet(), new FibonacciHeapOpenSet()};
+        for (IOpenSet set : test) {
+            assertTrue(set.isEmpty());
+        }
+        PathNode[] toInsert = new PathNode[size];
+        for (int i = 0; i < size; i++) {
+            PathNode pn = new PathNode(new BlockPos(0, 0, 0), new GoalBlock(new BlockPos(0, 0, 0)));
+            pn.combinedCost = Math.random();
+            toInsert[i] = pn;
+
+        }
+        System.out.println("Insertion");
+        for (IOpenSet set : test) {
+            long before = System.currentTimeMillis();
+            for (int i = 0; i < size; i++)
+                set.insert(toInsert[i]);
+            System.out.println(set.getClass() + " " + (System.currentTimeMillis() - before));
+            //all three take either 0 or 1ms to insert up to 10,000 nodes
+            //linkedlist takes 0ms most often (because there's no array resizing or allocation there, just pointer shuffling)
+        }
+        for (IOpenSet set : test) {
+            assertFalse(set.isEmpty());
+        }
+        System.out.println("Removal");
+        double[][] results = new double[test.length][size];
+        for (int i = 0; i < test.length; i++) {
+            long before = System.currentTimeMillis();
+            for (int j = 0; j < size; j++) {
+                results[i][j] = test[i].removeLowest().combinedCost;
+            }
+            System.out.println(test[i].getClass() + " " + (System.currentTimeMillis() - before));
+        }
+        for (int j = 0; j < size; j++) {
+            for (int i = 1; i < test.length; i++) {
+                assertEquals(results[i][j], results[0][j], 0);
+            }
+        }
+        for (IOpenSet set : test) {
+            assertTrue(set.isEmpty());
+        }
+    }
+}