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 result.putPreviousNode(nextNode, node);
181 if (close.contains(nextNode)){
182 close.remove(nextNode);
183 open.add(nextNode);
184 } else
185 if (open.contains(nextNode)){
186 open.decreaseKey(node);
187 } else {
188 open.add(nextNode);
189 }
190 }
191 // if estimatedCost is higher or equal, we don't have to take any actions
192 continue;
193 }
194 }
195 close.add(node);
196 }
197
198 return result;
199 }
200
201 /**
202 * Method performing an AStar search over graph defined inside {@link AStarMap} starting from 'start' node driving
203 * itself towards goal that is described by {@link AStarGoal}. Note that {@link AStarGoal} also contains a heuristic {@link AStarHeuristic}.
204 * <p><p>
205 * {@link AStarMap} provides informations about node neighbours and edge costs,
206 * while {@link AStarGoal} contains the definition of goal node and extra cost / extra info
207 * about map nodes.
208 * <p><p>
209 * This method performs 'unbounded' AStar search, i.e., it does not limit a number of iterations the algorithm will perform.
210 *
211 * @param <NODE>
212 * @param map
213 * @param start
214 * @param goal
215 * @return
216 */
217 public static <NODE> AStarResult<NODE> aStar(final AStarMap<NODE> map, final NODE start, final AStarGoal<NODE> goal) {
218 return aStar(map, start, goal, -1);
219 }
220
221 /**
222 * Method performing an AStar search over graph defined inside {@link AStarMap} starting from 'start' node driving
223 * itself towards 'goal' using heuristic and extra costs defined by {@link AStarEvaluator}.
224 * <p><p>
225 * {@link AStarMap} provides informations about node neighbours and edge costs,
226 * while {@link AStarEvaluator} contains definition of the heuristic + extra edge info about map nodes.
227 * <p><p>
228 * You may also specify maxIterations - "how long the A* should search" equals
229 * 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
230 * all nodes are evaluated.
231 * <p><p>
232 * <b>WARNING</b>: Class that is used for NODE must have correctly defined {@link Object#equals(Object)} because it will be used
233 * to recognized whether the current evaluated node is the same as the goal or not!
234 *
235 * @param <NODE>
236 * @param map
237 * @param evaluator
238 * @param start
239 * @param goal
240 * @param maxIterations maximum of iterations to be made by algorithm during the search (negative number == infinite)
241 * @return
242 */
243 public static <NODE> AStarResult<NODE> aStar(final AStarMap<NODE> map, final AStarEvaluator<NODE> evaluator, final NODE start, final NODE goal, int maxIterations) {
244 return
245 aStar(
246 map,
247 start,
248 new AStarGoal<NODE>() {
249
250 @Override
251 public boolean isGoalReached(NODE actualNode) {
252 return SafeEquals.equals(actualNode, goal);
253 }
254
255 @Override
256 public void setCloseList(Collection<NODE> closeList) {
257 // NOT NEEDED
258 }
259
260 @Override
261 public void setOpenList(Collection<NODE> openList) {
262 // NOT NEEDED
263 }
264
265 @Override
266 public int getExtraCost(NODE nodeFrom, NODE nodeTo) {
267 return evaluator.getExtraCost(nodeFrom, nodeTo);
268 }
269
270 @Override
271 public boolean isNodeOpened(NODE node) {
272 return evaluator.isNodeOpened(node);
273 }
274
275 @Override
276 public int getEstimatedDistanceToGoal(NODE node) {
277 return evaluator.getEstimatedDistanceToGoal(node);
278 }
279
280 },
281 maxIterations
282 );
283 }
284
285 /**
286 * Method performing an AStar search over graph defined inside {@link AStarMap} starting from 'start' node driving
287 * itself towards 'goal' using heuristic and extra costs defined by {@link AStarEvaluator}.
288 * <p><p>
289 * {@link AStarMap} provides informations about node neighbours and edge costs,
290 * while {@link AStarEvaluator} contains definition of the heuristic + extra edge info about map nodes.
291 * <p><p>
292 * This method performs 'unbounded' AStar search, i.e., it does not limit a number of iterations the algorithm will perform.
293 * <p><p>
294 * <b>WARNING</b>: Class that is used for NODE must have correctly defined {@link Object#equals(Object)} because it will be used
295 * to recognized whether the current evaluated node is the same as the goal or not!
296 *
297 * @param <NODE>
298 * @param map
299 * @param evaluator
300 * @param start
301 * @param goal
302 * @return
303 */
304 public static <NODE> AStarResult<NODE> aStar(final AStarMap<NODE> map, final AStarEvaluator<NODE> evaluator, final NODE start, final NODE goal) {
305 return aStar(map, evaluator, start, goal, -1);
306 }
307
308 /**
309 * Method performing an AStar search over graph defined inside {@link AStarMap} starting from 'start' node driving
310 * itself towards 'goal' using heuristic defined by {@link AStarHeuristic}.
311 * <p><p>
312 * {@link AStarMap} provides informations about node neighbours and edge costs,
313 * while {@link AStarHeuristic} contains definition of the heuristic.
314 * <p><p>
315 * You may also specify maxIterations - "how long the A* should search" equals
316 * 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
317 * all nodes are evaluated.
318 * <p><p>
319 * <b>WARNING</b>: Class that is used for NODE must have correctly defined {@link Object#equals(Object)} because it will be used
320 * to recognized whether the current evaluated node is the same as the goal or not!
321 *
322 * @param <NODE>
323 * @param map
324 * @param evaluator
325 * @param start
326 * @param goal
327 * @param maxIterations
328 * @return
329 */
330 public static <NODE> AStarResult<NODE> aStar(final AStarMap<NODE> map, final AStarHeuristic<NODE> heuristic, final NODE start, final NODE goal, int maxIterations) {
331 return aStar(map, start, new AStarGoal<NODE>() {
332
333 @Override
334 public boolean isGoalReached(NODE actualNode) {
335 return SafeEquals.equals(actualNode, goal);
336 }
337
338 @Override
339 public void setCloseList(Collection<NODE> closeList) {
340 // NOT NEEDED
341 }
342
343 @Override
344 public void setOpenList(Collection<NODE> openList) {
345 // NOT NEEDED
346 }
347
348 @Override
349 public int getExtraCost(NODE nodeFrom, NODE nodeTo) {
350 return 0;
351 }
352
353 @Override
354 public boolean isNodeOpened(NODE node) {
355 return true;
356 }
357
358 @Override
359 public int getEstimatedDistanceToGoal(NODE node) {
360 return heuristic.getEstimatedDistanceToGoal(node);
361 }
362
363 },
364 maxIterations);
365 }
366
367 /**
368 * Method performing an AStar search over graph defined inside {@link AStarMap} starting from 'start' node driving
369 * itself towards 'goal' using heuristic defined by {@link AStarHeuristic}.
370 * <p><p>
371 * {@link AStarMap} provides informations about node neighbours and edge costs,
372 * while {@link AStarHeuristic} contains definition of the heuristic.
373 * <p><p>
374 * This method performs 'unbounded' AStar search, i.e., it does not limit a number of iterations the algorithm will perform.
375 * <p><p>
376 * <b>WARNING</b>: Class that is used for NODE must have correctly defined {@link Object#equals(Object)} because it will be used
377 * to recognized whether the current evaluated node is the same as the goal or not!
378 *
379 * @param <NODE>
380 * @param map
381 * @param heuristic
382 * @param start
383 * @param goal
384 * @return
385 */
386 public static <NODE> AStarResult<NODE> aStar(final AStarMap<NODE> map, final AStarHeuristic<NODE> heuristic, final NODE start, final NODE goal) {
387 return aStar(map, heuristic, start, goal, -1);
388 }
389
390 }