clean up, update A star costs

AlmasB · Feb 21, 2024 · 0a76bfe · 0a76bfe
1 parent 4a08055
commit 0a76bfe
Show file tree

Hide file tree

Showing 6 changed files with 99 additions and 30 deletions.
diff --git a/fxgl-entity/src/main/java/com/almasb/fxgl/pathfinding/astar/AStarCell.java b/fxgl-entity/src/main/java/com/almasb/fxgl/pathfinding/astar/AStarCell.java
@@ -14,16 +14,39 @@
  */
 public class AStarCell extends Cell {
 
+    private static final int DEFAULT_MOVEMENT_COST = 30;
+
     private AStarCell parent;
 
     private CellState state;
 
+    /**
+     * Determines the movement (G) cost into this cell.
+     * For example, this can take into account different types of terrain:
+     * grass, mountains, sand, water, etc.
+     * This is typically greater than the (H) cost of the cell.
+     */
+    private int movementCost;
+
     private int gCost;
     private int hCost;
 
     public AStarCell(int x, int y, CellState state) {
+        this(x, y, state, DEFAULT_MOVEMENT_COST);
+    }
+
+    public AStarCell(int x, int y, CellState state, int movementCost) {
         super(x, y);
         this.state = state;
+        this.movementCost = movementCost;
+    }
+
+    public final void setMovementCost(int movementCost) {
+        this.movementCost = movementCost;
+    }
+
+    public final int getMovementCost() {
+        return movementCost;
     }
 
     public final void setParent(AStarCell parent) {

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
@@ -11,6 +11,7 @@
 import static com.almasb.fxgl.core.collection.grid.NeighborDirection.*;
 import com.almasb.fxgl.pathfinding.CellState;
 import com.almasb.fxgl.pathfinding.Pathfinder;
+import com.almasb.fxgl.pathfinding.heuristic.DiagonalHeuristic;
 import com.almasb.fxgl.pathfinding.heuristic.Heuristic;
 import com.almasb.fxgl.pathfinding.heuristic.ManhattanDistance;
 import com.almasb.fxgl.pathfinding.heuristic.OctileDistance;
@@ -25,16 +26,16 @@ public final class AStarPathfinder implements Pathfinder<AStarCell> {
     private final AStarGrid grid;
 
     private final Heuristic<AStarCell> defaultHeuristic;
-    private final Heuristic<AStarCell> diagonalHeuristic;
+    private final DiagonalHeuristic<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<>());
+        this(grid, new ManhattanDistance<>(), new OctileDistance<>());
     }
 
-    public AStarPathfinder(AStarGrid grid, Heuristic<AStarCell> defaultHeuristic, Heuristic<AStarCell> diagonalHeuristic) {
+    public AStarPathfinder(AStarGrid grid, Heuristic<AStarCell> defaultHeuristic, DiagonalHeuristic<AStarCell> diagonalHeuristic) {
         this.grid = grid;
         this.defaultHeuristic = defaultHeuristic;
         this.diagonalHeuristic = diagonalHeuristic;
@@ -119,7 +120,7 @@ public List<AStarCell> findPath(AStarCell[][] grid, AStarCell start, AStarCell t
         // 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(heuristic.getCost(x, y, target));
+                grid[x][y].setHCost(heuristic.getCost(x, y, target.getX(), target.getY()));
                 grid[x][y].setParent(null);
                 grid[x][y].setGCost(0);
             }
@@ -142,12 +143,16 @@ public List<AStarCell> findPath(AStarCell[][] grid, AStarCell start, AStarCell t
                 }
 
                 if (!closed.contains(neighbor)) {
-                    int gCost = isDiagonal(current, neighbor) ? diagonalHeuristic.getWeight() : defaultHeuristic.getWeight();
+                    int gCost = isDiagonal(current, neighbor)
+                            ? diagonalHeuristic.getDiagonalWeight()
+                            : defaultHeuristic.getWeight();
+
+                    gCost *= neighbor.getMovementCost();
+
                     int newGCost = current.getGCost() + gCost;
 
                     if (open.contains(neighbor)) {
                         if (newGCost < neighbor.getGCost()) {
-
                             neighbor.setParent(current);
                             neighbor.setGCost(newGCost);
                         }

diff --git a/fxgl-entity/src/main/java/com/almasb/fxgl/pathfinding/heuristic/DiagonalHeuristic.java b/fxgl-entity/src/main/java/com/almasb/fxgl/pathfinding/heuristic/DiagonalHeuristic.java
@@ -0,0 +1,26 @@
+/*
+ * 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 Almas Baim (https://github.com/AlmasB)
+ */
+public abstract class DiagonalHeuristic<T extends Cell> extends Heuristic<T> {
+
+    private final int diagonalWeight;
+
+    public DiagonalHeuristic(int weight, int diagonalWeight) {
+        super(weight);
+        this.diagonalWeight = diagonalWeight;
+    }
+
+    public int getDiagonalWeight() {
+        return diagonalWeight;
+    }
+}
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
@@ -9,11 +9,13 @@
 import com.almasb.fxgl.core.collection.grid.Cell;
 
 /**
+ * Describes a heuristic function h(n), where n is the next cell.
+ *
  * @author Jean-René Lavoie (jeanrlavoie@gmail.com)
  */
 public abstract class Heuristic<T extends Cell> {
 
-    public static final int DEFAULT_WEIGHT = 10;
+    protected static final int DEFAULT_WEIGHT = 10;
 
     private final int weight;
 
@@ -25,10 +27,19 @@ public Heuristic(int weight) {
         this.weight = weight;
     }
 
-   public abstract int getCost(int x, int y, T target);
-
     public int getWeight() {
         return weight;
     }
 
+    /**
+     * @return estimated weighted cost from start to target
+     */
+    public int getCost(T start, T target) {
+        return getCost(start.getX(), start.getY(), target.getX(), target.getY());
+    }
+
+    /**
+     * @return estimated weighted cost from start to target
+     */
+    public abstract int getCost(int startX, int startY, int targetX, int targetY);
 }
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
@@ -8,10 +8,14 @@
 
 import com.almasb.fxgl.core.collection.grid.Cell;
 
+import static java.lang.Math.*;
+
 /**
+ * See https://en.wikipedia.org/wiki/Taxicab_geometry for definition.
+ *
  * @author Jean-René Lavoie (jeanrlavoie@gmail.com)
  */
-public class ManhattanDistance<T extends Cell> extends Heuristic<T> {
+public final class ManhattanDistance<T extends Cell> extends Heuristic<T> {
 
     public ManhattanDistance() {
         super();
@@ -22,8 +26,7 @@ public ManhattanDistance(int weight) {
     }
 
     @Override
-    public int getCost(int x, int y, T target) {
-        return (Math.abs(target.getX() - x) + Math.abs(target.getY() - y)) * getWeight();
+    public int getCost(int startX, int startY, int targetX, int targetY) {
+        return (abs(targetX - startX) + abs(targetY - startY)) * 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
@@ -8,34 +8,35 @@
 
 import com.almasb.fxgl.core.collection.grid.Cell;
 
+import static java.lang.Math.*;
+
 /**
+ * See https://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html#diagonal-distance
+ * for definition.
+ *
  * @author Jean-René Lavoie (jeanrlavoie@gmail.com)
  */
-public class OctileDistance<T extends Cell> extends Heuristic<T> {
+public final class OctileDistance<T extends Cell> extends DiagonalHeuristic<T> {
 
-    private static final int DIAGONAL_WEIGHT = (int)(Math.sqrt(2) * 10.0);
-    private static final int DIAGONAL_FACTOR = DIAGONAL_WEIGHT - 10;
+    private final int diagonalFactor;
 
     public OctileDistance() {
-        super(DIAGONAL_WEIGHT);
+        this(DEFAULT_WEIGHT);
     }
 
     public OctileDistance(int weight) {
-        super(weight);
+        super(weight, (int)(sqrt(2) * weight));
+        diagonalFactor = getDiagonalWeight() - 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;
+    public int getCost(int startX, int startY, int targetX, int targetY) {
+        int dx = abs(startX - targetX);
+        int dy = abs(startY - targetY);
+
+        // D * max(dx, dy) + (D2-D) * min(dx, dy), where
+        // D - 4-directional weight
+        // D2 - diagonal weight
+        return getWeight() * max(dx, dy) + diagonalFactor * min(dx, dy);
     }
-
 }