1 package cz.cuni.amis.pathfinding.alg.astar;
2
3 import java.util.Collection;
4 import java.util.Collections;
5 import java.util.HashSet;
6 import java.util.Iterator;
7
8 import cz.cuni.amis.pathfinding.map.IPFGoal;
9 import cz.cuni.amis.pathfinding.map.IPFMap;
10 import cz.cuni.amis.pathfinding.map.IPFMapView;
11 import cz.cuni.amis.pathfinding.map.IPFMapView.DefaultView;
12 import cz.cuni.amis.utils.Iterators;
13 import cz.cuni.amis.utils.NullCheck;
14 import cz.cuni.amis.utils.astar.AStarGoal;
15 import cz.cuni.amis.utils.astar.AStarHeuristic;
16 import cz.cuni.amis.utils.astar.AStarMap;
17 import cz.cuni.amis.utils.heap.Heap;
18 import cz.cuni.amis.utils.heap.ImmutableHeap;
19
20 /**
21 * Implementation of generic A* algorithm, better refered to as A* Machine according to
22 * Dan Higgins, Generic A* Pathfind paper from AI Gaming Wisdom, 2002
23 * <p><p>
24 * What is A*<p>
25 * ----------<p>
26 * A* is space-search algorithm using a custom-built heuristic. It's an improved
27 * version of well-known Dijkstra algorithm which is used to find the shortest
28 * path in weighted graphs. Instead of picking the node with the smallest path
29 * from the start node it chooses node which seems to be on the shortest path
30 * to the goal node (and this guess is based on provided heuristic).
31 * <p><p>
32 * Note<p>
33 * ----<p>
34 * Insted of weights we speak about cost of the edges.
35 * <p><p>
36 * Limitation of A*<p>
37 * ----------------<p>
38 * 1) A* doesn't work over graphs with negative edge costs.<p>
39 * 2) heuristic has to be correct & monotonic
40 * <p>
41 * First we have to specify some interfaces for A* to work<p>
42 * ------------------------------------------------------<p>
43 * <ol>
44 * <li>{@link IPFMap} that provides the search-space notion for the A*, this implementation should be agent-agnostic, that is it should not incorporate
45 * any particular limitations/abilities into it</li>
46 * <li>{@link IPFMapView} that provides agent's customized view of the map (extra arc costs, node filtering, etc.)</li>
47 * <li>{@link IPFGoal} that provides the heuristic function + the definition of the goal</li>
48 * <p>
49 * Note about Nodes<p>
50 * ----------------<p>
51 * Note that we don't need to have a Node interface so you're free to have
52 * any nodes you want (POJOs). But implementation of A* requires the nodes to have
53 * {@link Object#hashCode()} and {@link Object#equals()} implemented correctly, which should be a good practice!
54 * (Note that also means you can't have two nodes which are equals in the map!)
55 * <p><p>
56 * Ideas behind {@link IPFMap}, {@link IPFMapView} {@link IPFGoal}<p>
57 * ---------------------------------------------------------------<p>
58 * Usually you will have only one world / state space representation (that is {@link IPFMap}) but you need to
59 * change the cost of edges between nodes according to your agent (imagine a fish) for which you
60 * search the path ({@link IPFMapView}). Finally you will need to search the space for different nodes
61 * possibly using different heuristics based on the goal pursued ({@link IPFGoal}).
62 * <p><p>
63 * Imagine the situation with the lake (map) / human (one agent) / fish (another agent).
64 * Human may swim across the lake but it's faster to run around it (so you need to give the edges between
65 * water tiles an extra cost using {@link IPFMapView#getArcExtraCost(Object, Object, int)}).<p>
66 * Fish can swim really fast but can't get out of the water (so you need to forbid tiles around the lake
67 * and give the edges between the lakes' tiles using {@link IPFMapView#isNodeOpened(Object)}).<p>
68 * Finally you may have hierarchical representation of your graph that has different arc-cost for humans
69 * and fishes (taking into account the lake), thus you might need to specify different heuristic function for
70 * humans and fishes using {@link IPFGoal#getEstimatedCostToGoal(Object)}.
71 * <p>
72 * So the AStarMap will represent the world with the lake with default cost of the edges.
73 * AStarGoal may change the edges cost / forbid some nodes completely. So you will
74 * implement one goal for a human and another for a fish.
75 * <p><p>
76 * Note about the speed<p>
77 * --------------------<p>
78 * Speed of algorithm is based upon the speed of AStarOpenList and AStarCloseList.
79 */
80 public class AStar<NODE> {
81
82 /**
83 * Holds the representation of the map.
84 */
85 private IPFMap<NODE> map;
86
87 /**
88 * Holds the agent-specific view of the map.
89 */
90 private IPFMapView<NODE> view;
91
92 /**
93 * AStar configured with "map" with no agent-specific view on the map, {@link DefaultView} is used.
94 * @param map
95 */
96 public AStar(IPFMap<NODE> map) {
97 this.map = map;
98 this.view = new IPFMapView.DefaultView();
99 NullCheck.check(this.map, "map");
100 }
101
102 /**
103 * AStar configured with "map" and agent-specific view on the map, if "view" is null, {@link DefaultView} is going to be used.
104 * @param map
105 * @param view may be null
106 */
107 public AStar(IPFMap<NODE> map, IPFMapView<NODE> view) {
108 this.map = map;
109 this.view = view;
110 NullCheck.check(this.map, "map");
111 if (this.view == null) {
112 this.view = new IPFMapView.DefaultView();
113 }
114 }
115
116 /**
117 * Map abstraction the AStar is working with.
118 * @return
119 */
120 public IPFMap<NODE> getMap() {
121 return map;
122 }
123
124 /**
125 * Sets map abstraction into the AStar.
126 * @param map
127 */
128 public synchronized void setMap(IPFMap<NODE> map) {
129 this.map = map;
130 }
131
132 /**
133 * Returns agent-specific map view for the map.
134 * @return
135 */
136 public IPFMapView<NODE> getMapView() {
137 return view;
138 }
139
140 /**
141 * Sets agent-specific map view for the map.
142 * @param mapView
143 */
144 public synchronized void setMapView(IPFMapView<NODE> mapView) {
145 this.view = mapView;
146 }
147
148 ////////////////////////////////////////////////////////////
149 // AStar runtime variables - cleared after aStar() finishes.
150 ////////////////////////////////////////////////////////////
151
152 private IPFGoal<NODE> goal = null;
153 private long iterationsMax = 0;
154 private AStarResult<NODE> result = null;
155
156
157 /**
158 * Method performing an AStar search over graph defined inside {@link IPFMap} starting from 'start' node driving
159 * itself towards goal that is described by {@link AStarGoal}. Note that {@link AStarGoal} also contains a heuristic {@link AStarHeuristic}.
160 * <p><p>
161 * {@link AStarMap} provides informations about node neighbours and edge costs,
162 * while {@link AStarGoal} contains the definition of goal node and extra cost / extra info
163 * about map nodes.
164 * <p><p>
165 * You may also specify maxIterations - "how long the A* should search" equals
166 * to number of evaluated nodes. If it's < 0 then A* will run until the 'goal' is found
167 * all nodes are evaluated and there is nowhere to search. If it is == 0, the A* won't even start!
168 *
169 * @param map
170 * @param start
171 * @param goal
172 * @param iterationsMax maximum of iterations to be made by algorithm during the search (negative number == infinite)
173 */
174 public synchronized AStarResult<NODE> findPath(IPFGoal<NODE> goal, long iterationsMax) {
175
176 // NOTE: values of the estimated cost is maintained in AStarResult
177 // AS HEAP: we're using Heap with AStarHeapComparator which is
178 // using data from AStarResult.estimatedCost ...
179 // that means you have to first alter AStarResult.estimatedCost
180 // before adding / decreasing key in AStarHeap
181
182 this.goal = goal;
183 this.iterationsMax = iterationsMax;
184
185 NODE start = goal.getStart();
186 this.result = new AStarResult<NODE>();
187
188 result.openList = new Heap<NODE>(new AStarHeapComparator<NODE>(result.estimatedCost), 64);
189 result.closeList = new HashSet<NODE>();
190 Collection<NODE> close = result.closeList;
191
192 goal.setCloseList(Collections.unmodifiableSet(result.closeList));
193 goal.setOpenList(new ImmutableHeap(result.openList));
194
195 result.startNode = start;
196
197 result.putCostToNode(result.startNode, 0);
198 result.putEstimatedCostToNode(result.startNode, goal.getEstimatedCostToGoal(result.startNode));
199 result.openList.add(result.startNode);
200
201 NODE node, nextNode;
202 Collection<NODE> neighbors;
203 Collection<NODE> extraNeighbors;
204 Iterator<NODE> nodeIter;
205 int nodePathCost, nextNodePathCost, travelCost, extraCost, nodeCost, nodeExtraCost, estimatedPathCost, newNextNodePathCost;
206
207 while ((!result.openList.empty()) &&
208 ((this.iterationsMax <= 0) || (result.interations < iterationsMax))
209 ){
210 ++result.interations; // new iteration begin
211
212 node = result.openList.getMin();
213
214 if (node == null){ // failure
215 result.success = false;
216 break;
217 }
218
219 result.openList.deleteMin();
220
221 if (goal.isGoalReached(node)){ // we've reached the goal HURRAY!
222 result.goalNode = node;
223 result.success = true;
224 break;
225 }
226
227 nodePathCost = result.getCostToNode(node);
228
229 neighbors = map.getNeighbors(node);
230 extraNeighbors = view.getExtraNeighbors(node, neighbors);
231 nodeIter = new Iterators<NODE>(
232 neighbors == null ? null : neighbors.iterator(),
233 extraNeighbors == null ? null : extraNeighbors.iterator()
234 );
235
236 while (nodeIter.hasNext()){
237 // iterate over all of the neighbors node
238 nextNode = nodeIter.next();
239 if (nextNode == null) continue;
240 // and evaluate them one by one
241
242 if (!view.isNodeOpened(nextNode)){ // stepping to this node is forbidden, skip it
243 continue;
244 }
245 if (!view.isArcOpened(node, nextNode)) { // travelling through this arc is forbidden, skip it
246 continue;
247 }
248
249 travelCost = map.getArcCost(node, nextNode);
250 extraCost = view.getArcExtraCost(node, nextNode, travelCost);
251
252 nodeCost = map.getNodeCost(nextNode);
253 nodeExtraCost = view.getNodeExtraCost(nextNode, nodeCost);
254
255 nextNodePathCost = result.getCostToNode(nextNode);
256 if (nextNodePathCost == -1){
257 // we've never touched nextNode
258 nextNodePathCost = nodePathCost + travelCost + extraCost + nodeCost + nodeExtraCost;
259 if (nextNodePathCost < 0) nextNodePathCost = 0;
260 result.putCostToNode(nextNode, nextNodePathCost);
261 result.putPreviousNode(nextNode, node);
262
263 estimatedPathCost = nextNodePathCost + goal.getEstimatedCostToGoal(nextNode);
264 result.putEstimatedCostToNode(nextNode, estimatedPathCost);
265
266 result.openList.add(nextNode);
267 continue;
268 } else {
269 // we've already touched the nextNode
270 newNextNodePathCost = nodePathCost + travelCost + extraCost + nodeCost + nodeExtraCost;
271 if (newNextNodePathCost < 0) newNextNodePathCost = 0;
272 if (newNextNodePathCost < nextNodePathCost){
273 estimatedPathCost = newNextNodePathCost + goal.getEstimatedCostToGoal(nextNode);
274 result.putCostToNode(nextNode, newNextNodePathCost);
275 result.putEstimatedCostToNode(nextNode, estimatedPathCost);
276 if (close.contains(nextNode)){
277 close.remove(nextNode);
278 result.openList.add(nextNode);
279 } else
280 if (result.openList.contains(nextNode)){
281 result.openList.decreaseKey(node);
282 } else {
283 result.openList.add(nextNode);
284 }
285 }
286 // if estimatedCost is higher or equal, we don't have to take any actions
287 continue;
288 }
289 }
290 close.add(node);
291 }
292
293 return result;
294 }
295
296 /**
297 * Method performing an AStar search over graph defined inside {@link IPFMap} starting from 'start' node driving
298 * itself towards goal that is described by {@link AStarGoal}. Note that {@link AStarGoal} also contains a heuristic {@link AStarHeuristic}.
299 * <p><p>
300 * {@link AStarMap} provides informations about node neighbours and edge costs,
301 * while {@link AStarGoal} contains the definition of goal node and extra cost / extra info
302 * about map nodes.
303 * <p><p>
304 * Does not have any cap on the number of evaluated nodes. Will run until the 'goal' is found
305 * all nodes are evaluated and there is nowhere to search.
306 *
307 * @param map
308 * @param start
309 * @param goal
310 */
311 public synchronized AStarResult<NODE> findPath(IPFGoal<NODE> goal) {
312 return findPath(goal, 0);
313 }
314
315 }