1 package org.opentrafficsim.road.network;
2
3 import java.util.Iterator;
4 import java.util.LinkedHashMap;
5 import java.util.List;
6 import java.util.Map;
7 import java.util.Set;
8 import java.util.SortedSet;
9 import java.util.TreeSet;
10
11 import org.djunits.Throw;
12 import org.djunits.value.vdouble.scalar.Length;
13 import org.djutils.immutablecollections.ImmutableSortedSet;
14 import org.djutils.immutablecollections.ImmutableTreeSet;
15 import org.djutils.multikeymap.MultiKeyMap;
16 import org.jgrapht.GraphPath;
17 import org.jgrapht.alg.shortestpath.DijkstraShortestPath;
18 import org.jgrapht.graph.SimpleDirectedWeightedGraph;
19 import org.opentrafficsim.base.Identifiable;
20 import org.opentrafficsim.core.dsol.OtsSimulatorInterface;
21 import org.opentrafficsim.core.gtu.GtuType;
22 import org.opentrafficsim.core.network.LateralDirectionality;
23 import org.opentrafficsim.core.network.Link;
24 import org.opentrafficsim.core.network.Network;
25 import org.opentrafficsim.core.network.NetworkException;
26 import org.opentrafficsim.core.network.Node;
27 import org.opentrafficsim.core.network.route.Route;
28 import org.opentrafficsim.road.network.lane.CrossSectionLink;
29 import org.opentrafficsim.road.network.lane.Lane;
30
31
32
33
34
35
36
37
38
39 public class RoadNetwork extends Network
40 {
41
42 private static final long serialVersionUID = 1L;
43
44
45 private Map<GtuType, RouteWeightedGraph> legalLaneGraph = new LinkedHashMap<>();
46
47
48 private RouteWeightedGraph physicalLaneGraph = null;
49
50
51 private MultiKeyMap<SortedSet<LaneChangeInfo>> legalLaneChangeInfoCache =
52 new MultiKeyMap<>(GtuType.class, Route.class, Lane.class);
53
54
55 private MultiKeyMap<SortedSet<LaneChangeInfo>> physicalLaneChangeInfoCache = new MultiKeyMap<>(Route.class, Lane.class);
56
57
58
59
60
61
62 public RoadNetwork(final String id, final OtsSimulatorInterface simulator)
63 {
64 super(id, simulator);
65 }
66
67
68
69
70
71
72
73
74
75
76
77
78
79 public ImmutableSortedSet<LaneChangeInfo> getLaneChangeInfo(final Lane lane, final Route route, final GtuType gtuType,
80 final Length range, final LaneAccessLaw laneAccessLaw)
81 {
82 Throw.whenNull(lane, "Lane may not be null.");
83 Throw.whenNull(route, "Route may not be null.");
84 Throw.whenNull(gtuType, "GTU type may not be null.");
85 Throw.whenNull(range, "Range may not be null.");
86 Throw.whenNull(laneAccessLaw, "Lane access law may not be null.");
87 Throw.when(range.le0(), IllegalArgumentException.class, "Range should be a positive value.");
88
89
90 SortedSet<LaneChangeInfo> info = getCompleteLaneChangeInfo(lane, route, gtuType, laneAccessLaw);
91 if (info == null)
92 {
93 return null;
94 }
95
96
97 LaneChangeInfo lcInfoBeyondHorizon = null;
98 Iterator<LaneChangeInfo> iterator = info.iterator();
99 while (lcInfoBeyondHorizon == null && iterator.hasNext())
100 {
101 LaneChangeInfo lcInfo = iterator.next();
102 if (lcInfo.getRemainingDistance().gt(range))
103 {
104 lcInfoBeyondHorizon = lcInfo;
105 }
106 }
107
108
109 if (lcInfoBeyondHorizon != null)
110 {
111 return new ImmutableTreeSet<>(info.headSet(lcInfoBeyondHorizon));
112 }
113 return new ImmutableTreeSet<>(info);
114 }
115
116
117
118
119
120
121
122
123
124
125
126 private SortedSet<LaneChangeInfo> getCompleteLaneChangeInfo(final Lane lane, final Route route, final GtuType gtuType,
127 final LaneAccessLaw laneAccessLaw)
128 {
129
130 SortedSet<LaneChangeInfo> outputLaneChangeInfo;
131 if (laneAccessLaw.equals(LaneAccessLaw.LEGAL))
132 {
133 outputLaneChangeInfo = this.legalLaneChangeInfoCache.get(gtuType, route, lane);
134
135 if (outputLaneChangeInfo == null)
136 {
137
138 RouteWeightedGraph graph = this.legalLaneGraph.get(gtuType);
139 if (graph == null)
140 {
141 graph = new RouteWeightedGraph();
142 this.legalLaneGraph.put(gtuType, graph);
143 buildGraph(graph, gtuType, laneAccessLaw);
144 }
145 List<LaneChangeInfoEdge> path = findPath(lane, graph, gtuType, route);
146
147 if (path != null)
148 {
149
150 boolean originalPath = true;
151 while (!path.isEmpty())
152 {
153 SortedSet<LaneChangeInfo> laneChangeInfo = extractLaneChangeInfo(path);
154 if (originalPath)
155 {
156 outputLaneChangeInfo = laneChangeInfo;
157 originalPath = false;
158 }
159 this.legalLaneChangeInfoCache.put(laneChangeInfo, gtuType, route, path.get(0).getFromLane());
160 path.remove(0);
161 }
162 }
163 }
164 }
165 else if (laneAccessLaw.equals(LaneAccessLaw.PHYSICAL))
166 {
167 outputLaneChangeInfo = this.physicalLaneChangeInfoCache.get(route, lane);
168
169 if (outputLaneChangeInfo == null)
170 {
171
172 if (this.physicalLaneGraph == null)
173 {
174 this.physicalLaneGraph = new RouteWeightedGraph();
175
176 buildGraph(this.physicalLaneGraph, gtuType, laneAccessLaw);
177 }
178 List<LaneChangeInfoEdge> path = findPath(lane, this.physicalLaneGraph, gtuType, route);
179
180 if (path != null)
181 {
182
183 boolean originalPath = true;
184 while (!path.isEmpty())
185 {
186 SortedSet<LaneChangeInfo> laneChangeInfo = extractLaneChangeInfo(path);
187 if (originalPath)
188 {
189 outputLaneChangeInfo = laneChangeInfo;
190 originalPath = false;
191 }
192 this.physicalLaneChangeInfoCache.put(laneChangeInfo, route, path.get(0).getFromLane());
193 path.remove(0);
194 }
195 }
196 }
197 }
198 else
199 {
200
201 throw new RuntimeException(String.format("Unknown LaneChangeLaw %s", laneAccessLaw));
202 }
203 return outputLaneChangeInfo;
204 }
205
206
207
208
209
210
211
212 private void buildGraph(final RouteWeightedGraph graph, final GtuType gtuType, final LaneAccessLaw laneChangeLaw)
213 {
214
215 for (Link link : this.getLinkMap().values())
216 {
217 for (Lane lane : ((CrossSectionLink) link).getLanes())
218 {
219 graph.addVertex(lane);
220 }
221
222 graph.addVertex(link.getEndNode());
223 }
224
225
226 boolean legal = laneChangeLaw.equals(LaneAccessLaw.LEGAL);
227 for (Link link : this.getLinkMap().values())
228 {
229 for (Lane lane : ((CrossSectionLink) link).getLanes())
230 {
231
232 for (LateralDirectionality lat : List.of(LateralDirectionality.LEFT, LateralDirectionality.RIGHT))
233 {
234 Set<Lane> adjacentLanes;
235 if (legal)
236 {
237 adjacentLanes = lane.accessibleAdjacentLanesLegal(lat, gtuType);
238 }
239 else
240 {
241 adjacentLanes = lane.accessibleAdjacentLanesPhysical(lat, gtuType);
242 }
243 for (Lane adjacentLane : adjacentLanes)
244 {
245 LaneChangeInfoEdgeType type = lat.equals(LateralDirectionality.LEFT) ? LaneChangeInfoEdgeType.LEFT
246 : LaneChangeInfoEdgeType.RIGHT;
247
248 LaneChangeInfoEdge edge = new LaneChangeInfoEdge(lane, type, null);
249 graph.addEdge(lane, adjacentLane, edge);
250 }
251 }
252
253 Set<Lane> nextLanes = lane.nextLanes(legal ? gtuType : null);
254 for (Lane nextLane : nextLanes)
255 {
256 LaneChangeInfoEdge edge =
257 new LaneChangeInfoEdge(lane, LaneChangeInfoEdgeType.DOWNSTREAM, nextLane.getParentLink());
258 graph.addEdge(lane, nextLane, edge);
259 }
260
261 LaneChangeInfoEdge edge = new LaneChangeInfoEdge(lane, LaneChangeInfoEdgeType.DOWNSTREAM, null);
262 graph.addEdge(lane, lane.getParentLink().getEndNode(), edge);
263 }
264 }
265 }
266
267
268
269
270
271
272
273
274
275 private List<LaneChangeInfoEdge> findPath(final Lane lane, final RouteWeightedGraph graph, final GtuType gtuType,
276 final Route route)
277 {
278
279 Node destination = null;
280 Route routeForWeights = route;
281 if (route == null)
282 {
283 destination = graph.getNoRouteDestinationNode(gtuType);
284 try
285 {
286 routeForWeights = getShortestRouteBetween(gtuType, lane.getParentLink().getStartNode(), destination);
287 }
288 catch (NetworkException exception)
289 {
290
291 throw new RuntimeException("Could not find route to destination.", exception);
292 }
293 }
294 else
295 {
296
297 List<Node> nodes = route.getNodes();
298 for (int i = nodes.size() - 1; i > 0; i--)
299 {
300 Link link = getLink(nodes.get(i - 1), nodes.get(i));
301 if (link instanceof CrossSectionLink && !((CrossSectionLink) link).getLanes().isEmpty())
302 {
303 destination = nodes.get(i);
304 break;
305 }
306 }
307 Throw.whenNull(destination, "Route has no links with lanes, "
308 + "unable to find a suitable destination node regarding lane change information.");
309 }
310
311
312 graph.setRoute(routeForWeights);
313
314
315 GraphPath<Identifiable, LaneChangeInfoEdge> path = DijkstraShortestPath.findPathBetween(graph, lane, destination);
316 return path == null ? null : path.getEdgeList();
317 }
318
319
320
321
322
323
324 private SortedSet<LaneChangeInfo> extractLaneChangeInfo(final List<LaneChangeInfoEdge> path)
325 {
326 SortedSet<LaneChangeInfo> info = new TreeSet<>();
327 Length x = Length.ZERO;
328 int n = 0;
329 boolean inLateralState = false;
330 for (LaneChangeInfoEdge edge : path)
331 {
332 LaneChangeInfoEdgeType lcType = edge.getLaneChangeInfoEdgeType();
333 int lat = lcType.equals(LaneChangeInfoEdgeType.LEFT) ? -1 : (lcType.equals(LaneChangeInfoEdgeType.RIGHT) ? 1 : 0);
334
335
336 if (n * lat < 0)
337 {
338
339
340
341
342
343
344 break;
345 }
346
347
348 if (lat == 0)
349 {
350
351 if (inLateralState)
352 {
353
354 boolean isDeadEnd = false;
355 info.add(new LaneChangeInfo(Math.abs(n), x, isDeadEnd,
356 n < 0 ? LateralDirectionality.LEFT : LateralDirectionality.RIGHT));
357 inLateralState = false;
358
359 }
360 else
361 {
362
363 x = x.plus(edge.getFromLane().getLength());
364 }
365 }
366 else
367 {
368
369 if (!inLateralState)
370 {
371 x = x.plus(edge.getFromLane().getLength());
372 inLateralState = true;
373 }
374
375 n += lat;
376 }
377 }
378 return info;
379 }
380
381
382
383
384
385 public void clearLaneChangeInfoCache()
386 {
387 this.legalLaneGraph.clear();
388 this.physicalLaneGraph = null;
389 this.legalLaneChangeInfoCache = new MultiKeyMap<>(GtuType.class, Route.class, Lane.class);
390 this.physicalLaneChangeInfoCache = new MultiKeyMap<>(Route.class, Lane.class);
391 }
392
393
394
395
396
397
398
399
400
401
402
403
404 private class RouteWeightedGraph extends SimpleDirectedWeightedGraph<Identifiable, LaneChangeInfoEdge>
405 {
406
407
408 private static final long serialVersionUID = 20220923L;
409
410
411 private Route route;
412
413
414 private Node noRouteDestination = null;
415
416
417
418
419 RouteWeightedGraph()
420 {
421 super(LaneChangeInfoEdge.class);
422 }
423
424
425
426
427
428 public void setRoute(final Route route)
429 {
430 Throw.whenNull(route, "Route may not be null for lane change information.");
431 this.route = route;
432 }
433
434
435
436
437
438
439
440
441
442 @Override
443 public double getEdgeWeight(final LaneChangeInfoEdge e)
444 {
445 if (e.getLaneChangeInfoEdgeType().equals(LaneChangeInfoEdgeType.LEFT)
446 || e.getLaneChangeInfoEdgeType().equals(LaneChangeInfoEdgeType.RIGHT))
447 {
448 int indexEndNode = this.route.indexOf(e.getFromLane().getParentLink().getEndNode());
449 return 1.0 + 1.0 / indexEndNode;
450 }
451 Link toLink = e.getToLink();
452 if (toLink == null)
453 {
454 return 0.0;
455 }
456 if (this.route.contains(toLink.getEndNode())
457 && this.route.indexOf(toLink.getEndNode()) == this.route.indexOf(toLink.getStartNode()) + 1)
458 {
459 return 1.0;
460 }
461 return Double.POSITIVE_INFINITY;
462 }
463
464
465
466
467
468
469 public Node getNoRouteDestinationNode(final GtuType gtuType)
470 {
471 if (this.noRouteDestination == null)
472 {
473
474 Lane lane = null;
475 Iterator<Identifiable> iterator = this.vertexSet().iterator();
476 while (lane == null && iterator.hasNext())
477 {
478 Identifiable next = iterator.next();
479 if (next instanceof Lane)
480 {
481 lane = (Lane) next;
482 }
483 }
484 Throw.when(lane == null, RuntimeException.class, "Requesting destination node on network without lanes.");
485
486 try
487 {
488 Link link = lane.getParentLink();
489 Set<Link> downstreamLinks = link.getEndNode().nextLinks(gtuType, link);
490 while (downstreamLinks.size() == 1)
491 {
492 link = downstreamLinks.iterator().next();
493 downstreamLinks = link.getEndNode().nextLinks(gtuType, link);
494 }
495 Throw.when(downstreamLinks.size() > 1, RuntimeException.class, "Using null route on network with split. "
496 + "Unable to find a destination to find lane change info towards.");
497 this.noRouteDestination = link.getEndNode();
498 }
499 catch (NetworkException ne)
500 {
501 throw new RuntimeException("Requesting lane change info from link that does not allow the GTU type.", ne);
502 }
503 }
504 return this.noRouteDestination;
505 }
506 }
507
508
509
510
511
512
513
514
515
516
517
518
519 private static class LaneChangeInfoEdge
520 {
521
522 private final Lane fromLane;
523
524
525 private final LaneChangeInfoEdgeType laneChangeInfoEdgeType;
526
527
528 private final Link toLink;
529
530
531
532
533
534
535
536 LaneChangeInfoEdge(final Lane fromLane, final LaneChangeInfoEdgeType laneChangeInfoEdgeType, final Link toLink)
537 {
538 this.fromLane = fromLane;
539 this.laneChangeInfoEdgeType = laneChangeInfoEdgeType;
540 this.toLink = toLink;
541 }
542
543
544
545
546
547 public Lane getFromLane()
548 {
549 return this.fromLane;
550 }
551
552
553
554
555
556 public LaneChangeInfoEdgeType getLaneChangeInfoEdgeType()
557 {
558 return this.laneChangeInfoEdgeType;
559 }
560
561
562
563
564
565 public Link getToLink()
566 {
567 return this.toLink;
568 }
569
570 @Override
571 public String toString()
572 {
573 return "LaneChangeInfoEdge [fromLane=" + this.fromLane + "]";
574 }
575
576 }
577
578
579
580
581
582
583
584
585
586
587 private enum LaneChangeInfoEdgeType
588 {
589
590 LEFT,
591
592
593 RIGHT,
594
595
596 DOWNSTREAM;
597 }
598
599 }