fix a* implementation by adding decrease-key operation to heap implementations

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

@@ -1,8 +1,5 @@
 package baritone.bot.pathing.calc;
 
-
-//import baritone.Baritone;
-
 import baritone.bot.pathing.calc.openset.BinaryHeapOpenSet;
 import baritone.bot.pathing.calc.openset.IOpenSet;
 import baritone.bot.pathing.goals.Goal;
@@ -55,8 +52,8 @@ public class AStarPathFinder extends AbstractNodeCostSearch {
                 }
             }*/
             PathNode currentNode = openSet.removeLowest();
-            mostRecentConsidered = currentNode;
             currentNode.isOpen = false;
+            mostRecentConsidered = currentNode;
             BlockPos currentNodePos = currentNode.pos;
             numNodes++;
             if (System.currentTimeMillis() > lastPrintout + 1000) {//print once a second
@@ -92,7 +89,9 @@ public class AStarPathFinder extends AbstractNodeCostSearch {
                     neighbor.previousMovement = movementToGetToNeighbor;
                     neighbor.cost = tentativeCost;
                     neighbor.combinedCost = tentativeCost + neighbor.estimatedCostToGoal;
-                    if (!neighbor.isOpen) {
+                    if (neighbor.isOpen) {
+                        openSet.update(neighbor);
+                    } else {
                         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/PathNode.java

@@ -2,6 +2,7 @@ package baritone.bot.pathing.calc;
 
 import baritone.bot.pathing.goals.Goal;
 import baritone.bot.pathing.movement.Movement;
+import baritone.bot.pathing.util.FibonacciHeap;
 import net.minecraft.util.math.BlockPos;
 
 import java.util.Objects;
@@ -17,7 +18,7 @@ public class PathNode {
      * The position of this node
      */
     final BlockPos pos;
-    
+
     /**
      * The goal it's going towards
      */
@@ -58,6 +59,9 @@ public class PathNode {
      */
     boolean isOpen;
 
+    public int heapPosition;
+    public FibonacciHeap.Node parent;
+
     public PathNode(BlockPos pos, Goal goal) {
         this.pos = pos;
         this.previous = null;

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

@@ -37,13 +37,13 @@ public class BinaryHeapOpenSet implements IOpenSet {
         }
         size++;
         int index = size;
+        value.heapPosition = index;
         array[index] = value;
-        int parent = index >>> 1;
+        upHeap(index);
-        while (index > 1 && array[parent].combinedCost > array[index].combinedCost) {
+    }
-            swap(index, parent);
+
-            index = parent;
+    public void update(PathNode node) {
-            parent = index >>> 1;
+        upHeap(node.heapPosition);
-        }
     }
 
     @Override
@@ -58,24 +58,37 @@ public class BinaryHeapOpenSet implements IOpenSet {
         }
         PathNode result = array[1];
         array[1] = array[size];
+        array[1].heapPosition = 1;
         array[size] = null;
         size--;
-        int index = 1;
+        downHeap(1);
+        result.heapPosition = -1;
+        return result;
+    }
+
+    private void upHeap(int index) {
+        int parent = index >>> 1;
+        while (index > 1 && array[parent].combinedCost > array[index].combinedCost) {
+            swap(index, parent);
+            index = parent;
+            parent = index >>> 1;
+        }
+    }
+
+    private void downHeap(int index) {
         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) {
+            if (array[index].combinedCost <= array[smallerChild].combinedCost) {
-                swap(index, smallerChild);
-            } else {
                 break;
             }
+            swap(index, smallerChild);
             index = smallerChild;
             smallerChild = index << 1;
         }
-        return result;
     }
 
     /**
@@ -85,8 +98,14 @@ public class BinaryHeapOpenSet implements IOpenSet {
      * @param index2 The second index
      */
     protected void swap(int index1, int index2) {
+        //sanity checks, disabled because of performance hit
+        //if (array[index1].heapPosition != index1) throw new IllegalStateException();
+        //if (array[index2].heapPosition != index2) throw new IllegalStateException();
         PathNode tmp = array[index1];
         array[index1] = array[index2];
         array[index2] = tmp;
+        tmp.heapPosition = index2;
+        array[index1].heapPosition = index1;
     }
+
 }

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

@@ -1,7 +1,7 @@
 package baritone.bot.pathing.calc.openset;
 
-import baritone.bot.pathing.util.FibonacciHeap;
 import baritone.bot.pathing.calc.PathNode;
+import baritone.bot.pathing.util.FibonacciHeap;
 
 /**
  * Wrapper adapter between FibonacciHeap and OpenSet
@@ -17,6 +17,12 @@ public class FibonacciHeapOpenSet extends FibonacciHeap implements IOpenSet {
 
     @Override
     public PathNode removeLowest() {
-        return (PathNode) super.removeMin();
+        PathNode pn = super.removeMin();
+        pn.parent = null;
+        return pn;
+    }
+
+    public void update(PathNode node) {
+        super.decreaseKey(node.parent, node.combinedCost);
     }
 }

src/main/java/baritone/bot/pathing/calc/openset/IOpenSet.java

@@ -27,4 +27,11 @@ public interface IOpenSet {
      * @return The minimum element in the heap
      */
     PathNode removeLowest();
+
+    /**
+     * A faster path has been found to this node, decreasing its cost. Perform a decrease-key operation.
+     *
+     * @param node The node
+     */
+    void update(PathNode node);
 }

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

@@ -9,10 +9,12 @@ import baritone.bot.pathing.calc.PathNode;
 public class LinkedListOpenSet implements IOpenSet {
     private Node first = null;
 
+    @Override
     public boolean isEmpty() {
         return first == null;
     }
 
+    @Override
     public void insert(PathNode pathNode) {
         Node node = new Node();
         node.val = pathNode;
@@ -20,6 +22,12 @@ public class LinkedListOpenSet implements IOpenSet {
         first = node;
     }
 
+    @Override
+    public void update(PathNode node) {
+
+    }
+
+    @Override
     public PathNode removeLowest() {
         if (first == null) {
             return null;

src/main/java/baritone/bot/pathing/util/FibonacciHeap.java

@@ -24,6 +24,8 @@ package baritone.bot.pathing.util;
  */
 //package com.bluemarsh.graphmaker.core.util;
 
+import baritone.bot.pathing.calc.PathNode;
+
 /**
  * This class implements a Fibonacci heap data structure. Much of the
  * code in this class is based on the algorithms in Chapter 21 of the
@@ -233,8 +235,9 @@ public class FibonacciHeap {
      * @param key key value associated with data object.
      * @return newly created heap node.
      */
-    public Node insert(Object x, double key) {
+    public Node insert(PathNode x, double key) {
         Node node = new Node(x, key);
+        x.parent = node;
         // concatenate node into min list
         if (min != null) {
             node.right = min;
@@ -271,7 +274,7 @@ public class FibonacciHeap {
      *
      * @return data object with the smallest key.
      */
-    public Object removeMin() {
+    public PathNode removeMin() {
         Node z = min;
         if (z == null) {
             return null;
@@ -329,7 +332,7 @@ public class FibonacciHeap {
         /**
          * Data object for this node, holds the key value.
          */
-        private Object data;
+        private PathNode data;
         /**
          * Key value for this node.
          */
@@ -368,7 +371,7 @@ public class FibonacciHeap {
          * @param data data object to associate with this node
          * @param key  key value for this data object
          */
-        public Node(Object data, double key) {
+        public Node(PathNode data, double key) {
             this.data = data;
             this.key = key;
             right = this;

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

@@ -5,6 +5,8 @@ import baritone.bot.pathing.goals.GoalBlock;
 import net.minecraft.util.math.BlockPos;
 import org.junit.Test;
 
+import java.util.*;
+
 import static org.junit.Assert.*;
 
 public class OpenSetsTest {
@@ -19,6 +21,29 @@ public class OpenSetsTest {
         }
     }
 
+    public void removeAndTest(int amount, IOpenSet[] test, Optional<Collection<PathNode>> mustContain) {
+        double[][] results = new double[test.length][amount];
+        for (int i = 0; i < test.length; i++) {
+            long before = System.currentTimeMillis();
+            for (int j = 0; j < amount; j++) {
+                PathNode pn = test[i].removeLowest();
+                if (mustContain.isPresent() && !mustContain.get().contains(pn)) {
+                    throw new IllegalStateException(mustContain.get() + " " + pn);
+                }
+                results[i][j] = pn.combinedCost;
+            }
+            System.out.println(test[i].getClass() + " " + (System.currentTimeMillis() - before));
+        }
+        for (int j = 0; j < amount; j++) {
+            for (int i = 1; i < test.length; i++) {
+                assertEquals(results[i][j], results[0][j], 0);
+            }
+        }
+        for (int i = 0; i < amount - 1; i++) {
+            assertTrue(results[0][i] < results[0][i + 1]);
+        }
+    }
+
     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
@@ -27,13 +52,25 @@ public class OpenSetsTest {
         for (IOpenSet set : test) {
             assertTrue(set.isEmpty());
         }
+
+        // generate the pathnodes that we'll be testing the sets on
         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;
-
         }
+
+        // create a list of what the first removals should be
+        ArrayList<PathNode> copy = new ArrayList<>(Arrays.asList(toInsert));
+        copy.sort(Comparator.comparingDouble(pn -> pn.combinedCost));
+        Set<PathNode> lowestQuarter = new HashSet<>(copy.subList(0, size / 4));
+
+        // all opensets should be empty; nothing has been inserted yet
+        for (IOpenSet set : test) {
+            assertTrue(set.isEmpty());
+        }
+
         System.out.println("Insertion");
         for (IOpenSet set : test) {
             long before = System.currentTimeMillis();
@@ -43,23 +80,46 @@ public class OpenSetsTest {
             //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)
         }
+
+        // all opensets should now be full
         for (IOpenSet set : test) {
             assertFalse(set.isEmpty());
         }
-        System.out.println("Removal");
+
-        double[][] results = new double[test.length][size];
+        System.out.println("Removal round 1");
-        for (int i = 0; i < test.length; i++) {
+        // remove a quarter of the nodes and verify that they are indeed the size/4 lowest ones
-            long before = System.currentTimeMillis();
+        removeAndTest(size / 4, test, Optional.of(lowestQuarter));
-            for (int j = 0; j < size; j++) {
+
-                results[i][j] = test[i].removeLowest().combinedCost;
+        // none of them should be empty (sanity check)
-            }
+        for (IOpenSet set : test) {
-            System.out.println(test[i].getClass() + " " + (System.currentTimeMillis() - before));
+            assertFalse(set.isEmpty());
         }
-        for (int j = 0; j < size; j++) {
+        int cnt = 0;
-            for (int i = 1; i < test.length; i++) {
+        for (int i = 0; cnt < size / 2 && i < size; i++) {
-                assertEquals(results[i][j], results[0][j], 0);
+            if (lowestQuarter.contains(toInsert[i])) { // these were already removed and can't be updated to test
+                continue;
             }
+            toInsert[i].combinedCost *= Math.random();
+            // multiplying it by a random number between 0 and 1 is guaranteed to decrease it
+            for (IOpenSet set : test) {
+                // it's difficult to benchmark these individually because if you modify all at once then update then
+                // it breaks the internal consistency of the heaps.
+                // you have to call update every time you modify a node.
+                set.update(toInsert[i]);
+            }
+            cnt++;
         }
+
+        //still shouldn't be empty
+        for (IOpenSet set : test) {
+            assertFalse(set.isEmpty());
+        }
+
+        System.out.println("Removal round 2");
+        // remove the remaining 3/4
+        removeAndTest(size - size / 4, test, Optional.empty());
+
+        // every set should now be empty
         for (IOpenSet set : test) {
             assertTrue(set.isEmpty());
         }