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}