feat: Support 8 directions AStar pathfinding, closes #1213

DeathPhoenix22 · web-flow · commit 641b614a11a4 · 2024-02-20T18:29:39.000Z
Co-authored-by: Jean-René Lavoie &lt;&gt;
diff --git a/fxgl-entity/src/main/java/com/almasb/fxgl/pathfinding/Pathfinder.java b/fxgl-entity/src/main/java/com/almasb/fxgl/pathfinding/Pathfinder.java
@@ -7,6 +7,7 @@
 package com.almasb.fxgl.pathfinding;
 
 import com.almasb.fxgl.core.collection.grid.Cell;
+import com.almasb.fxgl.core.collection.grid.NeighborFilteringOption;
 
 import java.util.List;
 
@@ -22,10 +23,25 @@ public interface Pathfinder<T extends Cell> {
      */
     List<T> findPath(int sourceX, int sourceY, int targetX, int targetY);
 
+    /**
+     * Empty list is returned if no path exists.
+     *
+     * @return a list of cells from source (excl.) to target (incl.)
+     */
+    List<T> findPath(int sourceX, int sourceY, int targetX, int targetY, NeighborFilteringOption neighborFilteringOption);
+
     /**
      * Empty list is returned if no path exists.
      *
      * @return a list of cells from source (excl.) to target (incl.) while ignoring busyCells
      */
     List<T> findPath(int sourceX, int sourceY, int targetX, int targetY, List<T> busyCells);
+
+    /**
+     * Empty list is returned if no path exists.
+     *
+     * @return a list of cells from source (excl.) to target (incl.) while ignoring busyCells
+     */
+    List<T> findPath(int sourceX, int sourceY, int targetX, int targetY, NeighborFilteringOption neighborFilteringOption, List<T> busyCells);
+
 }
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
@@ -6,8 +6,14 @@
 
 package com.almasb.fxgl.pathfinding.astar;
 
+import com.almasb.fxgl.core.collection.grid.Cell;
+import com.almasb.fxgl.core.collection.grid.NeighborFilteringOption;
+import static com.almasb.fxgl.core.collection.grid.NeighborFilteringOption.*;
 import com.almasb.fxgl.pathfinding.CellState;
 import com.almasb.fxgl.pathfinding.Pathfinder;
+import com.almasb.fxgl.pathfinding.heuristic.Heuristic;
+import com.almasb.fxgl.pathfinding.heuristic.ManhattanDistance;
+import com.almasb.fxgl.pathfinding.heuristic.OctileDistance;
 
 import java.util.*;
 
@@ -18,11 +24,20 @@ public final class AStarPathfinder implements Pathfinder<AStarCell> {
 
     private final AStarGrid grid;
 
+    private final Heuristic<AStarCell> defaultHeuristic;
+    private final Heuristic<AStarCell> diagonalHeuristic;
+
     private boolean isCachingPaths = false;
     private Map<CacheKey, List<AStarCell>> cache = new HashMap<>();
 
     public AStarPathfinder(AStarGrid grid) {
+        this(grid, new ManhattanDistance<>(10), new OctileDistance<>());
+    }
+
+    public AStarPathfinder(AStarGrid grid, Heuristic<AStarCell> defaultHeuristic, Heuristic<AStarCell> diagonalHeuristic) {
         this.grid = grid;
+        this.defaultHeuristic = defaultHeuristic;
+        this.diagonalHeuristic = diagonalHeuristic;
     }
 
     public AStarGrid getGrid() {
@@ -46,11 +61,21 @@ public List<AStarCell> findPath(int sourceX, int sourceY, int targetX, int targe
         return findPath(grid.getData(), grid.get(sourceX, sourceY), grid.get(targetX, targetY));
     }
 
+    @Override
+    public List<AStarCell> findPath(int sourceX, int sourceY, int targetX, int targetY, NeighborFilteringOption neighborFilteringOption) {
+        return findPath(grid.getData(), grid.get(sourceX, sourceY), grid.get(targetX, targetY), neighborFilteringOption);
+    }
+
     @Override
     public List<AStarCell> findPath(int sourceX, int sourceY, int targetX, int targetY, List<AStarCell> busyCells) {
         return findPath(grid.getData(), grid.get(sourceX, sourceY), grid.get(targetX, targetY), busyCells.toArray(new AStarCell[0]));
     }
 
+    @Override
+    public List<AStarCell> findPath(int sourceX, int sourceY, int targetX, int targetY, NeighborFilteringOption neighborFilteringOption, List<AStarCell> busyCells) {
+        return findPath(grid.getData(), grid.get(sourceX, sourceY), grid.get(targetX, targetY), neighborFilteringOption, busyCells.toArray(new AStarCell[0]));
+    }
+
     /**
      * Since the equality check is based on references,
      * start and target must be elements of the array.
@@ -62,6 +87,20 @@ public List<AStarCell> findPath(int sourceX, int sourceY, int targetX, int targe
      * @return          path as list of nodes from start (excl) to target (incl) or empty list if no path found
      */
     public List<AStarCell> findPath(AStarCell[][] grid, AStarCell start, AStarCell target, AStarCell... busyNodes) {
+        return findPath(grid, start, target, NeighborFilteringOption.FOUR_DIRECTIONS, busyNodes);
+    }
+
+    /**
+     * Since the equality check is based on references,
+     * start and target must be elements of the array.
+     *
+     * @param grid      the grid of nodes
+     * @param start     starting node
+     * @param target    target node
+     * @param busyNodes busy "unwalkable" nodes
+     * @return          path as list of nodes from start (excl) to target (incl) or empty list if no path found
+     */
+    public List<AStarCell> findPath(AStarCell[][] grid, AStarCell start, AStarCell target, NeighborFilteringOption neighborFilteringOption, AStarCell... busyNodes) {
         if (start == target || target.getState() == CellState.NOT_WALKABLE)
             return Collections.emptyList();
 
@@ -75,10 +114,12 @@ public List<AStarCell> findPath(AStarCell[][] grid, AStarCell start, AStarCell t
             }
         }
 
+        Heuristic<AStarCell> heuristic = (neighborFilteringOption.is(FOUR_DIRECTIONS)) ? defaultHeuristic : diagonalHeuristic;
+
         // reset grid cells data
         for (int y = 0; y < grid[0].length; y++) {
             for (int x = 0; x < grid.length; x++) {
-                grid[x][y].setHCost(Math.abs(target.getX() - x) + Math.abs(target.getY() - y));
+                grid[x][y].setHCost(heuristic.getCost(x, y, target));
                 grid[x][y].setParent(null);
                 grid[x][y].setGCost(0);
             }
@@ -92,7 +133,7 @@ public List<AStarCell> findPath(AStarCell[][] grid, AStarCell start, AStarCell t
         boolean found = false;
 
         while (!found && !closed.contains(target)) {
-            for (AStarCell neighbor : getValidNeighbors(current, busyNodes)) {
+            for (AStarCell neighbor : getValidNeighbors(current, neighborFilteringOption, busyNodes)) {
                 if (neighbor == target) {
                     target.setParent(current);
                     found = true;
@@ -101,16 +142,18 @@ public List<AStarCell> findPath(AStarCell[][] grid, AStarCell start, AStarCell t
                 }
 
                 if (!closed.contains(neighbor)) {
+                    int gCost = isDiagonal(current, neighbor) ? diagonalHeuristic.getWeight() : defaultHeuristic.getWeight();
+                    int newGCost = current.getGCost() + gCost;
+
                     if (open.contains(neighbor)) {
-                        int newG = current.getGCost() + 10;
+                        if (newGCost < neighbor.getGCost()) {
 
-                        if (newG < neighbor.getGCost()) {
                             neighbor.setParent(current);
-                            neighbor.setGCost(newG);
+                            neighbor.setGCost(newGCost);
                         }
                     } else {
                         neighbor.setParent(current);
-                        neighbor.setGCost(current.getGCost() + 10);
+                        neighbor.setGCost(newGCost);
                         open.add(neighbor);
                     }
                 }
@@ -165,10 +208,15 @@ 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
      */
-    private List<AStarCell> getValidNeighbors(AStarCell node, AStarCell... busyNodes) {
-        var result = grid.getNeighbors(node.getX(), node.getY());
+    private List<AStarCell> getValidNeighbors(AStarCell node, NeighborFilteringOption neighborFilteringOption, AStarCell... busyNodes) {
+        var result = grid.getNeighbors(node.getX(), node.getY(), neighborFilteringOption);
         result.removeAll(Arrays.asList(busyNodes));
         result.removeIf(cell -> !cell.isWalkable());
         return result;
     }
+
+    private boolean isDiagonal(Cell current, Cell neighbor) {
+        return neighbor.getX() - current.getX() != 0 && neighbor.getY() - current.getY() != 0;
+    }
+
 }
diff --git a/fxgl-entity/src/main/java/com/almasb/fxgl/pathfinding/heuristic/Heuristic.java b/fxgl-entity/src/main/java/com/almasb/fxgl/pathfinding/heuristic/Heuristic.java
@@ -0,0 +1,34 @@
+/*
+ * FXGL - JavaFX Game Library. The MIT License (MIT).
+ * Copyright (c) AlmasB (almaslvl@gmail.com).
+ * See LICENSE for details.
+ */
+
+package com.almasb.fxgl.pathfinding.heuristic;
+
+import com.almasb.fxgl.core.collection.grid.Cell;
+
+/**
+ * @author Jean-René Lavoie (jeanrlavoie@gmail.com)
+ */
+public abstract class Heuristic<T extends Cell> {
+
+    public static final int DEFAULT_WEIGHT = 10;
+
+    private final int weight;
+
+    public Heuristic() {
+        this(DEFAULT_WEIGHT);
+    }
+
+    public Heuristic(int weight) {
+        this.weight = weight;
+    }
+
+   public abstract int getCost(int x, int y, T target);
+
+    public int getWeight() {
+        return weight;
+    }
+
+}
diff --git a/fxgl-entity/src/main/java/com/almasb/fxgl/pathfinding/heuristic/ManhattanDistance.java b/fxgl-entity/src/main/java/com/almasb/fxgl/pathfinding/heuristic/ManhattanDistance.java
@@ -0,0 +1,29 @@
+/*
+ * FXGL - JavaFX Game Library. The MIT License (MIT).
+ * Copyright (c) AlmasB (almaslvl@gmail.com).
+ * See LICENSE for details.
+ */
+
+package com.almasb.fxgl.pathfinding.heuristic;
+
+import com.almasb.fxgl.core.collection.grid.Cell;
+
+/**
+ * @author Jean-René Lavoie (jeanrlavoie@gmail.com)
+ */
+public class ManhattanDistance<T extends Cell> extends Heuristic<T> {
+
+    public ManhattanDistance() {
+        super();
+    }
+
+    public ManhattanDistance(int weight) {
+        super(weight);
+    }
+
+    @Override
+    public int getCost(int x, int y, T target) {
+        return (Math.abs(target.getX() - x) + Math.abs(target.getY() - y)) * getWeight();
+    }
+
+}
diff --git a/fxgl-entity/src/main/java/com/almasb/fxgl/pathfinding/heuristic/OctileDistance.java b/fxgl-entity/src/main/java/com/almasb/fxgl/pathfinding/heuristic/OctileDistance.java
@@ -0,0 +1,41 @@
+/*
+ * FXGL - JavaFX Game Library. The MIT License (MIT).
+ * Copyright (c) AlmasB (almaslvl@gmail.com).
+ * See LICENSE for details.
+ */
+
+package com.almasb.fxgl.pathfinding.heuristic;
+
+import com.almasb.fxgl.core.collection.grid.Cell;
+
+/**
+ * @author Jean-René Lavoie (jeanrlavoie@gmail.com)
+ */
+public class OctileDistance<T extends Cell> extends Heuristic<T> {
+
+    private static final int DIAGONAL_WEIGHT = (int)(Math.sqrt(2) * 10.0);
+    private static final int DIAGONAL_FACTOR = DIAGONAL_WEIGHT - 10;
+
+    public OctileDistance() {
+        super(DIAGONAL_WEIGHT);
+    }
+
+    public OctileDistance(int weight) {
+        super(weight);
+    }
+
+    @Override
+    public int getCost(int x, int y, T target) {
+        int dx = Math.abs(x - target.getX());
+        int dy = Math.abs(y - target.getY());
+
+        if(dx == dy) {
+            return (dx + dy) * 10;
+        }
+        if(dx < dy) {
+            return DIAGONAL_FACTOR * dx + 10 * dy;
+        }
+        return DIAGONAL_FACTOR * dy + 10 * dx;
+    }
+
+}
diff --git a/fxgl-entity/src/main/java/module-info.java b/fxgl-entity/src/main/java/module-info.java
@@ -23,6 +23,7 @@
     exports com.almasb.fxgl.particle;
     exports com.almasb.fxgl.pathfinding;
     exports com.almasb.fxgl.pathfinding.astar;
+    exports com.almasb.fxgl.pathfinding.heuristic;
     exports com.almasb.fxgl.pathfinding.maze;
     exports com.almasb.fxgl.physics;
     exports com.almasb.fxgl.physics.box2d.dynamics;
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
