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}