From 6fb593c2bdb049d2ff3b622011383ba2093c20fa Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jean-Rene=CC=81=20Lavoie?= <> Date: Wed, 31 Jan 2024 15:37:34 -0500 Subject: [PATCH] feat: Support 8 directions AStar pathfinding --- .../almasb/fxgl/pathfinding/Pathfinder.java | 16 +++ .../pathfinding/astar/AStarPathfinder.java | 64 +++++++-- .../fxgl/pathfinding/heuristic/Heuristic.java | 34 +++++ .../heuristic/ManhattanDistance.java | 29 ++++ .../pathfinding/heuristic/OctileDistance.java | 41 ++++++ fxgl-entity/src/main/java/module-info.java | 1 + .../pathfinding/astar/AStarPathfinderTest.kt | 135 +++++++++++++++++- 7 files changed, 310 insertions(+), 10 deletions(-) create mode 100644 fxgl-entity/src/main/java/com/almasb/fxgl/pathfinding/heuristic/Heuristic.java create mode 100644 fxgl-entity/src/main/java/com/almasb/fxgl/pathfinding/heuristic/ManhattanDistance.java create mode 100644 fxgl-entity/src/main/java/com/almasb/fxgl/pathfinding/heuristic/OctileDistance.java 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 index 0293f35839..89e18e229f 100644 --- 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 { */ List 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 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 findPath(int sourceX, int sourceY, int targetX, int targetY, List busyCells); + + /** + * Empty list is returned if no path exists. + * + * @return a list of cells from source (excl.) to target (incl.) while ignoring busyCells + */ + List findPath(int sourceX, int sourceY, int targetX, int targetY, NeighborFilteringOption neighborFilteringOption, List 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 index 0dadd445a8..c0b52c26b4 100644 --- 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 { private final AStarGrid grid; + private final Heuristic defaultHeuristic; + private final Heuristic diagonalHeuristic; + private boolean isCachingPaths = false; private Map> cache = new HashMap<>(); public AStarPathfinder(AStarGrid grid) { + this(grid, new ManhattanDistance<>(10), new OctileDistance<>()); + } + + public AStarPathfinder(AStarGrid grid, Heuristic defaultHeuristic, Heuristic diagonalHeuristic) { this.grid = grid; + this.defaultHeuristic = defaultHeuristic; + this.diagonalHeuristic = diagonalHeuristic; } public AStarGrid getGrid() { @@ -46,11 +61,21 @@ public List findPath(int sourceX, int sourceY, int targetX, int targe return findPath(grid.getData(), grid.get(sourceX, sourceY), grid.get(targetX, targetY)); } + @Override + public List 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 findPath(int sourceX, int sourceY, int targetX, int targetY, List busyCells) { return findPath(grid.getData(), grid.get(sourceX, sourceY), grid.get(targetX, targetY), busyCells.toArray(new AStarCell[0])); } + @Override + public List findPath(int sourceX, int sourceY, int targetX, int targetY, NeighborFilteringOption neighborFilteringOption, List 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 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 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 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 findPath(AStarCell[][] grid, AStarCell start, AStarCell t } } + Heuristic 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 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 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 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 getValidNeighbors(AStarCell node, AStarCell... busyNodes) { - var result = grid.getNeighbors(node.getX(), node.getY()); + private List 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 new file mode 100644 index 0000000000..27f2d3c552 --- /dev/null +++ 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 { + + 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 new file mode 100644 index 0000000000..13fe556525 --- /dev/null +++ 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 extends Heuristic { + + 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 new file mode 100644 index 0000000000..d87e579202 --- /dev/null +++ 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 extends Heuristic { + + 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 index 156ae5908c..694a7981a6 100644 --- 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 index cb19052ead..6a869b1b40 100644 --- 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 @@ -5,7 +5,10 @@ */ package com.almasb.fxgl.pathfinding.astar +import com.almasb.fxgl.core.collection.grid.NeighborFilteringOption import com.almasb.fxgl.pathfinding.CellState +import com.almasb.fxgl.pathfinding.heuristic.ManhattanDistance +import com.almasb.fxgl.pathfinding.heuristic.OctileDistance import org.hamcrest.MatcherAssert.assertThat import org.hamcrest.Matchers.`is` import org.junit.jupiter.api.Assertions.assertEquals @@ -17,11 +20,14 @@ import java.util.function.Supplier class AStarPathfinderTest { private lateinit var grid: AStarGrid private lateinit var pathfinder: AStarPathfinder + private lateinit var pathfinderHeuristics: AStarPathfinder @BeforeEach fun setUp() { grid = AStarGrid(GRID_SIZE, GRID_SIZE) pathfinder = AStarPathfinder(grid) + // For proofing - Making the Diagonal heuristic too high results in ignoring diagonals completely (Except for the last move) + pathfinderHeuristics = AStarPathfinder(grid, ManhattanDistance(10), OctileDistance(100)) } @Test @@ -46,12 +52,70 @@ class AStarPathfinderTest { 5, 1, 5, 0) + // Now test Diagonal Path Finding + path = pathfinder.findPath(3, 0, 5, 0, NeighborFilteringOption.EIGHT_DIRECTIONS) + assertPathEquals(path, + 3, 1, + 3, 2, + 3, 3, + 3, 4, + 4, 5, + 5, 4, + 5, 3, + 5, 2, + 5, 1, + 5, 0) + // Make passing impossible. for (i in 0..19) grid[4, i].state = CellState.NOT_WALKABLE path = pathfinder.findPath(3, 0, 5, 0) assertTrue(path.isEmpty()) } + @Test + fun testFindPathHeuristics() { + var path = pathfinderHeuristics.findPath(3, 0, 5, 0) + assertPathEquals(path, 4, 0, 5, 0) + + // Add barriers. + for (i in 0..4) grid[4, i].state = CellState.NOT_WALKABLE + path = pathfinderHeuristics.findPath(3, 0, 5, 0) + assertPathEquals(path, + 3, 1, + 3, 2, + 3, 3, + 3, 4, + 3, 5, + 4, 5, + 5, 5, + 5, 4, + 5, 3, + 5, 2, + 5, 1, + 5, 0) + + // Now test Diagonal Path Finding + path = pathfinderHeuristics.findPath(3, 0, 5, 0, NeighborFilteringOption.EIGHT_DIRECTIONS) + assertPathEquals(path, + 3, 1, + 3, 2, + 3, 3, + 3, 4, + 3, 5, + 4, 5, + 5, 5, + 5, 4, + 5, 3, + 5, 2, + 5, 1, + 5, 0) + + // Make passing impossible. + for (i in 0..19) grid[4, i].state = CellState.NOT_WALKABLE + path = pathfinderHeuristics.findPath(3, 0, 5, 0) + assertTrue(path.isEmpty()) + } + @Test fun testFindPathWithBusyCells() { grid[3, 0].state = CellState.NOT_WALKABLE @@ -60,7 +124,7 @@ class AStarPathfinderTest { grid[3, 3].state = CellState.NOT_WALKABLE grid[3, 5].state = CellState.NOT_WALKABLE grid[1, 4].state = CellState.NOT_WALKABLE - val path = pathfinder.findPath(1, 1, 4, 5, ArrayList()) + var path = pathfinder.findPath(1, 1, 4, 5, ArrayList()) assertThat(path.size, `is`(7)) @@ -69,7 +133,7 @@ class AStarPathfinderTest { assertThat(last.x, `is`(4)) assertThat(last.y, `is`(5)) - val pathWithBusyCell = pathfinder.findPath(1, 1, 4, 5, listOf(grid[3, 4])) + var pathWithBusyCell = pathfinder.findPath(1, 1, 4, 5, listOf(grid[3, 4])) assertThat(pathWithBusyCell.size, `is`(9)) @@ -77,6 +141,73 @@ class AStarPathfinderTest { assertThat(last.x, `is`(4)) assertThat(last.y, `is`(5)) + + + // Perform Diagonal Testing + path = pathfinder.findPath(1, 1, 4, 5, NeighborFilteringOption.EIGHT_DIRECTIONS, ArrayList()) + + assertThat(path.size, `is`(4)) + + last = path.last() + + assertThat(last.x, `is`(4)) + assertThat(last.y, `is`(5)) + + pathWithBusyCell = pathfinder.findPath(1, 1, 4, 5, NeighborFilteringOption.EIGHT_DIRECTIONS, listOf(grid[3, 4])) + + assertThat(pathWithBusyCell.size, `is`(6)) + + last = pathWithBusyCell.last() + + assertThat(last.x, `is`(4)) + assertThat(last.y, `is`(5)) + } + + @Test + fun testFindPathWithBusyCellsHeuristics() { + grid[3, 0].state = CellState.NOT_WALKABLE + grid[3, 1].state = CellState.NOT_WALKABLE + grid[3, 2].state = CellState.NOT_WALKABLE + grid[3, 3].state = CellState.NOT_WALKABLE + grid[3, 5].state = CellState.NOT_WALKABLE + grid[1, 4].state = CellState.NOT_WALKABLE + var path = pathfinderHeuristics.findPath(1, 1, 4, 5, ArrayList()) + + assertThat(path.size, `is`(7)) + + var last = path.last() + + assertThat(last.x, `is`(4)) + assertThat(last.y, `is`(5)) + + var pathWithBusyCell = pathfinderHeuristics.findPath(1, 1, 4, 5, listOf(grid[3, 4])) + + assertThat(pathWithBusyCell.size, `is`(9)) + + last = pathWithBusyCell.last() + + assertThat(last.x, `is`(4)) + assertThat(last.y, `is`(5)) + + + // Perform Diagonal Testing + path = pathfinderHeuristics.findPath(1, 1, 4, 5, NeighborFilteringOption.EIGHT_DIRECTIONS, ArrayList()) + + assertThat(path.size, `is`(6)) + + last = path.last() + + assertThat(last.x, `is`(4)) + assertThat(last.y, `is`(5)) + + pathWithBusyCell = pathfinderHeuristics.findPath(1, 1, 4, 5, NeighborFilteringOption.EIGHT_DIRECTIONS, listOf(grid[3, 4])) + + assertThat(pathWithBusyCell.size, `is`(8)) + + last = pathWithBusyCell.last() + + assertThat(last.x, `is`(4)) + assertThat(last.y, `is`(5)) } private fun assertPathEquals(path: List, vararg points: Int) {