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 java.util.Collections;
035import java.util.Comparator;
036import java.util.Iterator;
037import java.util.LinkedList;
038import java.util.Random;
039
040import org.graphstream.algorithm.generator.DorogovtsevMendesGenerator;
041import org.graphstream.algorithm.generator.Generator;
042import org.graphstream.graph.Edge;
043import org.graphstream.graph.Graph;
044import org.graphstream.graph.Node;
045import org.graphstream.graph.implementations.AdjacencyListGraph;
046import org.graphstream.stream.Sink;
047
048/**
049 * An implementation of the D* algorithm.
050 * 
051 * @author Guilhelm Savin
052 * 
053 * @reference Stentz, Anthony (1994),
054 *            "Optimal and Efficient Path Planning for Partially-Known Environments"
055 *            , Proceedings of the International Conference on Robotics and
056 *            Automation: 3310–3317
057 */
058public class DStar implements DynamicAlgorithm, Sink {
059
060        protected static enum Tag {
061                NEW, OPEN, CLOSED, LOWER, RAISE
062        }
063
064        public static final String STATE_ATTRIBUTE = "d*.state";
065        public static final String COST_ATTRIBUTE = "d*.cost";
066
067        protected String edgeWeightAttribute;
068        protected double defaultEdgeWeight;
069        protected State g, position;
070        protected LinkedList<State> openList;
071        protected Graph env;
072
073        protected final Comparator<State> stateComparator = new Comparator<State>() {
074                public int compare(State o1, State o2) {
075                        return (int) Math.signum(k(o1) - k(o2));
076                }
077        };
078
079        public DStar() {
080                edgeWeightAttribute = "weight";
081                defaultEdgeWeight = 1;
082                g = null;
083                env = null;
084                openList = new LinkedList<State>();
085        }
086
087        public void terminate() {
088                env.removeSink(this);
089        }
090
091        public void compute() {
092                while (processState() >= 0 && position.t != Tag.CLOSED)
093                        ;
094        }
095
096        public void init(Graph graph) {
097                openList.clear();
098                env = graph;
099                env.addSink(this);
100        }
101
102        public void init(Node source, Node target, Graph graph) {
103                init(graph);
104                g = getState(target);
105                g.h = 0;
106                insert(g);
107
108                position = getState(source);
109        }
110
111        protected State minState() {
112                Collections.sort(openList, stateComparator);
113                return openList.getFirst();
114        }
115
116        protected double getKMin() {
117                double kmin = Double.MAX_VALUE;
118
119                for (State x : openList)
120                        kmin = Math.min(kmin, k(x));
121
122                return kmin;
123        }
124
125        protected double processState() {
126                State x;
127                double kOld;
128
129                // L1
130                x = minState();
131
132                // L2
133                if (x == null)
134                        return -1;
135
136                // L3
137                kOld = getKMin();
138                // L4
139                delete(x);
140
141                // L6 - L11
142                for (State y : x) {
143                        if (y.t == Tag.CLOSED && y.h <= kOld && x.h > y.h + c(y, x)) {
144                                x.b = y;
145                                x.h = y.h + c(y, x);
146
147                                assert !Double.isNaN(x.h);
148                        }
149                }
150
151                // L13 - L57
152                for (State y : x) {
153                        if (y.t == Tag.NEW) {
154                                y.b = x;
155                                y.h = x.h + c(x, y);
156                                y.p = y.h;
157
158                                assert !Double.isNaN(y.h);
159                                insert(y);
160                        } else {
161                                if (y.b == x && y.h != x.h + c(x, y)) {
162                                        // L24 - L 27
163                                        if (y.t == Tag.OPEN) {
164                                                if (y.h < y.p)
165                                                        y.p = y.h;
166                                                y.h = x.h + c(x, y);
167
168                                                assert !Double.isNaN(y.h);
169                                        }
170                                        // L28 - L31
171                                        else {
172                                                y.h = x.h + c(x, y);
173                                                y.p = y.h;
174
175                                                assert !Double.isNaN(y.h);
176                                        }
177
178                                        insert(y);
179                                }
180                                // L34 -
181                                else {
182                                        if (y.b != x && y.h > x.h + c(x, y)) {
183                                                // L37 - L42
184                                                if (x.p >= x.h) {
185                                                        y.b = x;
186                                                        y.h = x.h + c(x, y);
187
188                                                        if (y.t == Tag.CLOSED)
189                                                                y.p = y.h;
190
191                                                        assert !Double.isNaN(y.h);
192                                                        insert(y);
193                                                }
194                                                // L43 - 46
195                                                else {
196                                                        x.p = x.h;
197                                                        insert(x);
198                                                }
199                                        }
200                                        // L47 - 54
201                                        else {
202                                                if (y.b != x && x.h > y.h + c(y, x)
203                                                                && y.t == Tag.CLOSED && y.h > kOld) {
204                                                        y.p = y.h;
205
206                                                        assert !Double.isNaN(y.h);
207                                                        insert(y);
208                                                }
209                                        }
210                                }
211                        }
212                }
213
214                return getKMin();
215        }
216
217        protected void modifyCost(String edgeId, double cval) {
218                Edge e = env.getEdge(edgeId);
219
220                if (e.isDirected())
221                        modifyCost(getState(e.getSourceNode()),
222                                        getState(e.getTargetNode()), cval);
223                else {
224                        modifyCost(getState(e.getNode0()), getState(e.getNode1()), cval);
225                        modifyCost(getState(e.getNode1()), getState(e.getNode0()), cval);
226                }
227        }
228
229        protected void modifyCost(State x, State y, double cval) {
230                Edge e = x.node.getEdgeBetween(y.node);
231
232                if (e != null)
233                        e.setAttribute(COST_ATTRIBUTE, cval);
234
235                if (x.b == y)
236                        x.b = null;
237                
238                if (x.t == Tag.CLOSED) {
239                        x.p = x.h;
240                        insert(x);
241                }
242        }
243
244        public State getState(Node n) {
245                State s = n.getAttribute(STATE_ATTRIBUTE);
246
247                if (s == null) {
248                        s = new State(n);
249                        n.addAttribute(STATE_ATTRIBUTE, s);
250                }
251
252                return s;
253        }
254
255        protected double c(State x, State y) {
256                Edge e = x.node.getEdgeBetween(y.node);
257
258                if (e != null) {
259                        if (e.hasNumber(COST_ATTRIBUTE))
260                                return e.getNumber(COST_ATTRIBUTE);
261                        else
262                                return defaultEdgeWeight;
263                }
264
265                return Double.NaN;
266        }
267
268        protected double k(State x) {
269                if (x.t != Tag.OPEN)
270                        return Double.NaN;
271
272                return Math.min(x.h, x.p);
273        }
274
275        protected void insert(State x) {
276                openList.add(x);
277                x.t = Tag.OPEN;
278                x.p = x.h;
279        }
280
281        protected void delete(State x) {
282                openList.remove(x);
283                x.t = Tag.CLOSED;
284        }
285
286        protected boolean isMonotonic(State xn, int n) {
287                State xi1 = xn;
288                State xi = xi1.b;
289
290                for (int i = n; i > 0; i--) {
291                        if (!((xi.t == Tag.CLOSED && xi.h < xi1.h) || (xi.t == Tag.OPEN && xi.p < xi1.h)))
292                                return false;
293
294                        xi1 = xi;
295                        xi = xi1.b;
296                }
297
298                return true;
299        }
300
301        public void markPath(String attribute, Object on, Object off) {
302                for (Node n : env)
303                        n.setAttribute(attribute, off);
304
305                for (Edge e : env.getEachEdge())
306                        e.setAttribute(attribute, off);
307
308                State s = position;
309
310                do {
311                        s.node.setAttribute(attribute, on);
312                        s.node.getEdgeBetween(s.b.node).setAttribute(attribute, on);
313                        s = s.b;
314                } while (s != g);
315
316                g.node.setAttribute(attribute, on);
317        }
318
319        protected class State implements Iterable<State> {
320                /**
321                 * Associated node
322                 */
323                Node node;
324
325                /**
326                 * Associated tag
327                 */
328                Tag t;
329
330                /**
331                 * Backpointer
332                 */
333                State b;
334
335                /**
336                 * Previous cost
337                 */
338                double p;
339
340                /**
341                 * 
342                 */
343                double h;
344
345                public State(Node node) {
346                        this.node = node;
347                        t = Tag.NEW;
348                        b = null;
349                        p = 0;
350                        h = 0;
351                }
352
353                public Iterator<State> iterator() {
354                        return new NeighborStateIterator(this);
355                }
356        }
357
358        protected class NeighborStateIterator implements Iterator<State> {
359
360                Node source;
361                Iterator<Edge> it;
362
363                public NeighborStateIterator(State x) {
364                        source = x.node;
365                        it = source.iterator();
366                }
367
368                public boolean hasNext() {
369                        return it.hasNext();
370                }
371
372                public State next() {
373                        return getState(it.next().getOpposite(source));
374                }
375
376                public void remove() {
377                }
378        }
379
380        public void edgeAttributeAdded(String sourceId, long timeId, String edgeId,
381                        String attribute, Object value) {
382                if (attribute.equals(edgeWeightAttribute) && value != null
383                                && value instanceof Number)
384                        modifyCost(edgeId, ((Number) value).doubleValue());
385        }
386
387        public void edgeAttributeChanged(String sourceId, long timeId,
388                        String edgeId, String attribute, Object oldValue, Object newValue) {
389                if (attribute.equals(edgeWeightAttribute) && newValue != null
390                                && newValue instanceof Number)
391                        modifyCost(edgeId, ((Number) newValue).doubleValue());
392        }
393
394        public void edgeAttributeRemoved(String sourceId, long timeId,
395                        String edgeId, String attribute) {
396                // Nothing to do
397        }
398
399        public void graphAttributeAdded(String sourceId, long timeId,
400                        String attribute, Object value) {
401                // Nothing to do
402        }
403
404        public void graphAttributeChanged(String sourceId, long timeId,
405                        String attribute, Object oldValue, Object newValue) {
406                // Nothing to do
407        }
408
409        public void graphAttributeRemoved(String sourceId, long timeId,
410                        String attribute) {
411                // Nothing to do
412        }
413
414        public void nodeAttributeAdded(String sourceId, long timeId, String nodeId,
415                        String attribute, Object value) {
416                // Nothing to do
417        }
418
419        public void nodeAttributeChanged(String sourceId, long timeId,
420                        String nodeId, String attribute, Object oldValue, Object newValue) {
421                // Nothing to do
422        }
423
424        public void nodeAttributeRemoved(String sourceId, long timeId,
425                        String nodeId, String attribute) {
426                // Nothing to do
427        }
428
429        public void edgeAdded(String sourceId, long timeId, String edgeId,
430                        String fromNodeId, String toNodeId, boolean directed) {
431                modifyCost(edgeId, defaultEdgeWeight);
432        }
433
434        public void edgeRemoved(String sourceId, long timeId, String edgeId) {
435                modifyCost(edgeId, Double.NaN);
436        }
437
438        public void graphCleared(String sourceId, long timeId) {
439                //
440                // WTF ? You want a shortest path but you clear the graph.
441                //
442                throw new RuntimeException();
443        }
444
445        public void nodeAdded(String sourceId, long timeId, String nodeId) {
446                // Nothing to do
447        }
448
449        public void nodeRemoved(String sourceId, long timeId, String nodeId) {
450                Node n = env.getNode(nodeId);
451                State s = getState(n);
452
453                if (s == g || s == position) {
454                        //
455                        // WTF ? How does the robot go to g if you remove it ??
456                        //
457                        throw new RuntimeException();
458                }
459
460                Iterator<State> it = new NeighborStateIterator(s);
461
462                while (it.hasNext()) {
463                        State o = it.next();
464
465                        modifyCost(s, o, Double.NaN);
466                        modifyCost(o, s, Double.NaN);
467                }
468        }
469
470        public void stepBegins(String sourceId, long timeId, double step) {
471                // Nothing to do
472        }
473
474        public static void main(String... args) {
475                Generator gen = new DorogovtsevMendesGenerator();
476                AdjacencyListGraph g = new AdjacencyListGraph("g");
477                DStar dstar = new DStar();
478                Random r = new Random();
479                boolean alive = true;
480
481                g
482                                .addAttribute(
483                                                "ui.stylesheet",
484                                                "node.on { fill-color: red; } node.off { fill-color: black; } edge.on { fill-color: red; } edge.off { fill-color: black; }");
485
486                gen.addSink(g);
487                g.display(true);
488
489                gen.begin();
490                for (int i = 0; i < 150; i++)
491                        gen.nextEvents();
492
493                dstar.init(Toolkit.randomNode(g), Toolkit.randomNode(g), g);
494
495                do {
496                        dstar.compute();
497                        dstar.markPath("ui.class", "on", "off");
498
499                        try {
500                                Thread.sleep(2000);
501                        } catch (Exception e) {
502                        }
503
504                        State s = dstar.position;
505
506                        while (s.b != dstar.g && s.b != null && r.nextBoolean())
507                                s = s.b;
508
509                        if (r.nextBoolean() && s != dstar.position && s.b != dstar.g) {
510                                g.removeNode(s.node);
511                        } else {
512                                g.removeEdge(s.node.getEdgeBetween(s.b.node));
513                        }
514                        
515                        gen.nextEvents();
516                } while (alive);
517
518                gen.end();
519                dstar.terminate();
520
521        }
522}