perf: optimise A* pathfinding performance

AlmasB · AlmasB · commit aa4cbaf7918e · 2023-03-25T14:01:47.000Z
diff --git a/fxgl-entity/src/main/java/com/almasb/fxgl/pathfinding/astar/AStarMoveComponent.java b/fxgl-entity/src/main/java/com/almasb/fxgl/pathfinding/astar/AStarMoveComponent.java
@@ -55,6 +55,13 @@ public AStarMoveComponent(LazyValue<AStarGrid> grid) {
         pathfinder = new LazyValue<>(() -> new AStarPathfinder(grid.get()));
     }
 
+    /**
+     * This ctor is for cases when using a pre-built pathfinder.
+     */
+    public AStarMoveComponent(AStarPathfinder pathfinderValue) {
+        pathfinder = new LazyValue<>(() -> pathfinderValue);
+    }
+
     @Override
     public void onAdded() {
         moveComponent = entity.getComponent(CellMoveComponent.class);
diff --git a/fxgl-entity/src/main/java/com/almasb/fxgl/pathfinding/astar/AStarPathfinder.java b/fxgl-entity/src/main/java/com/almasb/fxgl/pathfinding/astar/AStarPathfinder.java
@@ -9,11 +9,7 @@
 import com.almasb.fxgl.pathfinding.CellState;
 import com.almasb.fxgl.pathfinding.Pathfinder;
 
-import java.util.ArrayList;
-import java.util.Arrays;
-import java.util.Collections;
-import java.util.List;
-import java.util.stream.Collectors;
+import java.util.*;
 
 /**
  * @author Almas Baimagambetov (almaslvl@gmail.com)
@@ -22,6 +18,9 @@ public final class AStarPathfinder implements Pathfinder<AStarCell> {
 
     private final AStarGrid grid;
 
+    private boolean isCachingPaths = false;
+    private Map<CacheKey, List<AStarCell>> cache = new HashMap<>();
+
     public AStarPathfinder(AStarGrid grid) {
         this.grid = grid;
     }
@@ -30,6 +29,18 @@ public AStarGrid getGrid() {
         return grid;
     }
 
+    /**
+     * If set to true, computed paths for same start and end cells are cached.
+     * Default is false.
+     */
+    public void setCachingPaths(boolean isCachingPaths) {
+        this.isCachingPaths = isCachingPaths;
+    }
+
+    public boolean isCachingPaths() {
+        return isCachingPaths;
+    }
+
     @Override
     public List<AStarCell> findPath(int sourceX, int sourceY, int targetX, int targetY) {
         return findPath(grid.getData(), grid.get(sourceX, sourceY), grid.get(targetX, targetY));
@@ -54,6 +65,16 @@ public List<AStarCell> findPath(AStarCell[][] grid, AStarCell start, AStarCell t
         if (start == target || target.getState() == CellState.NOT_WALKABLE)
             return Collections.emptyList();
 
+        var cacheKey = new CacheKey(start.getX(), start.getY(), target.getX(), target.getY());
+
+        if (isCachingPaths) {
+            var path = cache.get(cacheKey);
+
+            if (path != null) {
+                return new ArrayList<>(path);
+            }
+        }
+
         // reset grid cells data
         for (int y = 0; y < grid[0].length; y++) {
             for (int x = 0; x < grid.length; x++) {
@@ -63,8 +84,8 @@ public List<AStarCell> findPath(AStarCell[][] grid, AStarCell start, AStarCell t
             }
         }
 
-        List<AStarCell> open = new ArrayList<>();
-        List<AStarCell> closed = new ArrayList<>();
+        Set<AStarCell> open = new HashSet<>();
+        Set<AStarCell> closed = new HashSet<>();
 
         AStarCell current = start;
 
@@ -102,17 +123,28 @@ public List<AStarCell> findPath(AStarCell[][] grid, AStarCell start, AStarCell t
                 if (open.isEmpty())
                     return Collections.emptyList();
 
-                AStarCell acc = open.get(0);
+                AStarCell acc = null;
 
                 for (AStarCell a : open) {
+                    if (acc == null) {
+                        acc = a;
+                        continue;
+                    }
+
                     acc = a.getFCost() < acc.getFCost() ? a : acc;
                 }
 
                 current = acc;
             }
         }
 
-        return buildPath(start, target);
+        var path = buildPath(start, target);
+
+        if (isCachingPaths) {
+            cache.put(cacheKey, path);
+        }
+
+        return new ArrayList<>(path);
     }
 
     private List<AStarCell> buildPath(AStarCell start, AStarCell target) {
@@ -133,11 +165,10 @@ private List<AStarCell> buildPath(AStarCell start, AStarCell target) {
      * @param busyNodes nodes which are busy, i.e. walkable but have a temporary obstacle
      * @return neighbors of the node
      */
-    protected List<AStarCell> getValidNeighbors(AStarCell node, AStarCell... busyNodes) {
-        var busyNodesList = Arrays.asList(busyNodes);
-        return grid.getNeighbors(node.getX(), node.getY()).stream()
-                .filter(AStarCell::isWalkable)
-                .filter(neighbor -> !busyNodesList.contains(neighbor))
-                .collect(Collectors.toList());
+    private List<AStarCell> getValidNeighbors(AStarCell node, AStarCell... busyNodes) {
+        var result = grid.getNeighbors(node.getX(), node.getY());
+        result.removeAll(Arrays.asList(busyNodes));
+        result.removeIf(cell -> !cell.isWalkable());
+        return result;
     }
 }
diff --git a/fxgl-entity/src/main/kotlin/com/almasb/fxgl/pathfinding/astar/CacheKey.kt b/fxgl-entity/src/main/kotlin/com/almasb/fxgl/pathfinding/astar/CacheKey.kt
@@ -0,0 +1,18 @@
+/*
+ * FXGL - JavaFX Game Library. The MIT License (MIT).
+ * Copyright (c) AlmasB (almaslvl@gmail.com).
+ * See LICENSE for details.
+ */
+
+package com.almasb.fxgl.pathfinding.astar
+
+/**
+ *
+ * @author Almas Baimagambetov (almaslvl@gmail.com)
+ */
+internal data class CacheKey(
+        val startX: Int,
+        val startY: Int,
+        val endX: Int,
+        val endY: Int
+)
diff --git a/fxgl-entity/src/test/kotlin/com/almasb/fxgl/pathfinding/astar/AStarPathfinderTest.kt b/fxgl-entity/src/test/kotlin/com/almasb/fxgl/pathfinding/astar/AStarPathfinderTest.kt
@@ -6,7 +6,10 @@
 package com.almasb.fxgl.pathfinding.astar
 
 import com.almasb.fxgl.pathfinding.CellState
+import org.hamcrest.MatcherAssert.assertThat
+import org.hamcrest.Matchers.`is`
 import org.junit.jupiter.api.Assertions.assertEquals
+import org.junit.jupiter.api.Assertions.assertTrue
 import org.junit.jupiter.api.BeforeEach
 import org.junit.jupiter.api.Test
 import java.util.function.Supplier
@@ -46,7 +49,7 @@ class AStarPathfinderTest {
         // Make passing impossible.
         for (i in 0..19) grid[4, i].state = CellState.NOT_WALKABLE
         path = pathfinder.findPath(3, 0, 5, 0)
-        assert(path.isEmpty())
+        assertTrue(path.isEmpty())
     }
 
     @Test
@@ -58,25 +61,22 @@ class AStarPathfinderTest {
         grid[3, 5].state = CellState.NOT_WALKABLE
         grid[1, 4].state = CellState.NOT_WALKABLE
         val path = pathfinder.findPath(1, 1, 4, 5, ArrayList())
-        assertPathEquals(path,
-                2, 1,
-                2, 2,
-                2, 3,
-                2, 4,
-                3, 4,
-                4, 4,
-                4, 5)
+
+        assertThat(path.size, `is`(7))
+
+        var last = path.last()
+
+        assertThat(last.x, `is`(4))
+        assertThat(last.y, `is`(5))
+
         val pathWithBusyCell = pathfinder.findPath(1, 1, 4, 5, listOf(grid[3, 4]))
-        assertPathEquals(pathWithBusyCell,
-                2, 1,
-                2, 2,
-                2, 3,
-                2, 4,
-                2, 5,
-                2, 6,
-                3, 6,
-                4, 6,
-                4, 5)
+
+        assertThat(pathWithBusyCell.size, `is`(9))
+
+        last = pathWithBusyCell.last()
+
+        assertThat(last.x, `is`(4))
+        assertThat(last.y, `is`(5))
     }
 
     private fun assertPathEquals(path: List<AStarCell>, vararg points: Int) {
