View Javadoc
1   package org.opentrafficsim.core.network;
2   
3   import java.awt.geom.Rectangle2D;
4   import java.io.Serializable;
5   import java.rmi.RemoteException;
6   import java.util.ArrayList;
7   import java.util.HashMap;
8   import java.util.HashSet;
9   import java.util.LinkedHashSet;
10  import java.util.List;
11  import java.util.Map;
12  import java.util.Set;
13  
14  import javax.vecmath.Point3d;
15  
16  import org.djutils.immutablecollections.Immutable;
17  import org.djutils.immutablecollections.ImmutableHashMap;
18  import org.djutils.immutablecollections.ImmutableMap;
19  import org.jgrapht.GraphPath;
20  import org.jgrapht.alg.shortestpath.DijkstraShortestPath;
21  import org.jgrapht.graph.SimpleDirectedWeightedGraph;
22  import org.opentrafficsim.core.gtu.GTU;
23  import org.opentrafficsim.core.gtu.GTUType;
24  import org.opentrafficsim.core.network.route.CompleteRoute;
25  import org.opentrafficsim.core.network.route.Route;
26  import org.opentrafficsim.core.object.InvisibleObjectInterface;
27  import org.opentrafficsim.core.object.ObjectInterface;
28  import org.opentrafficsim.core.perception.PerceivableContext;
29  
30  import nl.tudelft.simulation.dsol.logger.SimLogger;
31  import nl.tudelft.simulation.event.EventProducer;
32  import nl.tudelft.simulation.language.d3.BoundingBox;
33  
34  /**
35   * A Network consists of a set of links. Each link has, in its turn, a start node and an end node.
36   * <p>
37   * Copyright (c) 2013-2019 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
38   * BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
39   * <p>
40   * $LastChangedDate: 2019-01-06 01:35:05 +0100 (Sun, 06 Jan 2019) $, @version $Revision: 4831 $, by $Author: averbraeck $,
41   * initial version Jul 22, 2015 <br>
42   * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
43   * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
44   * @author <a href="http://www.citg.tudelft.nl">Guus Tamminga</a>
45   */
46  public class OTSNetwork extends EventProducer implements Network, PerceivableContext, Serializable
47  {
48      /** */
49      private static final long serialVersionUID = 20150722;
50  
51      /** Id of this network. */
52      private final String id;
53  
54      /** Map of Nodes. */
55      private Map<String, Node> nodeMap = new HashMap<>();
56  
57      /** Map of Links. */
58      private Map<String, Link> linkMap = new HashMap<>();
59  
60      /** Map of ObjectInterface. */
61      private Map<String, ObjectInterface> objectMap = new HashMap<>();
62  
63      /** Map of InvisibleObjects. */
64      private Map<String, InvisibleObjectInterface> invisibleObjectMap = new HashMap<>();
65  
66      /** Map of Routes. */
67      private Map<GTUType, Map<String, Route>> routeMap = new HashMap<>();
68  
69      /** Graphs to calculate shortest paths per GTUType. */
70      private Map<GTUType, SimpleDirectedWeightedGraph<Node, LinkEdge<Link>>> linkGraphs = new HashMap<>();
71  
72      /** GTUs registered in this network. */
73      private Map<String, GTU> gtuMap = new HashMap<>();
74  
75      /**
76       * Construction of an empty network.
77       * @param id String; the network id.
78       */
79      public OTSNetwork(final String id)
80      {
81          this.id = id;
82      }
83  
84      /** {@inheritDoc} */
85      @Override
86      public final String getId()
87      {
88          return this.id;
89      }
90  
91      /***************************************************************************************/
92      /**************************************** NODES ****************************************/
93      /***************************************************************************************/
94  
95      /** {@inheritDoc} */
96      @Override
97      public final ImmutableMap<String, Node> getNodeMap()
98      {
99          return new ImmutableHashMap<>(this.nodeMap, Immutable.WRAP);
100     }
101 
102     /**
103      * @return the original NodeMap; only to be used in the 'network' package for cloning.
104      */
105     final Map<String, Node> getRawNodeMap()
106     {
107         return this.nodeMap;
108     }
109 
110     /** {@inheritDoc} */
111     @Override
112     public final void addNode(final Node node) throws NetworkException
113     {
114         if (containsNode(node))
115         {
116             throw new NetworkException("Node " + node + " already registered in network " + this.id);
117         }
118         if (this.nodeMap.keySet().contains(node.getId()))
119         {
120             throw new NetworkException("Node with name " + node.getId() + " already registered in network " + this.id);
121         }
122         this.nodeMap.put(node.getId(), node);
123         fireEvent(Network.NODE_ADD_EVENT, node.getId());
124         fireEvent(Network.ANIMATION_NODE_ADD_EVENT, node);
125     }
126 
127     /** {@inheritDoc} */
128     @Override
129     public final void removeNode(final Node node) throws NetworkException
130     {
131         if (!containsNode(node))
132         {
133             throw new NetworkException("Node " + node + " not registered in network " + this.id);
134         }
135         fireEvent(Network.NODE_REMOVE_EVENT, node.getId());
136         fireEvent(Network.ANIMATION_NODE_REMOVE_EVENT, node);
137         this.nodeMap.remove(node.getId());
138     }
139 
140     /** {@inheritDoc} */
141     @Override
142     public final boolean containsNode(final Node node)
143     {
144         // System.out.println(node);
145         return this.nodeMap.keySet().contains(node.getId());
146     }
147 
148     /** {@inheritDoc} */
149     @Override
150     public final boolean containsNode(final String nodeId)
151     {
152         return this.nodeMap.keySet().contains(nodeId);
153     }
154 
155     /** {@inheritDoc} */
156     @Override
157     public final Node getNode(final String nodeId)
158     {
159         return this.nodeMap.get(nodeId);
160     }
161 
162     /***************************************************************************************/
163     /**************************************** LINKS ****************************************/
164     /***************************************************************************************/
165 
166     /** {@inheritDoc} */
167     @Override
168     public final ImmutableMap<String, Link> getLinkMap()
169     {
170         return new ImmutableHashMap<>(this.linkMap, Immutable.WRAP);
171     }
172 
173     /**
174      * @return the original LinkMap; only to be used in the 'network' package for cloning.
175      */
176     final Map<String, Link> getRawLinkMap()
177     {
178         return this.linkMap;
179     }
180 
181     /** {@inheritDoc} */
182     @Override
183     public final void addLink(final Link link) throws NetworkException
184     {
185         if (containsLink(link))
186         {
187             throw new NetworkException("Link " + link + " already registered in network " + this.id);
188         }
189         if (this.linkMap.keySet().contains(link.getId()))
190         {
191             throw new NetworkException("Link with name " + link.getId() + " already registered in network " + this.id);
192         }
193         if (!containsNode(link.getStartNode()) || !containsNode(link.getEndNode()))
194         {
195             throw new NetworkException(
196                     "Start node or end node of Link " + link.getId() + " not registered in network " + this.id);
197         }
198         this.linkMap.put(link.getId(), link);
199         fireEvent(Network.LINK_ADD_EVENT, link.getId());
200         fireEvent(Network.ANIMATION_LINK_ADD_EVENT, link);
201     }
202 
203     /** {@inheritDoc} */
204     @Override
205     public final void removeLink(final Link link) throws NetworkException
206     {
207         if (!containsLink(link))
208         {
209             throw new NetworkException("Link " + link + " not registered in network " + this.id);
210         }
211         fireEvent(Network.LINK_REMOVE_EVENT, link.getId());
212         fireEvent(Network.ANIMATION_LINK_REMOVE_EVENT, link);
213         this.linkMap.remove(link.getId());
214     }
215 
216     /** {@inheritDoc} */
217     @Override
218     public final Link getLink(final Node node1, final Node node2)
219     {
220         for (Link link : this.linkMap.values())
221         {
222             if (link.getStartNode().equals(node1) && link.getEndNode().equals(node2))
223             {
224                 return link;
225             }
226         }
227         return null;
228     }
229 
230     /** {@inheritDoc} */
231     @Override
232     public final Link getLink(final String nodeId1, final String nodeId2) throws NetworkException
233     {
234         if (!containsNode(nodeId1))
235         {
236             throw new NetworkException("Node " + nodeId1 + " not in network " + this.id);
237         }
238         if (!containsNode(nodeId2))
239         {
240             throw new NetworkException("Node " + nodeId2 + " not in network " + this.id);
241         }
242         return getLink(getNode(nodeId1), getNode(nodeId2));
243     }
244 
245     /** {@inheritDoc} */
246     @Override
247     public final boolean containsLink(final Link link)
248     {
249         return this.linkMap.keySet().contains(link.getId());
250     }
251 
252     /** {@inheritDoc} */
253     @Override
254     public final boolean containsLink(final String linkId)
255     {
256         return this.linkMap.keySet().contains(linkId);
257     }
258 
259     /** {@inheritDoc} */
260     @Override
261     public final Link getLink(final String linkId)
262     {
263         return this.linkMap.get(linkId);
264     }
265 
266     /***************************************************************************************/
267     /************************ OBJECT INTERFACE IMPLEMENTING OBJECTS ************************/
268     /***************************************************************************************/
269 
270     /** {@inheritDoc} */
271     @Override
272     public final ImmutableMap<String, ObjectInterface> getObjectMap()
273     {
274         return new ImmutableHashMap<>(this.objectMap, Immutable.WRAP);
275     }
276 
277     /**
278      * @return the original ObjectMap; only to be used in the 'network' package for cloning.
279      */
280     final Map<String, ObjectInterface> getRawObjectMap()
281     {
282         return this.objectMap;
283     }
284 
285     /** {@inheritDoc} */
286     @SuppressWarnings("unchecked")
287     @Override
288     public final <T extends ObjectInterface> ImmutableMap<String, T> getObjectMap(final Class<T> objectType)
289     {
290         Map<String, T> result = new HashMap<>();
291         for (String key : this.objectMap.keySet())
292         {
293             ObjectInterface o = this.objectMap.get(key);
294             if (objectType.isInstance(o))
295             {
296                 result.put(key, (T) o);
297             }
298         }
299         return new ImmutableHashMap<>(result, Immutable.WRAP);
300     }
301 
302     /** {@inheritDoc} */
303     @Override
304     public final void addObject(final ObjectInterface object) throws NetworkException
305     {
306         if (containsObject(object))
307         {
308             throw new NetworkException("Object " + object + " already registered in network " + this.id);
309         }
310         if (containsObject(object.getFullId()))
311         {
312             throw new NetworkException("Object with name " + object.getFullId() + " already registered in network " + this.id);
313         }
314         this.objectMap.put(object.getFullId(), object);
315         fireEvent(Network.OBJECT_ADD_EVENT, object.getFullId());
316         fireEvent(Network.ANIMATION_OBJECT_ADD_EVENT, object);
317     }
318 
319     /** {@inheritDoc} */
320     @Override
321     public final void removeObject(final ObjectInterface object) throws NetworkException
322     {
323         if (!containsObject(object))
324         {
325             throw new NetworkException("Object " + object + " not registered in network " + this.id);
326         }
327         fireEvent(Network.OBJECT_REMOVE_EVENT, object.getFullId());
328         fireEvent(Network.ANIMATION_OBJECT_REMOVE_EVENT, object);
329         this.objectMap.remove(object.getFullId());
330     }
331 
332     /** {@inheritDoc} */
333     @Override
334     public final boolean containsObject(final ObjectInterface object)
335     {
336         return this.objectMap.containsKey(object.getFullId());
337     }
338 
339     /**
340      * {@inheritDoc}
341      * <p>
342      * Note that the objectId should be the <b>fullId</b> of the object, including any additions such as lane ids, link ids,
343      * etc.
344      */
345     @Override
346     public final boolean containsObject(final String objectId)
347     {
348         return this.objectMap.containsKey(objectId);
349     }
350 
351     /***************************************************************************************/
352     /********************************* INVISIBLE OBJECTS ***********************************/
353     /***************************************************************************************/
354 
355     /** {@inheritDoc} */
356     @Override
357     public final ImmutableMap<String, InvisibleObjectInterface> getInvisibleObjectMap()
358     {
359         return new ImmutableHashMap<>(this.invisibleObjectMap, Immutable.WRAP);
360     }
361 
362     /**
363      * @return the original InvisibleObjectMap; only to be used in the 'network' package for cloning.
364      */
365     final Map<String, InvisibleObjectInterface> getRawInvisibleObjectMap()
366     {
367         return this.invisibleObjectMap;
368     }
369 
370     /** {@inheritDoc} */
371     @Override
372     public final ImmutableMap<String, InvisibleObjectInterface> getInvisibleObjectMap(
373             final Class<InvisibleObjectInterface> objectType)
374     {
375         Map<String, InvisibleObjectInterface> result = new HashMap<>();
376         for (String key : this.objectMap.keySet())
377         {
378             InvisibleObjectInterface o = this.invisibleObjectMap.get(key);
379             if (objectType.isInstance(o))
380             {
381                 result.put(key, o);
382             }
383         }
384         return new ImmutableHashMap<>(result, Immutable.WRAP);
385     }
386 
387     /** {@inheritDoc} */
388     @Override
389     public final void addInvisibleObject(final InvisibleObjectInterface object) throws NetworkException
390     {
391         if (containsInvisibleObject(object))
392         {
393             throw new NetworkException("InvisibleObject " + object + " already registered in network " + this.id);
394         }
395         if (containsInvisibleObject(object.getFullId()))
396         {
397             throw new NetworkException(
398                     "InvisibleObject with name " + object.getFullId() + " already registered in network " + this.id);
399         }
400         this.invisibleObjectMap.put(object.getFullId(), object);
401         fireEvent(Network.INVISIBLE_OBJECT_ADD_EVENT, object.getFullId());
402         fireEvent(Network.ANIMATION_INVISIBLE_OBJECT_ADD_EVENT, object);
403     }
404 
405     /** {@inheritDoc} */
406     @Override
407     public final void removeInvisibleObject(final InvisibleObjectInterface object) throws NetworkException
408     {
409         if (!containsInvisibleObject(object))
410         {
411             throw new NetworkException("InvisibleObject " + object + " not registered in network " + this.id);
412         }
413         fireEvent(Network.INVISIBLE_OBJECT_REMOVE_EVENT, object.getFullId());
414         fireEvent(Network.ANIMATION_INVISIBLE_OBJECT_REMOVE_EVENT, object);
415         this.objectMap.remove(object.getFullId());
416     }
417 
418     /** {@inheritDoc} */
419     @Override
420     public final boolean containsInvisibleObject(final InvisibleObjectInterface object)
421     {
422         return this.invisibleObjectMap.containsKey(object.getFullId());
423     }
424 
425     /**
426      * {@inheritDoc}
427      * <p>
428      * Note that the objectId should be the <b>fullId</b> of the object, including any additions such as lane ids, link ids,
429      * etc.
430      */
431     @Override
432     public final boolean containsInvisibleObject(final String objectId)
433     {
434         return this.invisibleObjectMap.containsKey(objectId);
435     }
436 
437     /***************************************************************************************/
438     /*************************************** ROUTES ****************************************/
439     /***************************************************************************************/
440 
441     /** {@inheritDoc} */
442     @Override
443     public final ImmutableMap<String, Route> getDefinedRouteMap(final GTUType gtuType)
444     {
445         Map<String, Route> routes = new HashMap<>();
446         if (this.routeMap.containsKey(gtuType))
447         {
448             routes.putAll(this.routeMap.get(gtuType));
449         }
450         return new ImmutableHashMap<>(routes, Immutable.WRAP);
451     }
452 
453     /** {@inheritDoc} */
454     @Override
455     public final void addRoute(final GTUType gtuType, final Route route) throws NetworkException
456     {
457         if (containsRoute(gtuType, route))
458         {
459             throw new NetworkException(
460                     "Route " + route + " for GTUType " + gtuType + " already registered in network " + this.id);
461         }
462         if (this.routeMap.containsKey(gtuType) && this.routeMap.get(gtuType).keySet().contains(route.getId()))
463         {
464             throw new NetworkException("Route with name " + route.getId() + " for GTUType " + gtuType
465                     + " already registered in network " + this.id);
466         }
467         for (Node node : route.getNodes())
468         {
469             if (!containsNode(node))
470             {
471                 throw new NetworkException("Node " + node.getId() + " of route " + route.getId() + " for GTUType " + gtuType
472                         + " not registered in network " + this.id);
473             }
474         }
475         if (!this.routeMap.containsKey(gtuType))
476         {
477             this.routeMap.put(gtuType, new HashMap<String, Route>());
478         }
479         this.routeMap.get(gtuType).put(route.getId(), route);
480         fireEvent(Network.ROUTE_ADD_EVENT, new Object[] { gtuType.getId(), route.getId() });
481         fireEvent(Network.ANIMATION_ROUTE_ADD_EVENT, new Object[] { gtuType, route });
482     }
483 
484     /** {@inheritDoc} */
485     @Override
486     public final void removeRoute(final GTUType gtuType, final Route route) throws NetworkException
487     {
488         if (!containsRoute(gtuType, route))
489         {
490             throw new NetworkException("Route " + route + " for GTUType " + gtuType + " not registered in network " + this.id);
491         }
492         fireEvent(Network.ROUTE_REMOVE_EVENT, new Object[] { gtuType.getId(), route.getId() });
493         fireEvent(Network.ANIMATION_ROUTE_REMOVE_EVENT, new Object[] { gtuType, route });
494         this.routeMap.get(gtuType).remove(route.getId());
495     }
496 
497     /** {@inheritDoc} */
498     @Override
499     public final boolean containsRoute(final GTUType gtuType, final Route route)
500     {
501         if (this.routeMap.containsKey(gtuType))
502         {
503             return this.routeMap.get(gtuType).values().contains(route);
504         }
505         return false;
506     }
507 
508     /** {@inheritDoc} */
509     @Override
510     public final boolean containsRoute(final GTUType gtuType, final String routeId)
511     {
512         if (this.routeMap.containsKey(gtuType))
513         {
514             return this.routeMap.get(gtuType).keySet().contains(routeId);
515         }
516         return false;
517     }
518 
519     /**
520      * Returns the route with given id or {@code null} if no such route is available.
521      * @param routeId String; route id
522      * @return route with given id or {@code null} if no such route is available
523      */
524     public final Route getRoute(final String routeId)
525     {
526         for (GTUType gtuType : this.routeMap.keySet())
527         {
528             Route route = this.routeMap.get(gtuType).get(routeId);
529             if (route != null)
530             {
531                 return route;
532             }
533         }
534         return null;
535     }
536 
537     /** {@inheritDoc} */
538     @Override
539     public final Route getRoute(final GTUType gtuType, final String routeId)
540     {
541         if (this.routeMap.containsKey(gtuType))
542         {
543             return this.routeMap.get(gtuType).get(routeId);
544         }
545         return null;
546     }
547 
548     /** {@inheritDoc} */
549     @Override
550     public final Set<Route> getRoutesBetween(final GTUType gtuType, final Node nodeFrom, final Node nodeTo)
551     {
552         Set<Route> routes = new LinkedHashSet<>();
553         if (this.routeMap.containsKey(gtuType))
554         {
555             for (Route route : this.routeMap.get(gtuType).values())
556             {
557                 try
558                 {
559                     if (route.originNode().equals(nodeFrom) && route.destinationNode().equals(nodeTo))
560                     {
561                         routes.add(route);
562                     }
563                 }
564                 catch (NetworkException ne)
565                 {
566                     // thrown if no nodes exist in the route. Do not add the route in that case.
567                 }
568             }
569         }
570         return routes;
571     }
572 
573     /** {@inheritDoc} */
574     @Override
575     public final void buildGraph(final GTUType gtuType)
576     {
577         SimpleDirectedWeightedGraph<Node, LinkEdge<Link>> graph = buildGraph(gtuType, LinkWeight.LENGTH);
578         this.linkGraphs.put(gtuType, graph);
579     }
580 
581     /**
582      * Builds a graph using the specified link weight.
583      * @param gtuType GTUType; GTU type
584      * @param linkWeight LinkWeight; link weight
585      * @return SimpleDirectedWeightedGraph graph
586      */
587     private SimpleDirectedWeightedGraph<Node, LinkEdge<Link>> buildGraph(final GTUType gtuType, final LinkWeight linkWeight)
588     {
589         // TODO: take connections into account, and possibly do node expansion to build the graph
590         @SuppressWarnings({ "unchecked" })
591         // TODO: the next line with .class has problems compiling... So used a dirty hack instead for now...
592         Class<LinkEdge<Link>> linkEdgeClass = (Class<LinkEdge<Link>>) new LinkEdge<OTSLink>(null).getClass();
593         SimpleDirectedWeightedGraph<Node, LinkEdge<Link>> graph = new SimpleDirectedWeightedGraph<>(linkEdgeClass);
594         for (Node node : this.nodeMap.values())
595         {
596             graph.addVertex(node);
597         }
598         for (Link link : this.linkMap.values())
599         {
600             // determine if the link is accessible for the GTUType , and in which direction(s)
601             LongitudinalDirectionality directionality = link.getDirectionality(gtuType);
602             if (directionality.isForwardOrBoth())
603             {
604                 LinkEdge<Link> linkEdge = new LinkEdge<>(link);
605                 graph.addEdge(link.getStartNode(), link.getEndNode(), linkEdge);
606                 graph.setEdgeWeight(linkEdge, linkWeight.getWeight(link));
607             }
608             if (directionality.isBackwardOrBoth())
609             {
610                 LinkEdge<Link> linkEdge = new LinkEdge<>(link);
611                 graph.addEdge(link.getEndNode(), link.getStartNode(), linkEdge);
612                 graph.setEdgeWeight(linkEdge, linkWeight.getWeight(link));
613             }
614         }
615         return graph;
616     }
617 
618     /** {@inheritDoc} */
619     @Override
620     public final CompleteRoute getShortestRouteBetween(final GTUType gtuType, final Node nodeFrom, final Node nodeTo,
621             final LinkWeight linkWeight) throws NetworkException
622     {
623         CompleteRoute route = new CompleteRoute("Route for " + gtuType + " from " + nodeFrom + "to " + nodeTo, gtuType);
624         SimpleDirectedWeightedGraph<Node, LinkEdge<Link>> graph = getGraph(gtuType, linkWeight);
625         // DijkstraShortestPath<Node, LinkEdge<Link>> dijkstra = new DijkstraShortestPath<>(graph);
626         // GraphPath<Node, LinkEdge<Link>> path = dijkstra.getPath(nodeFrom, nodeTo);
627         GraphPath<Node, LinkEdge<Link>> path = DijkstraShortestPath.findPathBetween(graph, nodeFrom, nodeTo);
628         if (path == null)
629         {
630             return null;
631         }
632         route.addNode(nodeFrom);
633         for (LinkEdge<Link> link : path.getEdgeList())
634         {
635             if (!link.getLink().getEndNode().equals(route.destinationNode())
636                     && route.destinationNode().isDirectionallyConnectedTo(gtuType, link.getLink().getEndNode()))
637             {
638                 route.addNode(link.getLink().getEndNode());
639             }
640             else if (!link.getLink().getStartNode().equals(route.destinationNode())
641                     && route.destinationNode().isDirectionallyConnectedTo(gtuType, link.getLink().getStartNode()))
642             {
643                 route.addNode(link.getLink().getStartNode());
644             }
645             else
646             {
647                 throw new NetworkException("Cannot connect two links when calculating shortest route");
648             }
649         }
650         return route;
651     }
652 
653     /** {@inheritDoc} */
654     @Override
655     public final CompleteRoute getShortestRouteBetween(final GTUType gtuType, final Node nodeFrom, final Node nodeTo,
656             final List<Node> nodesVia) throws NetworkException
657     {
658         return getShortestRouteBetween(gtuType, nodeFrom, nodeTo, nodesVia, LinkWeight.LENGTH);
659     }
660 
661     /** {@inheritDoc} */
662     @Override
663     public final CompleteRoute getShortestRouteBetween(final GTUType gtuType, final Node nodeFrom, final Node nodeTo,
664             final List<Node> nodesVia, final LinkWeight linkWeight) throws NetworkException
665     {
666         CompleteRoute route = new CompleteRoute(
667                 "Route for " + gtuType + " from " + nodeFrom + "to " + nodeTo + " via " + nodesVia.toString(), gtuType);
668         SimpleDirectedWeightedGraph<Node, LinkEdge<Link>> graph = getGraph(gtuType, linkWeight);
669         List<Node> nodes = new ArrayList<>();
670         nodes.add(nodeFrom);
671         nodes.addAll(nodesVia);
672         nodes.add(nodeTo);
673         Node from = nodeFrom;
674         route.addNode(nodeFrom);
675         for (int i = 1; i < nodes.size(); i++)
676         {
677             Node to = nodes.get(i);
678             DijkstraShortestPath<Node, LinkEdge<Link>> dijkstra = new DijkstraShortestPath<>(graph);
679             GraphPath<Node, LinkEdge<Link>> path = dijkstra.getPath(from, to);
680             if (path == null)
681             {
682                 return null;
683             }
684             for (LinkEdge<Link> link : path.getEdgeList())
685             {
686                 if (!link.getLink().getEndNode().equals(route.destinationNode())
687                         && route.destinationNode().isDirectionallyConnectedTo(gtuType, link.getLink().getEndNode()))
688                 {
689                     route.addNode(link.getLink().getEndNode());
690                 }
691                 else if (!link.getLink().getStartNode().equals(route.destinationNode())
692                         && route.destinationNode().isDirectionallyConnectedTo(gtuType, link.getLink().getStartNode()))
693                 {
694                     route.addNode(link.getLink().getStartNode());
695                 }
696                 else
697                 {
698                     throw new NetworkException(
699                             "Cannot connect two links when calculating shortest route with intermediate nodes");
700                 }
701             }
702             from = to;
703         }
704         return route;
705     }
706 
707     /**
708      * Returns the graph, possibly a stored one.
709      * @param gtuType GTUType; GTU type
710      * @param linkWeight LinkWeight; link weight
711      * @return SimpleDirectedWeightedGraph
712      */
713     private SimpleDirectedWeightedGraph<Node, LinkEdge<Link>> getGraph(final GTUType gtuType, final LinkWeight linkWeight)
714     {
715         SimpleDirectedWeightedGraph<Node, LinkEdge<Link>> graph;
716         if (linkWeight.equals(LinkWeight.LENGTH))
717         {
718             // stored default
719             if (!this.linkGraphs.containsKey(gtuType))
720             {
721                 buildGraph(gtuType);
722             }
723             graph = this.linkGraphs.get(gtuType);
724         }
725         else
726         {
727             graph = buildGraph(gtuType, linkWeight);
728         }
729         return graph;
730     }
731 
732     /**
733      * @return a defensive copy of the routeMap.
734      */
735     public final ImmutableMap<GTUType, Map<String, Route>> getRouteMap()
736     {
737         return new ImmutableHashMap<>(this.routeMap, Immutable.WRAP);
738     }
739 
740     /**
741      * @return routeMap; only to be used in the 'network' package for cloning.
742      */
743     final Map<GTUType, Map<String, Route>> getRawRouteMap()
744     {
745         return this.routeMap;
746     }
747 
748     /**
749      * @param newRouteMap Map&lt;GTUType,Map&lt;String,Route&gt;&gt;; the routeMap to set, only to be used in the 'network'
750      *            package for cloning.
751      */
752     final void setRawRouteMap(final Map<GTUType, Map<String, Route>> newRouteMap)
753     {
754         this.routeMap = newRouteMap;
755     }
756 
757     /**
758      * @return linkGraphs; only to be used in the 'network' package for cloning.
759      */
760     final ImmutableMap<GTUType, SimpleDirectedWeightedGraph<Node, LinkEdge<Link>>> getLinkGraphs()
761     {
762         return new ImmutableHashMap<>(this.linkGraphs, Immutable.WRAP);
763     }
764 
765     /**
766      * @return linkGraphs; only to be used in the 'network' package for cloning.
767      */
768     final Map<GTUType, SimpleDirectedWeightedGraph<Node, LinkEdge<Link>>> getRawLinkGraphs()
769     {
770         return this.linkGraphs;
771     }
772 
773     /***************************************************************************************/
774     /**************************************** GTUs *****************************************/
775     /***************************************************************************************/
776 
777     /** {@inheritDoc} */
778     @Override
779     public final void addGTU(final GTU gtu)
780     {
781         this.gtuMap.put(gtu.getId(), gtu);
782         fireTimedEvent(Network.GTU_ADD_EVENT, gtu.getId(), gtu.getSimulator().getSimulatorTime());
783         fireTimedEvent(Network.ANIMATION_GTU_ADD_EVENT, gtu, gtu.getSimulator().getSimulatorTime());
784     }
785 
786     /** {@inheritDoc} */
787     @Override
788     public final void removeGTU(final GTU gtu)
789     {
790         fireTimedEvent(Network.GTU_REMOVE_EVENT, gtu.getId(), gtu.getSimulator().getSimulatorTime());
791         fireTimedEvent(Network.ANIMATION_GTU_REMOVE_EVENT, gtu, gtu.getSimulator().getSimulatorTime());
792         this.gtuMap.remove(gtu.getId());
793     }
794 
795     /** {@inheritDoc} */
796     @Override
797     public final boolean containsGTU(final GTU gtu)
798     {
799         return this.gtuMap.containsValue(gtu);
800     }
801 
802     /** {@inheritDoc} */
803     @Override
804     public final GTU getGTU(final String gtuId)
805     {
806         return this.gtuMap.get(gtuId);
807     }
808 
809     /** {@inheritDoc} */
810     @Override
811     public final Set<GTU> getGTUs()
812     {
813         // defensive copy
814         return new HashSet<>(this.gtuMap.values());
815     }
816 
817     /** {@inheritDoc} */
818     @Override
819     public final boolean containsGtuId(final String gtuId)
820     {
821         return this.gtuMap.containsKey(gtuId);
822     }
823 
824     /**
825      * @return gtuMap
826      */
827     final Map<String, GTU> getRawGtuMap()
828     {
829         return this.gtuMap;
830     }
831 
832     /***************************************************************************************/
833 
834     /**
835      * Calculate the extent of the network based on the network objects' locations and return the dimensions.
836      * @return Rectangle2D.Double; the extent of the network
837      */
838     public Rectangle2D.Double getExtent()
839     {
840         double minX = Double.MAX_VALUE;
841         double minY = Double.MAX_VALUE;
842         double maxX = -Double.MAX_VALUE;
843         double maxY = -Double.MAX_VALUE;
844         boolean content = false;
845         Point3d p3dL = new Point3d();
846         Point3d p3dU = new Point3d();
847         try
848         {
849             for (Node node : this.nodeMap.values())
850             {
851                 BoundingBox b = new BoundingBox(node.getBounds());
852                 b.getLower(p3dL);
853                 b.getUpper(p3dU);
854                 minX = Math.min(minX, node.getLocation().x + Math.min(p3dL.x, p3dU.x));
855                 minY = Math.min(minY, node.getLocation().y + Math.min(p3dL.y, p3dU.y));
856                 maxX = Math.max(maxX, node.getLocation().x + Math.max(p3dL.x, p3dU.x));
857                 maxY = Math.max(maxY, node.getLocation().y + Math.max(p3dL.y, p3dU.y));
858                 content = true;
859             }
860             for (Link link : this.linkMap.values())
861             {
862                 BoundingBox b = new BoundingBox(link.getBounds());
863                 b.getLower(p3dL);
864                 b.getUpper(p3dU);
865                 minX = Math.min(minX, link.getLocation().x + Math.min(p3dL.x, p3dU.x));
866                 minY = Math.min(minY, link.getLocation().y + Math.min(p3dL.y, p3dU.y));
867                 maxX = Math.max(maxX, link.getLocation().x + Math.max(p3dL.x, p3dU.x));
868                 maxY = Math.max(maxY, link.getLocation().y + Math.max(p3dL.y, p3dU.y));
869                 content = true;
870             }
871             for (ObjectInterface object : this.objectMap.values())
872             {
873                 BoundingBox b = new BoundingBox(object.getBounds());
874                 b.getLower(p3dL);
875                 b.getUpper(p3dU);
876                 minX = Math.min(minX, object.getLocation().x + Math.min(p3dL.x, p3dU.x));
877                 minY = Math.min(minY, object.getLocation().y + Math.min(p3dL.y, p3dU.y));
878                 maxX = Math.max(maxX, object.getLocation().x + Math.max(p3dL.x, p3dU.x));
879                 maxY = Math.max(maxY, object.getLocation().y + Math.max(p3dL.y, p3dU.y));
880                 content = true;
881             }
882         }
883         catch (RemoteException exception)
884         {
885             SimLogger.always().error(exception);
886         }
887         if (content)
888         {
889             double relativeMargin = 0.05;
890             double xMargin = relativeMargin * (maxX - minX);
891             double yMargin = relativeMargin * (maxY - minY);
892             return new Rectangle2D.Double(minX - xMargin / 2, minY - yMargin / 2, maxX - minX + xMargin, maxY - minY + xMargin);
893         }
894         else
895         {
896             return new Rectangle2D.Double(-500, -500, 1000, 1000);
897         }
898     }
899 
900     /** {@inheritDoc} */
901     @Override
902     public final String toString()
903     {
904         return "OTSNetwork [id=" + this.id + ", nodeMapSize=" + this.nodeMap.size() + ", linkMapSize=" + this.linkMap.size()
905                 + ", objectMapSize=" + this.objectMap.size() + ", routeMapSize=" + this.routeMap.size() + ", gtuMapSize="
906                 + this.gtuMap.size() + "]";
907     }
908 
909 }