1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
|
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 class Node<T> {
final T val;
final Map<Node<T>, Integer> neighbors;
Node(T val) {
this.val = val;
this.neighbors = new HashMap<>();
}
}
private static class NodeSearchData<T> {
final Node<T> predecessor;
final double actualCost, estimatedCost;
NodeSearchData(Node<T> 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 <T> double cost(Map<Node<T>, NodeSearchData<T>> searchData, Node<T> node) {
return Optional.ofNullable(searchData.get(node)).map(n -> n.actualCost).orElse(Double.POSITIVE_INFINITY);
}
private static <T> Node<T> predecessor(Map<Node<T>, NodeSearchData<T>> searchData, Node<T> node) {
return Optional.ofNullable(searchData.get(node)).map(n -> n.predecessor).orElse(null);
}
private static <T> List<T> buildPath(Map<Node<T>, NodeSearchData<T>> searchData, Node<T> last) {
LinkedList<T> path = new LinkedList<>();
for (Node<T> node = last; node != null; node = predecessor(searchData, node))
path.addFirst(node.val);
return Collections.unmodifiableList(path);
}
private static <T> List<T> findPath(Node<T> start, T target, BiFunction<T, T, Double> heuristic) {
Map<Node<T>, NodeSearchData<T>> searchData = new HashMap<>();
Queue<Node<T>> discovered = new PriorityQueue<>(Comparator.comparingDouble(n -> searchData.get(n).estimatedCost));
Set<Node<T>> visited = new HashSet<>();
searchData.put(start, new NodeSearchData<>(null, 0, heuristic.apply(start.val, target)));
discovered.add(start);
Node<T> current;
while (!discovered.isEmpty()) {
current = discovered.poll();
if (target.equals(current.val)) return buildPath(searchData, current);
for (Map.Entry<Node<T>, 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<TileVec2, Node<TileVec2>> buildGraph(Board b) {
Map<TileVec2, Node<TileVec2>> 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<TileVec2> node : map.values())
node.val.neighbors().stream()
.map(map::get)
.filter(Objects::nonNull)
.forEach(neighbor -> node.neighbors.put(neighbor, LEAP_COST));
return map;
}
private final Map<TileVec2, Node<TileVec2>> 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 from the origin to the target position
* @implNote uses A* with euclidean distance heuristic
*/
public List<TileVec2> findPath(TileVec2 origin, TileVec2 target) {
Node<TileVec2> startNode = graph.get(origin);
if (startNode == null) throw new IllegalArgumentException("Invalid starting point.");
return findPath(startNode, target, PathFinder::euclideanDistance);
}
}
|