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 * <p><p>
71 * Use amis-path-finding library instead, see svn://artemis.ms.mff.cuni.cz/pogamut/trunk/project/Utils/AmisPathFinding
72 */
73 @Deprecated
74 public class AStar<NODE> {
75
76 /**
77 * Method performing an AStar search over graph defined inside {@link AStarMap} starting from 'start' node driving
78 * itself towards goal that is described by {@link AStarGoal}. Note that {@link AStarGoal} also contains a heuristic {@link AStarHeuristic}.
79 * <p><p>
80 * {@link AStarMap} provides informations about node neighbours and edge costs,
81 * while {@link AStarGoal} contains the definition of goal node and extra cost / extra info
82 * about map nodes.
83 * <p><p>
84 * You may also specify maxIterations - "how long the A* should search" equals
85 * 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
86 * all nodes are evaluated.
87 *
88 * @param map
89 * @param start
90 * @param goal
91 * @param iterationsMax maximum of iterations to be made by algorithm during the search (negative number == infinite)
92 */
93 public static <NODE> AStarResult<NODE> aStar(AStarMap<NODE> map, NODE start, AStarGoal<NODE> goal, long iterationsMax){
94
95 // NOTE: values of the estimated cost is maintained in AStarResult
96 // AS HEAP: we're using AStarHeap with AStarHeapComparator which is
97 // using data from AStarResult.estimatedCost ...
98 // that means you have to first alter AStarResult.estimatedCost
99 // before adding / decreasing key in AStarHeap
100
101 AStarResult<NODE> result = new AStarResult<NODE>();
102
103 AStarHeap<NODE> open = new AStarHeap<NODE>(new AStarHeapComparator<NODE>(result.estimatedCost), 64);
104 result.openList = open;
105 Collection<NODE> close = result.closeList;
106
107 goal.setCloseList(result.closeList);
108 goal.setOpenList(result.openList);
109
110 result.startNode = start;
111
112 result.putCostToNode(result.startNode, 0);
113 result.putEstimatedCostToNode(result.startNode, goal.getEstimatedDistanceToGoal(result.startNode));
114 open.add(result.startNode);
115
116 NODE node, nextNode;
117 Collection<NODE> neighbours;
118 Iterator<NODE> nodeIter;
119 int nodePathCost, nextNodePathCost, travelCost, extraCost, estimatedPathCost,
120 newNextNodePathCost;
121
122 while ((!open.empty()) &&
123 ((iterationsMax <= 0) || (result.interations < iterationsMax))
124 ){
125 ++result.interations; // new iteratrion begin
126
127 node = open.getMin();
128
129 if (node == null){ // failure
130 result.success = false;
131 break;
132 }
133
134 open.deleteMin();
135
136 if (goal.isGoalReached(node)){ // we've reached the goal HURRAY!
137 result.goalNode = node;
138 result.success = true;
139 break;
140 }
141
142 nodePathCost = result.getCostToNode(node);
143
144 neighbours = map.getNodeNeighbours(node);
145 nodeIter = neighbours.iterator();
146
147 while (nodeIter.hasNext()){
148 nextNode = nodeIter.next();
149 // iterate over all of the neighbours node
150 // and evaluate them one by one
151
152 if (!goal.isNodeOpened(nextNode)){ // stepping to this node is forbidden, skip it
153 continue;
154 }
155
156 travelCost = map.getEdgeCost(node, nextNode);
157 extraCost = goal.getExtraCost(node, nextNode);
158
159 nextNodePathCost = result.getCostToNode(nextNode);
160 if (nextNodePathCost == -1){
161 // we've never touched nextNode
162 nextNodePathCost = nodePathCost + travelCost + extraCost;
163 if (nextNodePathCost < 0) nextNodePathCost = 0;
164 result.putCostToNode(nextNode, nextNodePathCost);
165 result.putPreviousNode(nextNode, node);
166
167 estimatedPathCost = nextNodePathCost + goal.getEstimatedDistanceToGoal(nextNode);
168 result.putEstimatedCostToNode(nextNode, estimatedPathCost);
169
170 open.add(nextNode);
171 continue;
172 } else {
173 // we've already touched the nextNode
174 newNextNodePathCost = nodePathCost + travelCost + extraCost;
175 if (newNextNodePathCost < 0) newNextNodePathCost = 0;
176 if (newNextNodePathCost < nextNodePathCost){
177 estimatedPathCost = newNextNodePathCost + goal.getEstimatedDistanceToGoal(nextNode);
178 result.putCostToNode(nextNode, newNextNodePathCost);
179 result.putEstimatedCostToNode(nextNode, estimatedPathCost);
180 if (close.contains(nextNode)){
181 close.remove(nextNode);
182 open.add(nextNode);
183 } else
184 if (open.contains(nextNode)){
185 open.decreaseKey(node);
186 } else {
187 open.add(nextNode);
188 }
189 }
190 // if estimatedCost is higher or equal, we don't have to take any actions
191 continue;
192 }
193 }
194 close.add(node);
195 }
196
197 return result;
198 }
199
200 /**
201 * Method performing an AStar search over graph defined inside {@link AStarMap} starting from 'start' node driving
202 * itself towards goal that is described by {@link AStarGoal}. Note that {@link AStarGoal} also contains a heuristic {@link AStarHeuristic}.
203 * <p><p>
204 * {@link AStarMap} provides informations about node neighbours and edge costs,
205 * while {@link AStarGoal} contains the definition of goal node and extra cost / extra info
206 * about map nodes.
207 * <p><p>
208 * This method performs 'unbounded' AStar search, i.e., it does not limit a number of iterations the algorithm will perform.
209 *
210 * @param <NODE>
211 * @param map
212 * @param start
213 * @param goal
214 * @return
215 */
216 public static <NODE> AStarResult<NODE> aStar(final AStarMap<NODE> map, final NODE start, final AStarGoal<NODE> goal) {
217 return aStar(map, start, goal, -1);
218 }
219
220 /**
221 * Method performing an AStar search over graph defined inside {@link AStarMap} starting from 'start' node driving
222 * itself towards 'goal' using heuristic and extra costs defined by {@link AStarEvaluator}.
223 * <p><p>
224 * {@link AStarMap} provides informations about node neighbours and edge costs,
225 * while {@link AStarEvaluator} contains definition of the heuristic + extra edge info about map nodes.
226 * <p><p>
227 * You may also specify maxIterations - "how long the A* should search" equals
228 * 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
229 * all nodes are evaluated.
230 * <p><p>
231 * <b>WARNING</b>: Class that is used for NODE must have correctly defined {@link Object#equals(Object)} because it will be used
232 * to recognized whether the current evaluated node is the same as the goal or not!
233 *
234 * @param <NODE>
235 * @param map
236 * @param evaluator
237 * @param start
238 * @param goal
239 * @param maxIterations maximum of iterations to be made by algorithm during the search (negative number == infinite)
240 * @return
241 */
242 public static <NODE> AStarResult<NODE> aStar(final AStarMap<NODE> map, final AStarEvaluator<NODE> evaluator, final NODE start, final NODE goal, int maxIterations) {
243 return
244 aStar(
245 map,
246 start,
247 new AStarGoal<NODE>() {
248
249 @Override
250 public boolean isGoalReached(NODE actualNode) {
251 return SafeEquals.equals(actualNode, goal);
252 }
253
254 @Override
255 public void setCloseList(Collection<NODE> closeList) {
256 // NOT NEEDED
257 }
258
259 @Override
260 public void setOpenList(Collection<NODE> openList) {
261 // NOT NEEDED
262 }
263
264 @Override
265 public int getExtraCost(NODE nodeFrom, NODE nodeTo) {
266 return evaluator.getExtraCost(nodeFrom, nodeTo);
267 }
268
269 @Override
270 public boolean isNodeOpened(NODE node) {
271 return evaluator.isNodeOpened(node);
272 }
273
274 @Override
275 public int getEstimatedDistanceToGoal(NODE node) {
276 return evaluator.getEstimatedDistanceToGoal(node);
277 }
278
279 },
280 maxIterations
281 );
282 }
283
284 /**
285 * Method performing an AStar search over graph defined inside {@link AStarMap} starting from 'start' node driving
286 * itself towards 'goal' using heuristic and extra costs defined by {@link AStarEvaluator}.
287 * <p><p>
288 * {@link AStarMap} provides informations about node neighbours and edge costs,
289 * while {@link AStarEvaluator} contains definition of the heuristic + extra edge info about map nodes.
290 * <p><p>
291 * This method performs 'unbounded' AStar search, i.e., it does not limit a number of iterations the algorithm will perform.
292 * <p><p>
293 * <b>WARNING</b>: Class that is used for NODE must have correctly defined {@link Object#equals(Object)} because it will be used
294 * to recognized whether the current evaluated node is the same as the goal or not!
295 *
296 * @param <NODE>
297 * @param map
298 * @param evaluator
299 * @param start
300 * @param goal
301 * @return
302 */
303 public static <NODE> AStarResult<NODE> aStar(final AStarMap<NODE> map, final AStarEvaluator<NODE> evaluator, final NODE start, final NODE goal) {
304 return aStar(map, evaluator, start, goal, -1);
305 }
306
307 /**
308 * Method performing an AStar search over graph defined inside {@link AStarMap} starting from 'start' node driving
309 * itself towards 'goal' using heuristic defined by {@link AStarHeuristic}.
310 * <p><p>
311 * {@link AStarMap} provides informations about node neighbours and edge costs,
312 * while {@link AStarHeuristic} contains definition of the heuristic.
313 * <p><p>
314 * You may also specify maxIterations - "how long the A* should search" equals
315 * 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
316 * all nodes are evaluated.
317 * <p><p>
318 * <b>WARNING</b>: Class that is used for NODE must have correctly defined {@link Object#equals(Object)} because it will be used
319 * to recognized whether the current evaluated node is the same as the goal or not!
320 *
321 * @param <NODE>
322 * @param map
323 * @param evaluator
324 * @param start
325 * @param goal
326 * @param maxIterations
327 * @return
328 */
329 public static <NODE> AStarResult<NODE> aStar(final AStarMap<NODE> map, final AStarHeuristic<NODE> heuristic, final NODE start, final NODE goal, int maxIterations) {
330 return aStar(map, start, new AStarGoal<NODE>() {
331
332 @Override
333 public boolean isGoalReached(NODE actualNode) {
334 return SafeEquals.equals(actualNode, goal);
335 }
336
337 @Override
338 public void setCloseList(Collection<NODE> closeList) {
339 // NOT NEEDED
340 }
341
342 @Override
343 public void setOpenList(Collection<NODE> openList) {
344 // NOT NEEDED
345 }
346
347 @Override
348 public int getExtraCost(NODE nodeFrom, NODE nodeTo) {
349 return 0;
350 }
351
352 @Override
353 public boolean isNodeOpened(NODE node) {
354 return true;
355 }
356
357 @Override
358 public int getEstimatedDistanceToGoal(NODE node) {
359 return heuristic.getEstimatedDistanceToGoal(node);
360 }
361
362 },
363 maxIterations);
364 }
365
366 /**
367 * Method performing an AStar search over graph defined inside {@link AStarMap} starting from 'start' node driving
368 * itself towards 'goal' using heuristic defined by {@link AStarHeuristic}.
369 * <p><p>
370 * {@link AStarMap} provides informations about node neighbours and edge costs,
371 * while {@link AStarHeuristic} contains definition of the heuristic.
372 * <p><p>
373 * This method performs 'unbounded' AStar search, i.e., it does not limit a number of iterations the algorithm will perform.
374 * <p><p>
375 * <b>WARNING</b>: Class that is used for NODE must have correctly defined {@link Object#equals(Object)} because it will be used
376 * to recognized whether the current evaluated node is the same as the goal or not!
377 *
378 * @param <NODE>
379 * @param map
380 * @param heuristic
381 * @param start
382 * @param goal
383 * @return
384 */
385 public static <NODE> AStarResult<NODE> aStar(final AStarMap<NODE> map, final AStarHeuristic<NODE> heuristic, final NODE start, final NODE goal) {
386 return aStar(map, heuristic, start, goal, -1);
387 }
388
389 }