From 8ee641f3107b8eb9a2d6d03064c362eeef9d1218 Mon Sep 17 00:00:00 2001 From: Leijurv Date: Sun, 5 Aug 2018 11:30:50 -0400 Subject: [PATCH] fix a* implementation by adding decrease-key operation to heap implementations --- .../bot/pathing/calc/AStarPathFinder.java | 9 +- .../baritone/bot/pathing/calc/PathNode.java | 6 +- .../calc/openset/BinaryHeapOpenSet.java | 41 ++++++--- .../calc/openset/FibonacciHeapOpenSet.java | 10 ++- .../bot/pathing/calc/openset/IOpenSet.java | 7 ++ .../calc/openset/LinkedListOpenSet.java | 8 ++ .../bot/pathing/util/FibonacciHeap.java | 11 ++- .../pathing/calc/openset/OpenSetsTest.java | 84 ++++++++++++++++--- 8 files changed, 141 insertions(+), 35 deletions(-) diff --git a/src/main/java/baritone/bot/pathing/calc/AStarPathFinder.java b/src/main/java/baritone/bot/pathing/calc/AStarPathFinder.java index 3883eb6a..f562ffba 100644 --- a/src/main/java/baritone/bot/pathing/calc/AStarPathFinder.java +++ b/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; } diff --git a/src/main/java/baritone/bot/pathing/calc/PathNode.java b/src/main/java/baritone/bot/pathing/calc/PathNode.java index 12a2f0a0..a2f042ed 100644 --- a/src/main/java/baritone/bot/pathing/calc/PathNode.java +++ b/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; diff --git a/src/main/java/baritone/bot/pathing/calc/openset/BinaryHeapOpenSet.java b/src/main/java/baritone/bot/pathing/calc/openset/BinaryHeapOpenSet.java index 9b72f777..7271f43f 100644 --- a/src/main/java/baritone/bot/pathing/calc/openset/BinaryHeapOpenSet.java +++ b/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; - while (index > 1 && array[parent].combinedCost > array[index].combinedCost) { - swap(index, parent); - index = parent; - parent = index >>> 1; - } + upHeap(index); + } + + public void update(PathNode node) { + 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) { - swap(index, smallerChild); - } else { + if (array[index].combinedCost <= array[smallerChild].combinedCost) { 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; } + } diff --git a/src/main/java/baritone/bot/pathing/calc/openset/FibonacciHeapOpenSet.java b/src/main/java/baritone/bot/pathing/calc/openset/FibonacciHeapOpenSet.java index a92d9b13..00e457f6 100644 --- a/src/main/java/baritone/bot/pathing/calc/openset/FibonacciHeapOpenSet.java +++ b/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); } } diff --git a/src/main/java/baritone/bot/pathing/calc/openset/IOpenSet.java b/src/main/java/baritone/bot/pathing/calc/openset/IOpenSet.java index cd91b9cd..28f5ea84 100644 --- a/src/main/java/baritone/bot/pathing/calc/openset/IOpenSet.java +++ b/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); } diff --git a/src/main/java/baritone/bot/pathing/calc/openset/LinkedListOpenSet.java b/src/main/java/baritone/bot/pathing/calc/openset/LinkedListOpenSet.java index 9d0b3aea..72a3d6b7 100644 --- a/src/main/java/baritone/bot/pathing/calc/openset/LinkedListOpenSet.java +++ b/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; diff --git a/src/main/java/baritone/bot/pathing/util/FibonacciHeap.java b/src/main/java/baritone/bot/pathing/util/FibonacciHeap.java index b9a882a0..e4fe4f60 100644 --- a/src/main/java/baritone/bot/pathing/util/FibonacciHeap.java +++ b/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; diff --git a/src/test/java/baritone/bot/pathing/calc/openset/OpenSetsTest.java b/src/test/java/baritone/bot/pathing/calc/openset/OpenSetsTest.java index a9645b68..8a21112f 100644 --- a/src/test/java/baritone/bot/pathing/calc/openset/OpenSetsTest.java +++ b/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> 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 copy = new ArrayList<>(Arrays.asList(toInsert)); + copy.sort(Comparator.comparingDouble(pn -> pn.combinedCost)); + Set 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]; - 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)); + + System.out.println("Removal round 1"); + // remove a quarter of the nodes and verify that they are indeed the size/4 lowest ones + removeAndTest(size / 4, test, Optional.of(lowestQuarter)); + + // none of them should be empty (sanity check) + for (IOpenSet set : test) { + assertFalse(set.isEmpty()); } - for (int j = 0; j < size; j++) { - for (int i = 1; i < test.length; i++) { - assertEquals(results[i][j], results[0][j], 0); + int cnt = 0; + for (int i = 0; cnt < size / 2 && i < size; i++) { + 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()); }