From e1252f1638102071406318c718416f7e3cf7f47b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bart=C5=82omiej=20Przemys=C5=82aw=20Pluta?= Date: Sun, 7 Mar 2021 11:05:42 +0100 Subject: [PATCH] Create PathFinder interface --- .../base/api/game/ai/pathfinding/Astar.java | 212 ++++++++++++++ .../api/game/ai/pathfinding/PathFinder.java | 10 + .../base/engine/pathfinding/Astar.java | 273 ------------------ 3 files changed, 222 insertions(+), 273 deletions(-) create mode 100644 api/src/main/java/com/bartlomiejpluta/base/api/game/ai/pathfinding/Astar.java create mode 100644 api/src/main/java/com/bartlomiejpluta/base/api/game/ai/pathfinding/PathFinder.java delete mode 100644 engine/src/main/java/com/bartlomiejpluta/base/engine/pathfinding/Astar.java diff --git a/api/src/main/java/com/bartlomiejpluta/base/api/game/ai/pathfinding/Astar.java b/api/src/main/java/com/bartlomiejpluta/base/api/game/ai/pathfinding/Astar.java new file mode 100644 index 00000000..9906f0b4 --- /dev/null +++ b/api/src/main/java/com/bartlomiejpluta/base/api/game/ai/pathfinding/Astar.java @@ -0,0 +1,212 @@ +package com.bartlomiejpluta.base.api.game.ai.pathfinding; + +import com.bartlomiejpluta.base.api.game.map.layer.object.ObjectLayer; +import com.bartlomiejpluta.base.api.game.map.layer.object.PassageAbility; +import org.joml.Vector2i; + +import java.util.*; +import java.util.function.Function; + +import static java.lang.Math.abs; + +@SuppressWarnings({"RedundantCast", "rawtypes"}) +public class Astar implements PathFinder { + + /* + * We are interested in following adjacent + * +---+---+---+ + * | | o | | + * +---+---+---+ + * | o | x | o | + * +---+---+---+ + * | | o | | + * +---+---+---+ + */ + private static final Vector2i[] ADJACENT = new Vector2i[]{ + new Vector2i(-1, 0), + new Vector2i(0, -1), + new Vector2i(1, 0), + new Vector2i(0, 1) + }; + + @Override + public List findPath(ObjectLayer layer, Vector2i start, Vector2i end, int range) { + int columns = layer.getMap().getColumns(); + int rows = layer.getMap().getRows(); + + Node startNode = new Node(start); + Node endNode = new Node(end); + + // The heuristic function defined as Manhattan distance to the end node + Function h = createManhattanDistanceHeuristic(endNode); + + // The start node has the actual cost 0 and estimated is a Manhattan distance to the end node + startNode.g = 0.0f; + startNode.f = (Float) h.apply(startNode); + + // We are starting with one open node (the start one) end empty closed lists + Queue open = new PriorityQueue(); + List closed = new LinkedList(); + open.add(startNode); + + // As long as there are at least one open node + while (!open.isEmpty()) { + + // We are retrieving the node with the **smallest** f score + // (That's the way the Astar.compare() comparator works) + // And the same time we are removing the node from open list + // and pushing it to closed one as we no longer need to analyze this node + Node current = (Node) open.poll(); + closed.add(current); + + // If we found the node with f score and it is + // actually an end node, we have most likely found a best path + if (current.equals(endNode)) { + return recreatePath(current); + } + + adjacent: + // For each node neighbour + // (we are analyzing the 4 neighbours, + // as described in the commend above ADJACENT static field) + for (Vector2i adjacent : ADJACENT) { + Vector2i position = new Vector2i(current.position).add(adjacent); + + // We are getting rid the neighbours beyond the map + if (position.x < 0 || position.x >= columns || position.y < 0 || position.y >= rows) { + continue; + } + + // We are limiting the algorithm to given range + // If current neighbour distance to start node exceeds given range parameter + // we are getting rid of this neighbour + if (manhattanDistance(startNode.position, position) > range) { + continue; + } + + // Define new neighbour + Node neighbour = new Node(position); + + // If we already analyzed this node, + // we are free to skip it to not analyze it once again + for (Object closedNode : closed) { + if (((Node) closedNode).position.equals(position)) { + continue adjacent; + } + } + + // Get rid of nodes that are not reachable (blocked or something is staying on there) + boolean reachable = layer.getPassageMap()[position.y][position.x] == PassageAbility.ALLOW; + if (!reachable) { + continue; + } + + // We are evaluating the basic A* parameters + // as well as the parent node which is needed to recreate + // path further + neighbour.parent = current; + neighbour.g = current.g + 1; + neighbour.f = neighbour.g + (Float) h.apply(neighbour); + + // If the node already exists in open list, + // we need to compare current neighbour with existing node + // to check which path is shorter. + // If the neighbour is shorter, we can update the existing node + // with neighbour's parameters + for (Object openNode : open) { + Node node = (Node) openNode; + if (node.position.equals(position) && neighbour.g < node.g) { + node.g = neighbour.g; + node.parent = current; + continue adjacent; + } + } + + // Push neighbour to open list to consider it later + open.add(neighbour); + } + } + + // If open list is empty and we didn't reach the end node + // it means that the path probably does not exist at all + return new LinkedList<>(); + } + + @SuppressWarnings("Convert2Lambda") + private Function createManhattanDistanceHeuristic(final Node toNode) { + return new Function() { + + @Override + public Object apply(Object node) { + return manhattanDistance(toNode.position, ((Node) node).position); + } + }; + } + + private float manhattanDistance(Vector2i a, Vector2i b) { + return (abs(a.x - b.x) + abs(a.y - b.y)); + } + + private List recreatePath(Node node) { + Node current = node; + List list = new LinkedList<>(); + list.add(((Node) node).position); + + while (current.parent != null) { + list.add(((Node) current).parent.position); + current = current.parent; + } + + return list; + } + + public void print(ObjectLayer layer, Iterable nodes) { + for (int row = 0; row < layer.getMap().getRows(); ++row) { + System.out.print("|"); + + tiles: + for (int column = 0; column < layer.getMap().getColumns(); ++column) { + + for (Object node : nodes) { + if (((Vector2i) node).equals(column, row)) { + System.out.print("#"); + continue tiles; + } + } + + System.out.print(layer.getPassageMap()[row][column] == PassageAbility.ALLOW ? " " : "."); + } + + System.out.println("|"); + } + } + + private static class Node implements Comparable { + public Node parent; + public final Vector2i position; + public float g = 0.0f; + public float f = 0.0f; + + public Node(Vector2i position) { + this.position = position; + } + + @Override + public boolean equals(Object o) { + if (this == o) return true; + if (o == null || getClass() != o.getClass()) return false; + Node node = (Node) o; + return position.equals(node.position); + } + + @Override + public int hashCode() { + return Objects.hash(position); + } + + @Override + public int compareTo(Object o) { + return Float.compare(f, ((Node) o).f); + } + } +} diff --git a/api/src/main/java/com/bartlomiejpluta/base/api/game/ai/pathfinding/PathFinder.java b/api/src/main/java/com/bartlomiejpluta/base/api/game/ai/pathfinding/PathFinder.java new file mode 100644 index 00000000..d4c962cb --- /dev/null +++ b/api/src/main/java/com/bartlomiejpluta/base/api/game/ai/pathfinding/PathFinder.java @@ -0,0 +1,10 @@ +package com.bartlomiejpluta.base.api.game.ai.pathfinding; + +import com.bartlomiejpluta.base.api.game.map.layer.object.ObjectLayer; +import org.joml.Vector2i; + +import java.util.List; + +public interface PathFinder { + List findPath(ObjectLayer layer, Vector2i start, Vector2i end, int range); +} diff --git a/engine/src/main/java/com/bartlomiejpluta/base/engine/pathfinding/Astar.java b/engine/src/main/java/com/bartlomiejpluta/base/engine/pathfinding/Astar.java deleted file mode 100644 index 5db2caf9..00000000 --- a/engine/src/main/java/com/bartlomiejpluta/base/engine/pathfinding/Astar.java +++ /dev/null @@ -1,273 +0,0 @@ -package com.bartlomiejpluta.base.engine.pathfinding; - -import com.bartlomiejpluta.base.api.game.map.layer.color.ColorLayer; -import com.bartlomiejpluta.base.api.game.map.layer.image.ImageLayer; -import com.bartlomiejpluta.base.api.game.map.layer.object.ObjectLayer; -import com.bartlomiejpluta.base.api.game.map.layer.object.PassageAbility; -import com.bartlomiejpluta.base.api.game.map.layer.tile.TileLayer; -import com.bartlomiejpluta.base.api.game.map.model.GameMap; -import com.bartlomiejpluta.base.engine.world.map.layer.object.DefaultObjectLayer; -import lombok.AllArgsConstructor; -import lombok.EqualsAndHashCode; -import lombok.RequiredArgsConstructor; -import org.joml.Vector2f; -import org.joml.Vector2i; - -import java.util.*; -import java.util.function.Function; - -import static java.lang.Math.abs; - -@SuppressWarnings("ConstantConditions") -@AllArgsConstructor -public class Astar implements Comparator { - - /* - * We are interested in following adjacent - * +---+---+---+ - * | | o | | - * +---+---+---+ - * | o | x | o | - * +---+---+---+ - * | | o | | - * +---+---+---+ - */ - private static final Vector2i[] ADJACENT = new Vector2i[]{ - new Vector2i(-1, 0), - new Vector2i(0, -1), - new Vector2i(1, 0), - new Vector2i(0, 1) - }; - - public List findPath(ObjectLayer layer, Node start, Node end) { - int columns = layer.getMap().getColumns(); - int rows = layer.getMap().getRows(); - - // The heuristic function defined as Manhattan distance to the end node - var h = createManhattanDistanceHeuristic(end); - - // The start node has the actual cost 0 and estimated is a Manhattan distance to the end node - start.g = 0.0f; - start.f = h.apply(start); - - // We are starting with one open node (the start one) end empty closed lists - var open = new PriorityQueue<>(this); - var closed = new LinkedList(); - open.add(start); - - // As long as there are at least one open node - while (!open.isEmpty()) { - - // We are retrieving the node with the **smallest** f score - // (That's the way the Astar.compare() comparator works) - // And the same time we are removing the node from open list - // and pushing it to closed one as we no longer need to analyze this node - var current = open.poll(); - closed.add(current); - - // If we found the node with f score and it is - // actually an end node, we have most likely found a best path - if (current.equals(end)) { - return recreatePath(current); - } - - adjacent: - // For each node neighbour - // (we are analyzing the 4 neighbours, - // as described in the commend above ADJACENT static field) - for (var adjacent : ADJACENT) { - var position = new Vector2i(current.position).add(adjacent); - - // We are getting rid the neighbours beyond the map - if (position.x < 0 || position.x >= columns || position.y < 0 || position.y >= rows) { - continue; - } - - // Define new neighbour - var neighbour = new Node(position); - - // If we already analyzed this node, - // we are free to skip it to not analyze it once again - for (var closedNode : closed) { - if (closedNode.position.equals(position)) { - continue adjacent; - } - } - - // Get rid of nodes that are not reachable (blocked or something is staying on there) - var reachable = layer.getPassageMap()[position.y][position.x] == PassageAbility.ALLOW; - if (!reachable) { - continue; - } - - // We are evaluating the basic A* parameters - // as well as the parent node which is needed to recreate - // path further - neighbour.parent = current; - neighbour.g = current.g + 1; - neighbour.f = neighbour.g + h.apply(neighbour); - - // If the node already exists in open list, - // we need to compare current neighbour with existing node - // to check which path is shorter. - // If the neighbour is shorter, we can update the existing node - // with neighbour's parameters - for (var openNode : open) { - if (openNode.position.equals(position) && neighbour.g < openNode.g) { - openNode.g = neighbour.g; - openNode.parent = current; - continue adjacent; - } - } - - // Push neighbour to open list to consider it later - open.add(neighbour); - } - } - - // If open list is empty and we didn't reach the end node - // it means that the path probably does not exist at all - return Collections.emptyList(); - } - - @SuppressWarnings("Convert2Lambda") - private Function createManhattanDistanceHeuristic(Node toNode) { - return new Function<>() { - - @Override - public Float apply(Node node) { - return (float) (abs(toNode.position.x - node.position.x) + abs(toNode.position.y - node.position.y)); - } - }; - } - - private List recreatePath(Node node) { - var current = node; - var list = new ArrayList(); - list.add(node); - - while (current.parent != null) { - list.add(current.parent); - current = current.parent; - } - - return list; - } - - private void print(ObjectLayer layer, Iterable nodes) { - for (int row = 0; row < layer.getMap().getRows(); ++row) { - System.out.print("|"); - - tiles: - for (int column = 0; column < layer.getMap().getColumns(); ++column) { - - for (var node : nodes) { - if (node.position.equals(column, row)) { - System.out.print(" # "); - continue tiles; - } - } - - System.out.print(layer.getPassageMap()[row][column] == PassageAbility.ALLOW ? " " : " . "); - } - - System.out.println("|"); - } - } - - @Override - public int compare(Node o1, Node o2) { - return Float.compare(o1.f, o2.f); - } - - @EqualsAndHashCode(of = "position") - @RequiredArgsConstructor - protected static class Node { - public Node parent; - public final Vector2i position; - public float g = 0.0f; - public float f = 0.0f; - } - - public static void main(String[] args) { - final int rows = 50; - final int columns = 50; - final int threshold = 70; - var start = new Vector2i(1, 1); - var end = new Vector2i(49, 49); - - - final Random random = new Random(); - final Vector2f stepSize = new Vector2f(32, 32); - - var passageMap = new PassageAbility[rows][columns]; - for (int i = 0; i < rows; ++i) { - passageMap[i] = new PassageAbility[columns]; - - for (int j = 0; j < columns; ++j) { - passageMap[i][j] = random.nextInt(100) >= threshold ? PassageAbility.BLOCK : PassageAbility.ALLOW; - } - } - - var map = new GameMap() { - - @Override - public float getWidth() { - return 0; - } - - @Override - public float getHeight() { - return 0; - } - - @Override - public int getRows() { - return rows; - } - - @Override - public int getColumns() { - return columns; - } - - @Override - public Vector2f getSize() { - return null; - } - - @Override - public TileLayer getTileLayer(int layerIndex) { - return null; - } - - @Override - public ImageLayer getImageLayer(int layerIndex) { - return null; - } - - @Override - public ColorLayer getColorLayer(int layerIndex) { - return null; - } - - @Override - public ObjectLayer getObjectLayer(int layerIndex) { - return null; - } - }; - - var layer = new DefaultObjectLayer(map, rows, columns, stepSize, new ArrayList<>(), passageMap); - - var astar = new Astar(); - - passageMap[start.y][start.x] = PassageAbility.ALLOW; - passageMap[end.y][end.x] = PassageAbility.ALLOW; - - var time = System.currentTimeMillis(); - var output = astar.findPath(layer, new Node(start), new Node(end)); - var elapsed = System.currentTimeMillis() - time; - astar.print(layer, output); - System.out.println("Time: " + elapsed + "ms"); - } -}