1 package cz.cuni.amis.utils.astar;
2
3 import java.util.Collection;
4 import java.util.Iterator;
5
6 import cz.cuni.amis.utils.SafeEquals;
7
8 /**
9 * ========================================================
10 * This file holds implementation of generic A* algorithm,
11 * better refered to as A* Machine according to
12 * Dan Higgins, Generic A* Pathfind, AI Gaming Wisdom, 2002
13 * ========================================================
14 * <p><p>
15 *
16 * What is A*<p>
17 * ----------<p>
18 * A* is space-search algorithm using a custom-built heuristic. It's an improved
19 * version of well-known Dijkstra algorithm which is used to find the shortest
20 * path in weighted graphs. Instead of picking the node with the smallest path
21 * from the start node it chooses node which seems to be on the shortest path
22 * to the goal node (and this guess is based on provided heuristic).
23 * <p><p>
24 * Note<p>
25 * ----<p>
26 * Insted of weights we speak about cost of the edges.
27 * <p><p>
28 * Limitation of A*<p>
29 * ----------------<p>
30 * 1) A* doesn't work over graphs with negative edge costs.<p>
31 * 2) heuristic has to be correct -> it has to be lower estimation of the cost to
32 * the goal node (in 2D, 3D an euklidian metric will do the job).<p>
33 * <p>
34 * First we have to specify some interfaces for A*<p>
35 * -----------------------------------------------<p>
36 * we will need a few things: <p>
37 * Open List Class<p>
38 * Close List Class<p>
39 * Goal (which can tell us about extra costs, when work is done, etc)<p>
40 * Map (which tells us about travel cost between nodes and can return
41 * node's neighbours)<p>
42 * <p>
43 * Note about Nodes<p>
44 * ----------------<p>
45 * Note that we don't need to have a Node interface so you're free to have
46 * any nodes you want (POJOs). But implementation of A* requires the nodes to have
47 * hashCode() and equals() implemented correctly, which should be a good practice!
48 * (Note that also means you can't have two nodes which are equals in the map!)
49 * <p><p>
50 * Idea behind AStarGoal / AStarMap<p>
51 * --------------------------------<p>
52 * Usually you will have only one world / state space representation but you need to
53 * change the cost of edges between nodes according to let say creature for which you
54 * search the path.
55 * <p><p>
56 * Imagine the situation with the lake / human / fish.
57 * Human may swim across the lake but it's faster to run around it (so you need to give the edges between
58 * water tiles an extra cost).<p>
59 * Fish can swim really fast but can't get out of the water (so you need to forbid tiles around the lake
60 * and give the edges between the lakes' tiles an negative extra cost).<p>
61 * <p>
62 * So the AStarMap will represent the world with the lake with default cost of the edges.
63 * AStarGoal may change the edges cost / forbid some nodes completely. So you will
64 * implement one goal for a human and another for a fish.
65 * <p><p>
66 * Note about the speed<p>
67 * --------------------<p>
68 * Speed of algorithm is based upon the speed of AStarOpenList and AStarCloseList.
69 */
70 public class AStar<NODE> {
71
72 /**
73 * Method performing an AStar search over graph defined inside {@link AStarMap} starting from 'start' node driving
74 * itself towards goal that is described by {@link AStarGoal}. Note that {@link AStarGoal} also contains a heuristic {@link AStarHeuristic}.
75 * <p><p>
76 * {@link AStarMap} provides informations about node neighbours and edge costs,
77 * while {@link AStarGoal} contains the definition of goal node and extra cost / extra info
78 * about map nodes.
79 * <p><p>
80 * You may also specify maxIterations - "how long the A* should search" equals
81 * to number of evaluated nodes. If it's 0 then A* won't even start! If it is < 0 it will run until the 'goal' is found
82 * all nodes are evaluated.
83 *
84 * @param map
85 * @param start
86 * @param goal
87 * @param iterationsMax maximum of iterations to be made by algorithm during the search (negative number == infinite)
88 */
89 public static <NODE> AStarResult<NODE> aStar(AStarMap<NODE> map, NODE start, AStarGoal<NODE> goal, long iterationsMax){
90
91 // NOTE: values of the estimated cost is maintained in AStarResult
92 // AS HEAP: we're using AStarHeap with AStarHeapComparator which is
93 // using data from AStarResult.estimatedCost ...
94 // that means you have to first alter AStarResult.estimatedCost
95 // before adding / decreasing key in AStarHeap
96
97 AStarResult<NODE> result = new AStarResult<NODE>();
98
99 AStarHeap<NODE> open = new AStarHeap<NODE>(new AStarHeapComparator<NODE>(result.estimatedCost), 64);
100 result.openList = open;
101 Collection<NODE> close = result.closeList;
102
103 goal.setCloseList(result.closeList);
104 goal.setOpenList(result.openList);
105
106 result.startNode = start;
107
108 result.putCostToNode(result.startNode, 0);
109 result.putEstimatedCostToNode(result.startNode, goal.getEstimatedDistanceToGoal(result.startNode));
110 open.add(result.startNode);
111
112 NODE node, nextNode;
113 Collection<NODE> neighbours;
114 Iterator<NODE> nodeIter;
115 int nodePathCost, nextNodePathCost, travelCost, extraCost, estimatedPathCost,
116 newNextNodePathCost;
117
118 while ((!open.empty()) &&
119 ((iterationsMax <= 0) || (result.interations < iterationsMax))
120 ){
121 ++result.interations; // new iteratrion begin
122
123 node = open.getMin();
124
125 if (node == null){ // failure
126 result.success = false;
127 break;
128 }
129
130 open.deleteMin();
131
132 if (goal.isGoalReached(node)){ // we've reached the goal HURRAY!
133 result.goalNode = node;
134 result.success = true;
135 break;
136 }
137
138 nodePathCost = result.getCostToNode(node);
139
140 neighbours = map.getNodeNeighbours(node);
141 nodeIter = neighbours.iterator();
142
143 while (nodeIter.hasNext()){
144 nextNode = nodeIter.next();
145 // iterate over all of the neighbours node
146 // and evaluate them one by one
147
148 if (!goal.isNodeOpened(nextNode)){ // stepping to this node is forbidden, skip it
149 continue;
150 }
151
152 travelCost = map.getEdgeCost(node, nextNode);
153 extraCost = goal.getExtraCost(node, nextNode);
154
155 nextNodePathCost = result.getCostToNode(nextNode);
156 if (nextNodePathCost == -1){
157 // we've never touched nextNode
158 nextNodePathCost = nodePathCost + travelCost + extraCost;
159 if (nextNodePathCost < 0) nextNodePathCost = 0;
160 result.putCostToNode(nextNode, nextNodePathCost);
161 result.putPreviousNode(nextNode, node);
162
163 estimatedPathCost = nextNodePathCost + goal.getEstimatedDistanceToGoal(nextNode);
164 result.putEstimatedCostToNode(nextNode, estimatedPathCost);
165
166 open.add(nextNode);
167 continue;
168 } else {
169 // we've already touched the nextNode
170 newNextNodePathCost = nodePathCost + travelCost + extraCost;
171 if (newNextNodePathCost < 0) newNextNodePathCost = 0;
172 if (newNextNodePathCost < nextNodePathCost){
173 estimatedPathCost = newNextNodePathCost + goal.getEstimatedDistanceToGoal(nextNode);
174 result.putCostToNode(nextNode, newNextNodePathCost);
175 result.putEstimatedCostToNode(nextNode, estimatedPathCost);
176 if (close.contains(nextNode)){
177 close.remove(nextNode);
178 open.add(nextNode);
179 } else
180 if (open.contains(nextNode)){
181 open.decreaseKey(node);
182 } else {
183 open.add(nextNode);
184 }
185 }
186 // if estimatedCost is higher or equal, we don't have to take any actions
187 continue;
188 }
189 }
190 close.add(node);
191 }
192
193 return result;
194 }
195
196 /**
197 * Method performing an AStar search over graph defined inside {@link AStarMap} starting from 'start' node driving
198 * itself towards goal that is described by {@link AStarGoal}. Note that {@link AStarGoal} also contains a heuristic {@link AStarHeuristic}.
199 * <p><p>
200 * {@link AStarMap} provides informations about node neighbours and edge costs,
201 * while {@link AStarGoal} contains the definition of goal node and extra cost / extra info
202 * about map nodes.
203 * <p><p>
204 * This method performs 'unbounded' AStar search, i.e., it does not limit a number of iterations the algorithm will perform.
205 *
206 * @param <NODE>
207 * @param map
208 * @param start
209 * @param goal
210 * @return
211 */
212 public static <NODE> AStarResult<NODE> aStar(final AStarMap<NODE> map, final NODE start, final AStarGoal<NODE> goal) {
213 return aStar(map, start, goal, -1);
214 }
215
216 /**
217 * Method performing an AStar search over graph defined inside {@link AStarMap} starting from 'start' node driving
218 * itself towards 'goal' using heuristic and extra costs defined by {@link AStarEvaluator}.
219 * <p><p>
220 * {@link AStarMap} provides informations about node neighbours and edge costs,
221 * while {@link AStarEvaluator} contains definition of the heuristic + extra edge info about map nodes.
222 * <p><p>
223 * You may also specify maxIterations - "how long the A* should search" equals
224 * to number of evaluated nodes. If it's 0 then A* won't even start! If it is < 0 it will run until the 'goal' is found
225 * all nodes are evaluated.
226 * <p><p>
227 * <b>WARNING</b>: Class that is used for NODE must have correctly defined {@link Object#equals(Object)} because it will be used
228 * to recognized whether the current evaluated node is the same as the goal or not!
229 *
230 * @param <NODE>
231 * @param map
232 * @param evaluator
233 * @param start
234 * @param goal
235 * @param maxIterations maximum of iterations to be made by algorithm during the search (negative number == infinite)
236 * @return
237 */
238 public static <NODE> AStarResult<NODE> aStar(final AStarMap<NODE> map, final AStarEvaluator<NODE> evaluator, final NODE start, final NODE goal, int maxIterations) {
239 return
240 aStar(
241 map,
242 start,
243 new AStarGoal<NODE>() {
244
245 @Override
246 public boolean isGoalReached(NODE actualNode) {
247 return SafeEquals.equals(actualNode, goal);
248 }
249
250 @Override
251 public void setCloseList(Collection<NODE> closeList) {
252 // NOT NEEDED
253 }
254
255 @Override
256 public void setOpenList(Collection<NODE> openList) {
257 // NOT NEEDED
258 }
259
260 @Override
261 public int getExtraCost(NODE nodeFrom, NODE nodeTo) {
262 return evaluator.getExtraCost(nodeFrom, nodeTo);
263 }
264
265 @Override
266 public boolean isNodeOpened(NODE node) {
267 return evaluator.isNodeOpened(node);
268 }
269
270 @Override
271 public int getEstimatedDistanceToGoal(NODE node) {
272 return evaluator.getEstimatedDistanceToGoal(node);
273 }
274
275 },
276 maxIterations
277 );
278 }
279
280 /**
281 * Method performing an AStar search over graph defined inside {@link AStarMap} starting from 'start' node driving
282 * itself towards 'goal' using heuristic and extra costs defined by {@link AStarEvaluator}.
283 * <p><p>
284 * {@link AStarMap} provides informations about node neighbours and edge costs,
285 * while {@link AStarEvaluator} contains definition of the heuristic + extra edge info about map nodes.
286 * <p><p>
287 * This method performs 'unbounded' AStar search, i.e., it does not limit a number of iterations the algorithm will perform.
288 * <p><p>
289 * <b>WARNING</b>: Class that is used for NODE must have correctly defined {@link Object#equals(Object)} because it will be used
290 * to recognized whether the current evaluated node is the same as the goal or not!
291 *
292 * @param <NODE>
293 * @param map
294 * @param evaluator
295 * @param start
296 * @param goal
297 * @return
298 */
299 public static <NODE> AStarResult<NODE> aStar(final AStarMap<NODE> map, final AStarEvaluator<NODE> evaluator, final NODE start, final NODE goal) {
300 return aStar(map, evaluator, start, goal, -1);
301 }
302
303 /**
304 * Method performing an AStar search over graph defined inside {@link AStarMap} starting from 'start' node driving
305 * itself towards 'goal' using heuristic defined by {@link AStarHeuristic}.
306 * <p><p>
307 * {@link AStarMap} provides informations about node neighbours and edge costs,
308 * while {@link AStarHeuristic} contains definition of the heuristic.
309 * <p><p>
310 * You may also specify maxIterations - "how long the A* should search" equals
311 * to number of evaluated nodes. If it's 0 then A* won't even start! If it is < 0 it will run until the 'goal' is found
312 * all nodes are evaluated.
313 * <p><p>
314 * <b>WARNING</b>: Class that is used for NODE must have correctly defined {@link Object#equals(Object)} because it will be used
315 * to recognized whether the current evaluated node is the same as the goal or not!
316 *
317 * @param <NODE>
318 * @param map
319 * @param evaluator
320 * @param start
321 * @param goal
322 * @param maxIterations
323 * @return
324 */
325 public static <NODE> AStarResult<NODE> aStar(final AStarMap<NODE> map, final AStarHeuristic<NODE> heuristic, final NODE start, final NODE goal, int maxIterations) {
326 return aStar(map, start, new AStarGoal<NODE>() {
327
328 @Override
329 public boolean isGoalReached(NODE actualNode) {
330 return SafeEquals.equals(actualNode, goal);
331 }
332
333 @Override
334 public void setCloseList(Collection<NODE> closeList) {
335 // NOT NEEDED
336 }
337
338 @Override
339 public void setOpenList(Collection<NODE> openList) {
340 // NOT NEEDED
341 }
342
343 @Override
344 public int getExtraCost(NODE nodeFrom, NODE nodeTo) {
345 return 0;
346 }
347
348 @Override
349 public boolean isNodeOpened(NODE node) {
350 return true;
351 }
352
353 @Override
354 public int getEstimatedDistanceToGoal(NODE node) {
355 return heuristic.getEstimatedDistanceToGoal(node);
356 }
357
358 },
359 maxIterations);
360 }
361
362 /**
363 * Method performing an AStar search over graph defined inside {@link AStarMap} starting from 'start' node driving
364 * itself towards 'goal' using heuristic defined by {@link AStarHeuristic}.
365 * <p><p>
366 * {@link AStarMap} provides informations about node neighbours and edge costs,
367 * while {@link AStarHeuristic} contains definition of the heuristic.
368 * <p><p>
369 * This method performs 'unbounded' AStar search, i.e., it does not limit a number of iterations the algorithm will perform.
370 * <p><p>
371 * <b>WARNING</b>: Class that is used for NODE must have correctly defined {@link Object#equals(Object)} because it will be used
372 * to recognized whether the current evaluated node is the same as the goal or not!
373 *
374 * @param <NODE>
375 * @param map
376 * @param heuristic
377 * @param start
378 * @param goal
379 * @return
380 */
381 public static <NODE> AStarResult<NODE> aStar(final AStarMap<NODE> map, final AStarHeuristic<NODE> heuristic, final NODE start, final NODE goal) {
382 return aStar(map, heuristic, start, goal, -1);
383 }
384
385 }