001/* 002 * Copyright 2006 - 2013 003 * Stefan Balev <stefan.balev@graphstream-project.org> 004 * Julien Baudry <julien.baudry@graphstream-project.org> 005 * Antoine Dutot <antoine.dutot@graphstream-project.org> 006 * Yoann Pigné <yoann.pigne@graphstream-project.org> 007 * Guilhelm Savin <guilhelm.savin@graphstream-project.org> 008 * 009 * This file is part of GraphStream <http://graphstream-project.org>. 010 * 011 * GraphStream is a library whose purpose is to handle static or dynamic 012 * graph, create them from scratch, file or any source and display them. 013 * 014 * This program is free software distributed under the terms of two licenses, the 015 * CeCILL-C license that fits European law, and the GNU Lesser General Public 016 * License. You can use, modify and/ or redistribute the software under the terms 017 * of the CeCILL-C license as circulated by CEA, CNRS and INRIA at the following 018 * URL <http://www.cecill.info> or under the terms of the GNU LGPL as published by 019 * the Free Software Foundation, either version 3 of the License, or (at your 020 * option) any later version. 021 * 022 * This program is distributed in the hope that it will be useful, but WITHOUT ANY 023 * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A 024 * PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details. 025 * 026 * You should have received a copy of the GNU Lesser General Public License 027 * along with this program. If not, see <http://www.gnu.org/licenses/>. 028 * 029 * The fact that you are presently reading this means that you have had 030 * knowledge of the CeCILL-C and LGPL licenses and that you accept their terms. 031 */ 032package org.graphstream.algorithm; 033 034import static org.graphstream.algorithm.Toolkit.edgeLength; 035import static org.graphstream.algorithm.Toolkit.nodePosition; 036 037import java.util.ArrayList; 038import java.util.HashMap; 039import java.util.Iterator; 040 041import org.graphstream.graph.Edge; 042import org.graphstream.graph.Graph; 043import org.graphstream.graph.Node; 044import org.graphstream.graph.Path; 045 046/** 047 * An implementation of the A* algorithm. 048 * 049 * <p> 050 * A* computes the shortest path from a node to another in a graph. It guarantees 051 * that the path found is the shortest one, given its heuristic is admissible, 052 * and a path exists between the two nodes. It will fail if the two nodes are in 053 * two distinct connected components. 054 * </p> 055 * 056 * <p> 057 * In this A* implementation, the various costs (often called g, h and f) are 058 * given by a {@link org.graphstream.algorithm.AStar.Costs} class. This class 059 * must provide a way to compute: 060 * <ul> 061 * <li>The cost of moving from a node to another, often called g;</li> 062 * <li>The estimated cost from a node to the destination, the heuristic, often 063 * noted h;</li> 064 * <li>f is the sum of g and h and is computed automatically.</li> 065 * </ul> 066 * </p> 067 * 068 * <p> 069 * By default the {@link org.graphstream.algorithm.AStar.Costs} implementation 070 * used uses a heuristic that always returns 0. This makes A* an * equivalent of 071 * the Dijkstra algorithm, but also makes it less efficient. 072 * </p> 073 * 074 * <p> 075 * If there are several equivalent shortest paths between the two nodes, the returned 076 * one is arbitrary. Therefore this AStar algorithm works with multi-graphs but if two 077 * edges between two nodes have the same properties, the one that will be chosen will 078 * be arbitrary. 079 * </p> 080 * 081 * <h2>Usage</h2> 082 * 083 * <p>The basic usage is to create an instance of A* (optionally specify a {@link Costs} 084 * object), then to ask it to compute from a shortest path from one target to one 085 * destination, and finally to ask for that path: 086 * </p> 087 * <pre> 088 * AStart astar = new AStar(graph); 089 * astar.compute("A", "Z"); // with A and Z node identifiers in the graph. 090 * Path path = astar.getShortestPath(); 091 * </pre> 092 * <p> 093 * The advantage of A* is that it can consider any cost function to drive the 094 * search. You can (and should) create your own cost functions implementing the 095 * {@link org.graphstream.algorithm.AStar.Costs} interface. 096 * </p> 097 * <p> 098 * You can also test the default euclidean "distance" cost function on a graph that has 099 * "x" and "y" values. You specify the {@link Costs} function before calling the 100 * {@link #compute(String,String)} method: 101 * </p> 102 * <pre> 103 * AStart astar = new AStar(graph); 104 * astar.setCosts(new DistanceCosts()); 105 * astar.compute("A", "Z"); 106 * Path path = astar.getShortestPath(); 107 * </pre> 108 * 109 * <h2>Example</h2> 110 * 111 * <pre> 112 * import java.io.IOException; 113 * import java.io.StringReader; 114 * 115 * import org.graphstream.algorithm.AStar; 116 * import org.graphstream.algorithm.AStar.DistanceCosts; 117 * import org.graphstream.graph.Graph; 118 * import org.graphstream.graph.implementations.DefaultGraph; 119 * import org.graphstream.stream.file.FileSourceDGS; 120 * 121 * public class AStarTest { 122 * 123 * // B-(1)-C 124 * // / \ 125 * // (1) (10) 126 * // / \ 127 * // A F 128 * // \ / 129 * // (1) (1) 130 * // \ / 131 * // D-(1)-E 132 * static String my_graph = 133 * "DGS004\n" 134 * + "my 0 0\n" 135 * + "an A xy: 0,1\n" 136 * + "an B xy: 1,2\n" 137 * + "an C xy: 2,2\n" 138 * + "an D xy: 1,0\n" 139 * + "an E xy: 2,0\n" 140 * + "an F xy: 3,1\n" 141 * + "ae AB A B weight:1 \n" 142 * + "ae AD A D weight:1 \n" 143 * + "ae BC B C weight:1 \n" 144 * + "ae CF C F weight:10 \n" 145 * + "ae DE D E weight:1 \n" 146 * + "ae EF E F weight:1 \n" 147 * ; 148 * 149 * public static void main(String[] args) throws IOException { 150 * Graph graph = new DefaultGraph("A* Test"); 151 * StringReader reader = new StringReader(my_graph); 152 * 153 * FileSourceDGS source = new FileSourceDGS(); 154 * source.addSink(graph); 155 * source.readAll(reader); 156 * 157 * AStar astar = new AStar(graph); 158 * //astar.setCosts(new DistanceCosts()); 159 * astar.compute("C", "F"); 160 * 161 * System.out.println(astar.getShortestPath()); 162 * } 163 * } 164 * </pre> 165 * 166 * @complexity The complexity of A* depends on the heuristic. 167 */ 168public class AStar implements Algorithm { 169 /** 170 * The graph. 171 */ 172 protected Graph graph; 173 174 /** 175 * The source node id. 176 */ 177 protected String source; 178 179 /** 180 * The target node id. 181 */ 182 protected String target; 183 184 /** 185 * How to compute the path cost, the cost between two nodes and the 186 * heuristic. The heuristic to estimate the distance from the current 187 * position to the target. 188 */ 189 protected Costs costs = new DefaultCosts(); 190 191 /** 192 * The open set. 193 */ 194 protected HashMap<Node, AStarNode> open = new HashMap<Node, AStarNode>(); 195 196 /** 197 * The closed set. 198 */ 199 protected HashMap<Node, AStarNode> closed = new HashMap<Node, AStarNode>(); 200 201 /** 202 * If found the shortest path is stored here. 203 */ 204 protected Path result; 205 206 /** 207 * Set to false if the algorithm ran, but did not found any path from the 208 * source to the target, or if the algorithm did not run yet. 209 */ 210 protected boolean pathFound = false; 211 212 /** 213 * New A* algorithm. 214 */ 215 public AStar() { 216 } 217 218 /** 219 * New A* algorithm on a given graph. 220 * 221 * @param graph 222 * The graph where the algorithm will compute paths. 223 */ 224 public AStar(Graph graph) { 225 init(graph); 226 } 227 228 /** 229 * New A* algorithm on the given graph. 230 * 231 * @param graph 232 * The graph where the algorithm will compute paths. 233 * @param src 234 * The start node. 235 * @param trg 236 * The destination node. 237 */ 238 public AStar(Graph graph, String src, String trg) { 239 this(graph); 240 setSource(src); 241 setTarget(trg); 242 } 243 244 /** 245 * Change the source node. This clears the already computed path, but 246 * preserves the target node name. 247 * 248 * @param nodeName 249 * Identifier of the source node. 250 */ 251 public void setSource(String nodeName) { 252 clearAll(); 253 source = nodeName; 254 } 255 256 /** 257 * Change the target node. This clears the already computed path, but 258 * preserves the source node name. 259 * 260 * @param nodeName 261 * Identifier of the target node. 262 */ 263 public void setTarget(String nodeName) { 264 clearAll(); 265 target = nodeName; 266 } 267 268 /** 269 * Specify how various costs are computed. The costs object is in charge of 270 * computing the cost of displacement from one node to another (and 271 * therefore allows to compute the cost from the source node to any node). 272 * It also allows to compute the heuristic to use for evaluating the cost 273 * from the current position to the target node. Calling this DOES NOT clear 274 * the currently computed paths. 275 * 276 * @param costs 277 * The cost method to use. 278 */ 279 public void setCosts(Costs costs) { 280 this.costs = costs; 281 } 282 283 /* 284 * @see 285 * org.graphstream.algorithm.Algorithm#init(org.graphstream.graph.Graph) 286 */ 287 public void init(Graph graph) { 288 clearAll(); 289 this.graph = graph; 290 } 291 292 /* 293 * @see org.graphstream.algorithm.Algorithm#compute() 294 */ 295 public void compute() { 296 if (source != null && target != null) { 297 Node sourceNode = graph.getNode(source); 298 Node targetNode = graph.getNode(target); 299 300 if (sourceNode == null) 301 throw new RuntimeException("source node '" + source 302 + "' does not exist in the graph"); 303 304 if (targetNode == null) 305 throw new RuntimeException("target node '" + target 306 + "' does not exist in the graph"); 307 308 aStar(sourceNode, targetNode); 309 } 310 } 311 312 /** 313 * The computed path, or null if nor result was found. 314 * 315 * @return The computed path, or null if no path was found. 316 */ 317 public Path getShortestPath() { 318 return result; 319 } 320 321 /** 322 * After having called {@link #compute()} or 323 * {@link #compute(String, String)}, if the {@link #getShortestPath()} 324 * returns null, or this method return true, there is no path from the given 325 * source node to the given target node. In other words, the graph has 326 * several connected components. It also return true if the algorithm did 327 * not run. 328 * 329 * @return True if there is no possible path from the source to the 330 * destination or if the algorithm did not run. 331 */ 332 public boolean noPathFound() { 333 return (! pathFound); 334 } 335 336 /** 337 * Build the shortest path from the target/destination node, following the 338 * parent links. 339 * 340 * @param target 341 * The destination node. 342 * @return The path. 343 */ 344 public Path buildPath(AStarNode target) { 345 Path path = new Path(); 346 347 ArrayList<AStarNode> thePath = new ArrayList<AStarNode>(); 348 AStarNode node = target; 349 350 while (node != null) { 351 thePath.add(node); 352 node = node.parent; 353 } 354 355 int n = thePath.size(); 356 357 if (n > 1) { 358 AStarNode current = thePath.get(n - 1); 359 AStarNode follow = thePath.get(n - 2); 360 361 path.add(current.node, follow.edge); 362 363 current = follow; 364 365 for (int i = n - 3; i >= 0; i--) { 366 follow = thePath.get(i); 367 path.add(follow.edge); 368 current = follow; 369 } 370 } 371 372 return path; 373 } 374 375 /** 376 * Call {@link #compute()} after having called {@link #setSource(String)} 377 * and {@link #setTarget(String)}. 378 * 379 * @param source 380 * Identifier of the source node. 381 * @param target 382 * Identifier of the target node. 383 */ 384 public void compute(String source, String target) { 385 setSource(source); 386 setTarget(target); 387 compute(); 388 } 389 390 /** 391 * Clear the already computed path. This does not clear the source node 392 * name, the target node name and the weight attribute name. 393 */ 394 protected void clearAll() { 395 open.clear(); 396 closed.clear(); 397 398 result = null; 399 pathFound = false; 400 } 401 402 /** 403 * The A* algorithm proper. 404 * 405 * @param sourceNode 406 * The source node. 407 * @param targetNode 408 * The target node. 409 */ 410 protected void aStar(Node sourceNode, Node targetNode) { 411 clearAll(); 412 open.put( 413 sourceNode, 414 new AStarNode(sourceNode, null, null, 0, costs.heuristic( 415 sourceNode, targetNode))); 416 417 pathFound = false; 418 419 while (!open.isEmpty()) { 420 AStarNode current = getNextBetterNode(); 421 422 assert (current != null); 423 424 if (current.node == targetNode) { 425 // We found it ! 426 assert current.edge != null; 427 pathFound = true; 428 result = buildPath(current); 429 return; 430 } else { 431 open.remove(current.node); 432 closed.put(current.node, current); 433 434 // For each successor of the current node : 435 436 Iterator<? extends Edge> nexts = current.node 437 .getLeavingEdgeIterator(); 438 439 while (nexts.hasNext()) { 440 Edge edge = nexts.next(); 441 Node next = edge.getOpposite(current.node); 442 double h = costs.heuristic(next, targetNode); 443 double g = current.g + costs.cost(current.node, edge, next); 444 double f = g + h; 445 446 // If the node is already in open with a better rank, we 447 // skip it. 448 449 AStarNode alreadyInOpen = open.get(next); 450 451 if (alreadyInOpen != null && alreadyInOpen.rank <= f) 452 continue; 453 454 // If the node is already in closed with a better rank; we 455 // skip it. 456 457 AStarNode alreadyInClosed = closed.get(next); 458 459 if (alreadyInClosed != null && alreadyInClosed.rank <= f) 460 continue; 461 462 closed.remove(next); 463 open.put(next, new AStarNode(next, edge, current, g, h)); 464 } 465 } 466 } 467 } 468 469 /** 470 * Find the node with the lowest rank in the open list. 471 * 472 * @return The node of open that has the lowest rank. 473 */ 474 protected AStarNode getNextBetterNode() { 475 // TODO: consider using a priority queue here ? 476 // The problem is that we use open has a hash to ensure 477 // a node we will add to to open is not yet in it. 478 479 double min = Float.MAX_VALUE; 480 AStarNode theChosenOne = null; 481 482 for (AStarNode node : open.values()) { 483 if (node.rank < min) { 484 theChosenOne = node; 485 min = node.rank; 486 } 487 } 488 489 return theChosenOne; 490 } 491 492 // Nested classes 493 494 /** 495 * the distance between the current position and the target. 496 */ 497 public interface Costs { 498 /** 499 * Estimate cost from the given node to the target node. 500 * 501 * @param node 502 * A node. 503 * @param target 504 * The target node. 505 * @return The estimated cost between a node and a target node. 506 */ 507 double heuristic(Node node, Node target); 508 509 /** 510 * Cost of displacement from parent to next. The next node must be 511 * directly connected to parent, or -1 is returned. 512 * 513 * @param parent 514 * The node we come from. 515 * @param from 516 * The definition of an heuristic. The heuristic is in charge of evaluating 517 * The edge used between the two nodes (in case this is a 518 * multi-graph). 519 * @param next 520 * The node we go to. 521 * @return The real cost of moving from parent to next, or -1 if next is 522 * not directly connected to parent by an edge. 523 */ 524 double cost(Node parent, Edge from, Node next); 525 } 526 527 /** 528 * An implementation of the Costs interface that provides a default 529 * heuristic. It computes the G part using "weights" on edges. These weights 530 * must be stored in an attribute on edges. By default this attribute must 531 * be named "weight", but this can be changed. The weight attribute must be 532 * a {@link Number} an must be translatable to a double value. This implementation 533 * always return 0 for the H value. This makes the A* algorithm an 534 * equivalent of the Dijkstra algorithm. 535 */ 536 public static class DefaultCosts implements Costs { 537 /** 538 * The attribute used to retrieve the cost of an edge cross. 539 */ 540 protected String weightAttribute = "weight"; 541 542 /** 543 * New default costs for the A* algorithm. The cost of each edge is 544 * obtained from a numerical attribute stored under the name "weight". 545 * This attribute must be a descendant of Number (Double, Float, 546 * Integer, etc.). 547 */ 548 public DefaultCosts() { 549 } 550 551 /** 552 * New default costs for the A* algorithm. The cost of each edge is 553 * obtained from the attribute stored on each edge under the 554 * "weightAttributeName". This attribute must be a descendant of Number 555 * (Double, Float, Integer, etc.). 556 * 557 * @param weightAttributeName 558 * The name of cost attributes on edges. 559 */ 560 public DefaultCosts(String weightAttributeName) { 561 weightAttribute = weightAttributeName; 562 } 563 564 /** 565 * The heuristic. This one always returns zero, therefore transforming 566 * this A* into the Dijkstra algorithm. 567 * 568 * @return The estimation. 569 */ 570 public double heuristic(Node node, Node target) { 571 return 0; 572 } 573 574 /** 575 * The cost of moving from parent to next. If there is no cost 576 * attribute, the edge is considered to cost value "1". 577 * 578 * @param parent 579 * The node we come from. 580 * @param edge 581 * The edge between parent and next. 582 * @param next 583 * The node we go to. 584 * @return The movement cost. 585 */ 586 public double cost(Node parent, Edge edge, Node next) { 587 // Edge choice = parent.getEdgeToward( next.getId() ); 588 589 if (edge != null && edge.hasNumber(weightAttribute)) 590 return ((Number) edge.getNumber(weightAttribute)).doubleValue(); 591 592 return 1; 593 } 594 } 595 596 /** 597 * An implementation of the Costs interface that assume that the weight of 598 * edges is an Euclidean distance in 2D or 3D. No weight attribute is used. 599 * Instead, for the G value, the edge weights are used. For the H value the 600 * Euclidean distance in 2D or 3D between the current node and the target 601 * node is used. For this Costs implementation to work, the graph nodes must 602 * have a position (either individual "x", "y" and "z" attribute, or "xy" 603 * attribute or even "xyz" attributes. If there are only "x" and "y" or "xy" 604 * attribute this works in 2D, else the third coordinate is taken into 605 * account. 606 */ 607 public static class DistanceCosts implements AStar.Costs { 608 public double heuristic(Node node, Node target) { 609 double xy1[] = nodePosition(node); 610 double xy2[] = nodePosition(target); 611 612 double x = xy2[0] - xy1[0]; 613 double y = xy2[1] - xy1[1]; 614 double z = (xy1.length > 2 && xy2.length > 2) ? (xy2[2] - xy1[2]) 615 : 0; 616 617 return Math.sqrt((x * x) + (y * y) + (z * z)); 618 } 619 620 public double cost(Node parent, Edge edge, Node next) { 621 return edgeLength(edge);// parent.getEdgeToward( next.getId() ) ); 622 } 623 } 624 625 /** 626 * Representation of a node in the A* algorithm. 627 * 628 * <p> 629 * This representation contains : 630 * <ul> 631 * <li>the node itself;</li> 632 * <li>its parent node (to reconstruct the path);</li> 633 * <li>the g value (cost from the source to this node);</li> 634 * <li>the h value (estimated cost from this node to the target);</li> 635 * <li>the f value or rank, the sum of g and h.</li> 636 * </ul> 637 * </p> 638 */ 639 protected class AStarNode { 640 /** 641 * The node. 642 */ 643 public Node node; 644 645 /** 646 * The node's parent. 647 */ 648 public AStarNode parent; 649 650 /** 651 * The edge used to go from parent to node. 652 */ 653 public Edge edge; 654 655 /** 656 * Cost from the source node to this one. 657 */ 658 public double g; 659 660 /** 661 * Estimated cost from this node to the destination. 662 */ 663 public double h; 664 665 /** 666 * Sum of g and h. 667 */ 668 public double rank; 669 670 /** 671 * New A* node. 672 * 673 * @param node 674 * The node. 675 * @param edge 676 * The edge used to go from parent to node (useful for 677 * multi-graphs). 678 * @param parent 679 * It's parent node. 680 * @param g 681 * The cost from the source to this node. 682 * @param h 683 * The estimated cost from this node to the target. 684 */ 685 public AStarNode(Node node, Edge edge, AStarNode parent, double g, 686 double h) { 687 this.node = node; 688 this.edge = edge; 689 this.parent = parent; 690 this.g = g; 691 this.h = h; 692 this.rank = g + h; 693 } 694 } 695}