- Notifications
You must be signed in to change notification settings - Fork 1.9k
/
Copy pathAStar.java
137 lines (119 loc) · 5.28 KB
/
AStar.java
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
132
133
134
135
136
137
packagecom.jwetherell.algorithms.graph;
importjava.util.ArrayList;
importjava.util.Collections;
importjava.util.Comparator;
importjava.util.HashMap;
importjava.util.HashSet;
importjava.util.List;
importjava.util.Map;
importjava.util.Set;
importcom.jwetherell.algorithms.data_structures.Graph;
importcom.jwetherell.algorithms.data_structures.Graph.Edge;
importcom.jwetherell.algorithms.data_structures.Graph.Vertex;
/**
* In computer science, A* is a computer algorithm that is widely used in path finding and graph traversal, the process
* of plotting an efficiently traversable path between multiple points, called nodes.
* <p>
* @see <a href="https://en.wikipedia.org/wiki/A*_search_algorithm">A* Algorithm (Wikipedia)</a>
* <br>
* @author Justin Wetherell <phishman3579@gmail.com>
*/
publicclassAStar<TextendsComparable<T>> {
publicAStar() { }
/**
* Find the path using the A* algorithm from start vertex to end vertex or NULL if no path exists.
*
* @param graph
* Graph to search.
* @param start
* Start vertex.
* @param goal
* Goal vertex.
*
* @return
* List of Edges to get from start to end or NULL if no path exists.
*/
publicList<Graph.Edge<T>> aStar(Graph<T> graph, Graph.Vertex<T> start, Graph.Vertex<T> goal) {
finalintsize = graph.getVertices().size(); // used to size data structures appropriately
finalSet<Graph.Vertex<T>> closedSet = newHashSet<Graph.Vertex<T>>(size); // The set of nodes already evaluated.
finalList<Graph.Vertex<T>> openSet = newArrayList<Graph.Vertex<T>>(size); // The set of tentative nodes to be evaluated, initially containing the start node
openSet.add(start);
finalMap<Graph.Vertex<T>,Graph.Vertex<T>> cameFrom = newHashMap<Graph.Vertex<T>,Graph.Vertex<T>>(size); // The map of navigated nodes.
finalMap<Graph.Vertex<T>,Integer> gScore = newHashMap<Graph.Vertex<T>,Integer>(); // Cost from start along best known path.
gScore.put(start, 0);
// Estimated total cost from start to goal through y.
finalMap<Graph.Vertex<T>,Integer> fScore = newHashMap<Graph.Vertex<T>,Integer>();
for (Graph.Vertex<T> v : graph.getVertices())
fScore.put(v, Integer.MAX_VALUE);
fScore.put(start, heuristicCostEstimate(start,goal));
finalComparator<Graph.Vertex<T>> comparator = newComparator<Graph.Vertex<T>>() {
/**
* {@inheritDoc}
*/
@Override
publicintcompare(Vertex<T> o1, Vertex<T> o2) {
if (fScore.get(o1) < fScore.get(o2))
return -1;
if (fScore.get(o2) < fScore.get(o1))
return1;
return0;
}
};
while (!openSet.isEmpty()) {
finalGraph.Vertex<T> current = openSet.get(0);
if (current.equals(goal))
returnreconstructPath(cameFrom, goal);
openSet.remove(0);
closedSet.add(current);
for (Graph.Edge<T> edge : current.getEdges()) {
finalGraph.Vertex<T> neighbor = edge.getToVertex();
if (closedSet.contains(neighbor))
continue; // Ignore the neighbor which is already evaluated.
finalinttenativeGScore = gScore.get(current) + distanceBetween(current,neighbor); // length of this path.
if (!openSet.contains(neighbor))
openSet.add(neighbor); // Discover a new node
elseif (tenativeGScore >= gScore.get(neighbor))
continue;
// This path is the best until now. Record it!
cameFrom.put(neighbor, current);
gScore.put(neighbor, tenativeGScore);
finalintestimatedFScore = gScore.get(neighbor) + heuristicCostEstimate(neighbor, goal);
fScore.put(neighbor, estimatedFScore);
// fScore has changed, re-sort the list
Collections.sort(openSet,comparator);
}
}
returnnull;
}
/**
* Default distance is the edge cost. If there is no edge between the start and next then
* it returns Integer.MAX_VALUE;
*/
protectedintdistanceBetween(Graph.Vertex<T> start, Graph.Vertex<T> next) {
for (Edge<T> e : start.getEdges()) {
if (e.getToVertex().equals(next))
returne.getCost();
}
returnInteger.MAX_VALUE;
}
/**
* Default heuristic: cost to each vertex is 1.
*/
@SuppressWarnings("unused")
protectedintheuristicCostEstimate(Graph.Vertex<T> start, Graph.Vertex<T> goal) {
return1;
}
privateList<Graph.Edge<T>> reconstructPath(Map<Graph.Vertex<T>,Graph.Vertex<T>> cameFrom, Graph.Vertex<T> current) {
finalList<Graph.Edge<T>> totalPath = newArrayList<Graph.Edge<T>>();
while (current != null) {
finalGraph.Vertex<T> previous = current;
current = cameFrom.get(current);
if (current != null) {
finalGraph.Edge<T> edge = current.getEdge(previous);
totalPath.add(edge);
}
}
Collections.reverse(totalPath);
returntotalPath;
}
}