From f11d73b61282ca6a2f2574e7ad3d7d46f9ca1f52 Mon Sep 17 00:00:00 2001 From: pacien Date: Sun, 14 Jan 2018 00:51:13 +0100 Subject: Implement path finder Signed-off-by: pacien --- src/docs/class.puml | 8 +- src/main/java/fr/umlv/java/wallj/board/Board.java | 14 +++ .../java/fr/umlv/java/wallj/board/PathFinder.java | 138 +++++++++++++++++++++ .../java/fr/umlv/java/wallj/board/TileVec2.java | 4 + .../fr/umlv/java/wallj/board/PathFinderTest.java | 60 +++++++++ src/test/resources/maps/island.txt | 10 ++ 6 files changed, 230 insertions(+), 4 deletions(-) create mode 100644 src/main/java/fr/umlv/java/wallj/board/PathFinder.java create mode 100644 src/test/java/fr/umlv/java/wallj/board/PathFinderTest.java create mode 100644 src/test/resources/maps/island.txt (limited to 'src') diff --git a/src/docs/class.puml b/src/docs/class.puml index 68aaf5e..869a707 100644 --- a/src/docs/class.puml +++ b/src/docs/class.puml @@ -3,10 +3,6 @@ skinparam linetype ortho package utils { - class PathFinder { - static List findPath(Board, TileVec2, TileVec2) - } - class Matrix { static int getWidth(...) static int getHeight(...) @@ -116,6 +112,10 @@ package board { TileVec2(col, row) Vec2 toPixelPos() } + + class PathFinder { + static List findPath(Board, TileVec2, TileVec2) + } } package model { diff --git a/src/main/java/fr/umlv/java/wallj/board/Board.java b/src/main/java/fr/umlv/java/wallj/board/Board.java index 015a4c0..112f7a5 100644 --- a/src/main/java/fr/umlv/java/wallj/board/Board.java +++ b/src/main/java/fr/umlv/java/wallj/board/Board.java @@ -3,7 +3,11 @@ package fr.umlv.java.wallj.board; import fr.umlv.java.wallj.model.BlockType; import fr.umlv.java.wallj.utils.Matrix; +import java.util.AbstractMap; import java.util.Arrays; +import java.util.Map; +import java.util.stream.IntStream; +import java.util.stream.Stream; /** * An immutable BlockType matrix. @@ -67,6 +71,16 @@ public final class Board { return TileVec2.of(Matrix.getWidth(map), Matrix.getHeight(map)); } + /** + * @return a stream of block types and their associated tile position vectors + */ + public Stream> stream() { + TileVec2 dim = getDim(); + return IntStream.range(0, dim.getRow() * dim.getCol()) + .mapToObj(i -> TileVec2.of(i % dim.getCol(), i / dim.getCol())) + .map(v -> new AbstractMap.SimpleImmutableEntry<>(v, getBlockTypeAt(v))); + } + @Override public boolean equals(Object o) { if (this == o) return true; diff --git a/src/main/java/fr/umlv/java/wallj/board/PathFinder.java b/src/main/java/fr/umlv/java/wallj/board/PathFinder.java new file mode 100644 index 0000000..9c509f3 --- /dev/null +++ b/src/main/java/fr/umlv/java/wallj/board/PathFinder.java @@ -0,0 +1,138 @@ +package fr.umlv.java.wallj.board; + +import java.util.*; +import java.util.function.BiFunction; +import java.util.stream.Collectors; + +/** + * Utility to find your way in this a-maze-ing world. + * + * @author Pacien TRAN-GIRARD + */ +public class PathFinder { + + private static final int LEAP_COST = 1; + private static final List NEIGHBOR_OFFSETS = Arrays.asList( + TileVec2.of(0, -1), + TileVec2.of(-1, 0), + TileVec2.of(0, 1), + TileVec2.of(1, 0)); + + private static class Node { + final T val; + final Map, Integer> neighbors; + + Node(T val) { + this.val = val; + this.neighbors = new HashMap<>(); + } + } + + private static class NodeSearchData { + final Node predecessor; + final double actualCost, estimatedCost; + + NodeSearchData(Node predecessor, double actualCost, double estimatedCost) { + this.predecessor = predecessor; + this.actualCost = actualCost; + this.estimatedCost = estimatedCost; + } + } + + private static double euclideanDistance(TileVec2 a, TileVec2 b) { + return Math.sqrt(Math.pow(a.getRow() - b.getRow(), 2) + + Math.pow(a.getCol() - b.getCol(), 2)); + } + + private static double cost(Map, NodeSearchData> searchData, Node node) { + return Optional.ofNullable(searchData.get(node)).map(n -> n.actualCost).orElse(Double.POSITIVE_INFINITY); + } + + private static Node predecessor(Map, NodeSearchData> searchData, Node node) { + return Optional.ofNullable(searchData.get(node)).map(n -> n.predecessor).orElse(null); + } + + private static List buildPath(Map, NodeSearchData> searchData, Node last) { + LinkedList path = new LinkedList<>(); + + for (Node node = last; node != null; node = predecessor(searchData, node)) + path.addFirst(node.val); + + return Collections.unmodifiableList(path); + } + + private static List findPath(Node start, T target, BiFunction heuristic) { + Map, NodeSearchData> searchData = new HashMap<>(); + TreeSet> discovered = new TreeSet<>(Comparator.comparingDouble(n -> searchData.get(n).estimatedCost)); + Set> visited = new HashSet<>(); + + searchData.put(start, new NodeSearchData<>(null, 0, heuristic.apply(start.val, target))); + discovered.add(start); + + Node current; + while (!discovered.isEmpty()) { + current = discovered.pollFirst(); + if (target.equals(current.val)) return buildPath(searchData, current); + + for (Map.Entry, Integer> neighborEntry : current.neighbors.entrySet()) { + if (visited.contains(neighborEntry.getKey())) continue; + + double challengeCost = cost(searchData, current) + neighborEntry.getValue(); + double currentCost = cost(searchData, neighborEntry.getKey()); + if (challengeCost < currentCost) + searchData.put(neighborEntry.getKey(), new NodeSearchData<>(current, challengeCost, + challengeCost + heuristic.apply(neighborEntry.getKey().val, target))); + + discovered.add(neighborEntry.getKey()); + } + + visited.add(current); + } + + throw new IllegalArgumentException("Destination target unreachable."); + } + + private static Map> buildGraph(Board b) { + Map> map = b.stream() + .filter(e -> e.getValue().isTraversable()) + .map(e -> new AbstractMap.SimpleEntry<>(e.getKey(), new Node<>(e.getKey()))) + .collect(Collectors.toMap(Map.Entry::getKey, Map.Entry::getValue)); + + for (Node node : map.values()) + NEIGHBOR_OFFSETS.stream() + .map(offsetVector -> map.get(node.val.add(offsetVector))) + .filter(Objects::nonNull) + .forEach(neighbor -> node.neighbors.put(neighbor, LEAP_COST)); + + return map; + } + + private final Map> graph; + + /** + * Builds a new path finder for the supplied board. + * A well-build (validated) board should be fully connected. + * + * @param board the board + */ + public PathFinder(Board board) { + graph = buildGraph(Objects.requireNonNull(board)); + } + + /** + * Returns a path from a starting point to a target if it exists, + * or throw an IllegalArgumentException if any of the given coordinates are invalid. + * The returned path may not be the same between executions. + * + * @param origin the origin coordinates + * @param target the target coordinates + * @return a path + * @implNote uses A* with euclidean distance heuristic + */ + public List findPath(TileVec2 origin, TileVec2 target) { + Node startNode = graph.get(origin); + if (startNode == null) throw new IllegalArgumentException("Invalid starting point."); + return findPath(startNode, target, PathFinder::euclideanDistance); + } + +} diff --git a/src/main/java/fr/umlv/java/wallj/board/TileVec2.java b/src/main/java/fr/umlv/java/wallj/board/TileVec2.java index b00c044..70beed9 100644 --- a/src/main/java/fr/umlv/java/wallj/board/TileVec2.java +++ b/src/main/java/fr/umlv/java/wallj/board/TileVec2.java @@ -58,6 +58,10 @@ public final class TileVec2 { return new Vec2(col * TILE_DIM, row * TILE_DIM); } + public TileVec2 add(TileVec2 v) { + return TileVec2.of(col + v.col, row + v.row); + } + @Override public boolean equals(Object o) { if (this == o) return true; diff --git a/src/test/java/fr/umlv/java/wallj/board/PathFinderTest.java b/src/test/java/fr/umlv/java/wallj/board/PathFinderTest.java new file mode 100644 index 0000000..3af4679 --- /dev/null +++ b/src/test/java/fr/umlv/java/wallj/board/PathFinderTest.java @@ -0,0 +1,60 @@ +package fr.umlv.java.wallj.board; + +import org.junit.jupiter.api.Assertions; +import org.junit.jupiter.api.Test; + +import java.io.IOException; +import java.net.URISyntaxException; +import java.nio.file.Path; +import java.nio.file.Paths; +import java.util.List; + +/** + * @author Pacien TRAN-GIRARD + */ +final class PathFinderTest { + + private Path getResourcePath(String str) throws URISyntaxException { + return Paths.get(getClass().getResource(str).toURI()); + } + + private boolean isPathConnected(List path) { + for (int i = 1; i < path.size(); ++i) { + TileVec2 predecessor = path.get(i - 1), current = path.get(i); + + if (Math.abs(predecessor.getCol() - current.getCol()) > 1 || + Math.abs(predecessor.getRow() - current.getRow()) > 1) return false; + + if (Math.abs(predecessor.getCol() - current.getCol()) == 1 && + Math.abs(predecessor.getRow() - current.getRow()) == 1) return false; + } + + return true; + } + + @Test + void testFailImpossibleFindPath() throws URISyntaxException, IOException { + Board board = BoardParser.parse(getResourcePath("/maps/island.txt")); + PathFinder pathFinder = new PathFinder(board); + + Assertions.assertThrows(IllegalArgumentException.class, () -> { + pathFinder.findPath(TileVec2.of(7, 3), TileVec2.of(16, 5)); // into a wall + }); + } + + @Test + void testFindPath() throws URISyntaxException, IOException { + Board board = BoardParser.parse(getResourcePath("/maps/island.txt")); + PathFinder pathFinder = new PathFinder(board); + + TileVec2 origin = TileVec2.of(7, 3); + TileVec2 target = TileVec2.of(33, 4); + List path = pathFinder.findPath(origin, target); + + Assertions.assertEquals(path.get(0), origin); + Assertions.assertEquals(path.get(path.size() - 1), target); + Assertions.assertTrue(isPathConnected(path)); + Assertions.assertTrue(path.stream().allMatch(v -> board.getBlockTypeAt(v).isTraversable())); + } + +} diff --git a/src/test/resources/maps/island.txt b/src/test/resources/maps/island.txt new file mode 100644 index 0000000..9febab7 --- /dev/null +++ b/src/test/resources/maps/island.txt @@ -0,0 +1,10 @@ +WWWWWWWWWWWWWWWWWWWWWWWWWWWWWWWWWWWWWWW +W W +W W +W W W +W WWWWW W +W WWWWWWW W +W WWWWWWW W +W WWW W +W W +WWWWWWWWWWWWWWWWWWWWWWWWWWWWWWWWWWWWWWW \ No newline at end of file -- cgit v1.2.3