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/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 + 3 files changed, 156 insertions(+) create mode 100644 src/main/java/fr/umlv/java/wallj/board/PathFinder.java (limited to 'src/main/java/fr') 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; -- cgit v1.2.3