1 package org.opentrafficsim.road.gtu.lane.perception;
2
3 import java.io.Serializable;
4 import java.util.HashMap;
5 import java.util.HashSet;
6 import java.util.Iterator;
7 import java.util.LinkedHashMap;
8 import java.util.LinkedHashSet;
9 import java.util.List;
10 import java.util.Map;
11 import java.util.Set;
12 import java.util.SortedSet;
13 import java.util.TreeMap;
14 import java.util.TreeSet;
15
16 import org.djunits.value.vdouble.scalar.Length;
17 import org.djunits.value.vdouble.scalar.Time;
18 import org.djutils.exceptions.Throw;
19 import org.opentrafficsim.core.gtu.GTUDirectionality;
20 import org.opentrafficsim.core.gtu.GTUException;
21 import org.opentrafficsim.core.gtu.GTUType;
22 import org.opentrafficsim.core.gtu.RelativePosition;
23 import org.opentrafficsim.core.network.LateralDirectionality;
24 import org.opentrafficsim.core.network.Link;
25 import org.opentrafficsim.core.network.Node;
26 import org.opentrafficsim.core.network.route.Route;
27 import org.opentrafficsim.core.perception.Historical;
28 import org.opentrafficsim.core.perception.HistoricalValue;
29 import org.opentrafficsim.core.perception.HistoryManager;
30 import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
31 import org.opentrafficsim.road.gtu.lane.perception.RollingLaneStructureRecord.RecordLink;
32 import org.opentrafficsim.road.network.lane.CrossSectionLink;
33 import org.opentrafficsim.road.network.lane.DirectedLanePosition;
34 import org.opentrafficsim.road.network.lane.Lane;
35 import org.opentrafficsim.road.network.lane.LaneDirection;
36 import org.opentrafficsim.road.network.lane.object.LaneBasedObject;
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103 public class RollingLaneStructure implements LaneStructure, Serializable
104 {
105
106 private static final long serialVersionUID = 20160400L;
107
108
109 private final Historical<RollingLaneStructureRecord> root;
110
111
112 private Length lookAhead;
113
114
115 private Route previousRoute;
116
117
118 private TreeMap<RelativeLane, RollingLaneStructureRecord> crossSectionRecords = new TreeMap<>();
119
120
121 private TreeMap<RelativeLane, RollingLaneStructureRecord> firstRecords = new TreeMap<>();
122
123
124 private Map<RelativeLane, Set<RollingLaneStructureRecord>> relativeLaneMap = new HashMap<>();
125
126
127 private Map<LaneStructureRecord, RelativeLane> relativeLanes = new HashMap<>();
128
129
130 private final Set<Lane> ignoreSet = new HashSet<>();
131
132
133 private final Set<RollingLaneStructureRecord> upstreamEdge = new LinkedHashSet<>();
134
135
136 private final Set<RollingLaneStructureRecord> downstreamEdge = new LinkedHashSet<>();
137
138
139 private final Length down;
140
141
142 private final Length up;
143
144
145 private final Length downSplit;
146
147
148 private final Length upMerge;
149
150
151 private final LaneBasedGTU containingGtu;
152
153
154 @SuppressWarnings("checkstyle:visibilitymodifier")
155 public AnimationAccess animationAccess = new AnimationAccess();
156
157
158
159
160
161
162
163
164
165
166 public RollingLaneStructure(final Length lookAhead, final Length down, final Length up, final Length downSplit,
167 final Length upMerge, final LaneBasedGTU gtu)
168 {
169 HistoryManager historyManager = gtu.getSimulator().getReplication().getHistoryManager(gtu.getSimulator());
170 this.root = new HistoricalValue<>(historyManager);
171 this.lookAhead = lookAhead;
172 this.down = down;
173 this.up = up;
174 this.downSplit = downSplit;
175 this.upMerge = upMerge;
176 this.containingGtu = gtu;
177 }
178
179
180
181
182
183
184
185
186 @Override
187 public final void update(final DirectedLanePosition pos, final Route route, final GTUType gtuType) throws GTUException
188 {
189
190
191
192
193
194
195
196
197
198
199
200 Lane lane = pos.getLane();
201 GTUDirectionality direction = pos.getGtuDirection();
202 Length position = pos.getPosition();
203 double fracPos = direction.isPlus() ? position.si / lane.getLength().si : 1.0 - position.si / lane.getLength().si;
204
205
206 if (this.previousRoute != route || this.root.get() == null)
207 {
208
209 this.previousRoute = route;
210
211
212 this.upstreamEdge.clear();
213 this.downstreamEdge.clear();
214 this.crossSectionRecords.clear();
215 this.relativeLanes.clear();
216
217
218 RollingLaneStructureRecord newRoot = constructRecord(lane, direction, null, RecordLink.CROSS, RelativeLane.CURRENT);
219 this.root.set(newRoot);
220 this.crossSectionRecords.put(RelativeLane.CURRENT, newRoot);
221 for (LateralDirectionality latDirection : new LateralDirectionality[] { LateralDirectionality.LEFT,
222 LateralDirectionality.RIGHT })
223 {
224 RollingLaneStructureRecord current = newRoot;
225 RelativeLane relativeLane = RelativeLane.CURRENT;
226 Set<Lane> adjacentLanes =
227 current.getLane().accessibleAdjacentLanesPhysical(latDirection, gtuType, current.getDirection());
228 while (!adjacentLanes.isEmpty())
229 {
230 Throw.when(adjacentLanes.size() > 1, RuntimeException.class,
231 "Multiple adjacent lanes encountered during construction of lane map.");
232 relativeLane = latDirection.isLeft() ? relativeLane.getLeft() : relativeLane.getRight();
233 Lane adjacentLane = adjacentLanes.iterator().next();
234 RollingLaneStructureRecord adjacentRecord =
235 constructRecord(adjacentLane, direction, current, RecordLink.CROSS, relativeLane);
236 this.crossSectionRecords.put(relativeLane, adjacentRecord);
237 if (latDirection.isLeft())
238 {
239 if (adjacentLane
240 .accessibleAdjacentLanesPhysical(LateralDirectionality.RIGHT, gtuType, current.getDirection())
241 .contains(current.getLane()))
242 {
243 adjacentRecord.setRight(current, gtuType);
244 }
245 current.setLeft(adjacentRecord, gtuType);
246 }
247 else
248 {
249 if (adjacentLane
250 .accessibleAdjacentLanesPhysical(LateralDirectionality.LEFT, gtuType, current.getDirection())
251 .contains(current.getLane()))
252 {
253 adjacentRecord.setLeft(current, gtuType);
254 }
255 current.setRight(adjacentRecord, gtuType);
256 }
257 current = adjacentRecord;
258 adjacentLanes =
259 current.getLane().accessibleAdjacentLanesPhysical(latDirection, gtuType, current.getDirection());
260 }
261 }
262 this.upstreamEdge.addAll(this.crossSectionRecords.values());
263 this.downstreamEdge.addAll(this.crossSectionRecords.values());
264 this.firstRecords.putAll(this.crossSectionRecords);
265
266
267 newRoot.updateStartDistance(fracPos, this);
268
269
270 expandUpstreamEdge(gtuType, fracPos);
271
272
273 deriveFirstRecords();
274 }
275 else
276 {
277
278
279 RollingLaneStructureRecord newRoot = this.root.get();
280 if (!lane.equals(newRoot.getLane()))
281 {
282
283
284 newRoot = null;
285 RelativeLane lateralMove = null;
286 double closest = Double.POSITIVE_INFINITY;
287 for (RelativeLane relativeLane : this.relativeLaneMap.keySet())
288 {
289 for (RollingLaneStructureRecord record : this.relativeLaneMap.get(relativeLane))
290 {
291 if (record.getLane().equals(lane) && record.getStartDistance().si < closest
292 && record.getStartDistance().si + record.getLength().si > 0.0)
293 {
294 newRoot = record;
295 lateralMove = relativeLane;
296
297 closest = record.getStartDistance().si;
298 }
299 }
300 }
301
302 this.root.set(newRoot);
303
304
305 updateStartDistanceSources();
306
307
308 if (!lateralMove.isCurrent())
309 {
310 RelativeLane delta =
311 new RelativeLane(lateralMove.getLateralDirectionality().flip(), lateralMove.getNumLanes());
312
313 TreeMap<RelativeLane, Set<RollingLaneStructureRecord>> newRelativeLaneMap = new TreeMap<>();
314 for (RelativeLane relativeLane : this.relativeLaneMap.keySet())
315 {
316 RelativeLane newRelativeLane = relativeLane.add(delta);
317 newRelativeLaneMap.put(newRelativeLane, this.relativeLaneMap.get(relativeLane));
318 }
319 this.relativeLaneMap = newRelativeLaneMap;
320
321 Map<LaneStructureRecord, RelativeLane> newRelativeLanes = new HashMap<>();
322 for (LaneStructureRecord record : this.relativeLanes.keySet())
323 {
324 newRelativeLanes.put(record, this.relativeLanes.get(record).add(delta));
325 }
326 this.relativeLanes = newRelativeLanes;
327 }
328
329 this.crossSectionRecords.clear();
330 this.crossSectionRecords.put(RelativeLane.CURRENT, newRoot);
331 for (LateralDirectionality latDirection : new LateralDirectionality[] { LateralDirectionality.LEFT,
332 LateralDirectionality.RIGHT })
333 {
334 RollingLaneStructureRecord record = newRoot;
335 RollingLaneStructureRecord next = newRoot;
336 RelativeLane delta = new RelativeLane(latDirection, 1);
337 RelativeLane relLane = RelativeLane.CURRENT;
338 while (next != null)
339 {
340 next = latDirection.isLeft() ? record.getLeft() : record.getRight();
341 if (next != null)
342 {
343 next.changeStartDistanceSource(record, RecordLink.CROSS);
344 relLane = relLane.add(delta);
345 this.crossSectionRecords.put(relLane, next);
346 record = next;
347 }
348 }
349 }
350
351 }
352 newRoot.updateStartDistance(fracPos, this);
353
354
355 retreatUpstreamEdge();
356
357
358 deriveFirstRecords();
359
360 }
361
362
363 expandDownstreamEdge(gtuType, fracPos, route);
364
365 }
366
367
368
369
370 private void deriveFirstRecords()
371 {
372 this.firstRecords.clear();
373 this.firstRecords.putAll(this.crossSectionRecords);
374 for (RelativeLane lane : this.relativeLaneMap.keySet())
375 {
376 getFirstRecord(lane);
377 }
378 }
379
380
381
382
383
384
385
386
387 private void updateStartDistanceSources()
388 {
389
390
391 Set<RollingLaneStructureRecord> set = new HashSet<>();
392 RollingLaneStructureRecord rootRecord = this.root.get();
393 set.add(rootRecord);
394 rootRecord.changeStartDistanceSource(null, RecordLink.CROSS);
395 RollingLaneStructureRecord prev = rootRecord;
396 RollingLaneStructureRecord next = prev.getLeft();
397 while (next != null)
398 {
399 set.add(next);
400 next.changeStartDistanceSource(prev, RecordLink.CROSS);
401 prev = next;
402 next = next.getLeft();
403 }
404 prev = rootRecord;
405 next = prev.getRight();
406 while (next != null)
407 {
408 set.add(next);
409 next.changeStartDistanceSource(prev, RecordLink.CROSS);
410 prev = next;
411 next = next.getRight();
412 }
413
414 while (!set.isEmpty())
415 {
416
417 Set<RollingLaneStructureRecord> newSet = new HashSet<>();
418 for (RollingLaneStructureRecord record : set)
419 {
420 for (LateralDirectionality latDirection : new LateralDirectionality[] { LateralDirectionality.LEFT,
421 LateralDirectionality.RIGHT })
422 {
423 prev = record;
424 next = latDirection.isLeft() ? record.getLeft() : record.getRight();
425 while (next != null && !set.contains(next))
426 {
427 next.changeStartDistanceSource(prev, RecordLink.LATERAL_END);
428 removeDownstream(next, latDirection.flip());
429 newSet.add(next);
430 prev = next;
431 next = latDirection.isLeft() ? next.getLeft() : next.getRight();
432 }
433 }
434 }
435 set.addAll(newSet);
436
437
438 newSet.clear();
439 for (RollingLaneStructureRecord record : set)
440 {
441 for (RollingLaneStructureRecord prevRecord : record.getPrev())
442 {
443 Iterator<RollingLaneStructureRecord> it = prevRecord.getNext().iterator();
444 while (it.hasNext())
445 {
446 RollingLaneStructureRecord otherDown = it.next();
447 if (!otherDown.getLane().getParentLink().equals(record.getLane().getParentLink()))
448 {
449
450 otherDown.changeStartDistanceSource(null, null);
451
452 removeDownstream(otherDown, LateralDirectionality.NONE);
453 removeRecord(otherDown);
454 it.remove();
455 }
456 }
457 LaneStructureRecord source = prevRecord.getStartDistanceSource();
458 if (source == null || (source != null && !source.equals(record)))
459 {
460 prevRecord.changeStartDistanceSource(record, RecordLink.UP);
461 newSet.add(prevRecord);
462 }
463 }
464 }
465
466
467 set = newSet;
468 }
469 }
470
471
472
473
474
475
476 private void removeDownstream(final RollingLaneStructureRecord record, final LateralDirectionality lat)
477 {
478 for (RollingLaneStructureRecord next : record.getNext())
479 {
480 RollingLaneStructureRecord adj = lat.isLeft() ? next.getLeft() : lat.isRight() ? next.getRight() : null;
481 if (adj == null)
482 {
483 next.changeStartDistanceSource(null, null);
484 removeDownstream(next, lat);
485 removeRecord(next);
486 }
487 }
488 record.clearNextList();
489 }
490
491
492
493
494
495 private void removeRecord(final RollingLaneStructureRecord record)
496 {
497 RelativeLane lane = this.relativeLanes.get(record);
498 if (lane != null)
499 {
500 this.relativeLanes.remove(record);
501 }
502 record.changeStartDistanceSource(null, null);
503
504 }
505
506
507
508
509
510
511
512 private void expandUpstreamEdge(final GTUType gtuType, final double fractionalPosition) throws GTUException
513 {
514 this.ignoreSet.clear();
515 for (LaneStructureRecord record : this.upstreamEdge)
516 {
517 this.ignoreSet.add(record.getLane());
518 }
519 Set<RollingLaneStructureRecord> nextSet = new LinkedHashSet<>();
520 boolean expand = true;
521 while (expand)
522 {
523 expand = false;
524
525
526 Iterator<RollingLaneStructureRecord> iterator = this.upstreamEdge.iterator();
527 Set<RollingLaneStructureRecord> modifiedEdge = new LinkedHashSet<>(this.upstreamEdge);
528 while (iterator.hasNext())
529 {
530 RollingLaneStructureRecord prev = iterator.next();
531 Map<Lane, GTUDirectionality> nexts = prev.getLane().upstreamLanes(prev.getDirection(), gtuType);
532 if (prev.getStartDistance().si < this.up.si)
533 {
534
535 prev.setCutOffStart(this.up.minus(prev.getStartDistance()));
536 for (Lane prevLane : nexts.keySet())
537 {
538 this.ignoreSet.add(prevLane);
539 }
540 }
541 else
542 {
543
544 prev.clearCutOffStart();
545 iterator.remove();
546 if (!nexts.isEmpty())
547 {
548 for (Lane prevLane : nexts.keySet())
549 {
550 RelativeLane relativeLane = this.relativeLanes.get(prev);
551 RollingLaneStructureRecord next =
552 constructRecord(prevLane, nexts.get(prevLane), prev, RecordLink.UP, relativeLane);
553 this.ignoreSet.add(prevLane);
554 next.updateStartDistance(fractionalPosition, this);
555 connectLaterally(next, gtuType, modifiedEdge);
556 next.addNext(prev);
557 prev.addPrev(next);
558 nextSet.add(next);
559 modifiedEdge.add(next);
560 }
561 }
562 }
563 }
564 this.upstreamEdge.addAll(nextSet);
565 expand |= !nextSet.isEmpty();
566 nextSet.clear();
567
568
569 Set<RollingLaneStructureRecord> lateralSet =
570 expandLateral(this.upstreamEdge, RecordLink.LATERAL_END, gtuType, fractionalPosition);
571 nextSet.addAll(lateralSet);
572
573
574 this.upstreamEdge.addAll(nextSet);
575 expand |= !nextSet.isEmpty();
576 nextSet.clear();
577 }
578 }
579
580
581
582
583
584
585
586
587
588
589 private Set<RollingLaneStructureRecord> expandLateral(final Set<RollingLaneStructureRecord> edge,
590 final RecordLink recordLink, final GTUType gtuType, final double fractionalPosition)
591 {
592 Set<RollingLaneStructureRecord> nextSet = new LinkedHashSet<>();
593 Set<Lane> laneSet = new HashSet<>();
594 for (LaneStructureRecord record : edge)
595 {
596 laneSet.add(record.getLane());
597 }
598 Iterator<RollingLaneStructureRecord> iterator = edge.iterator();
599 while (iterator.hasNext())
600 {
601 RollingLaneStructureRecord record = iterator.next();
602 for (LateralDirectionality latDirection : new LateralDirectionality[] { LateralDirectionality.LEFT,
603 LateralDirectionality.RIGHT })
604 {
605 if (record.getRight() != null && latDirection.isRight() || record.getLeft() != null && latDirection.isLeft())
606 {
607
608 continue;
609 }
610 RelativeLane relativeLane = this.relativeLanes.get(record);
611 RollingLaneStructureRecord prev = record;
612 Set<Lane> adjacentLanes =
613 prev.getLane().accessibleAdjacentLanesPhysical(latDirection, gtuType, prev.getDirection());
614 while (!adjacentLanes.isEmpty())
615 {
616 Throw.when(adjacentLanes.size() > 1, RuntimeException.class,
617 "Multiple adjacent lanes encountered during construction of lane map.");
618 relativeLane = latDirection.isLeft() ? relativeLane.getLeft() : relativeLane.getRight();
619 Lane nextLane = adjacentLanes.iterator().next();
620 if (!laneSet.contains(nextLane) && !this.ignoreSet.contains(nextLane))
621 {
622 RollingLaneStructureRecord next =
623 constructRecord(nextLane, record.getDirection(), prev, recordLink, relativeLane);
624 this.ignoreSet.add(nextLane);
625 next.updateStartDistance(fractionalPosition, this);
626 nextSet.add(next);
627 laneSet.add(nextLane);
628 if (latDirection.isLeft())
629 {
630 prev.setLeft(next, gtuType);
631 if (nextLane
632 .accessibleAdjacentLanesPhysical(LateralDirectionality.RIGHT, gtuType, prev.getDirection())
633 .contains(prev.getLane()))
634 {
635 next.setRight(prev, gtuType);
636 }
637 for (RollingLaneStructureRecord edgeRecord : edge)
638 {
639 if (!edgeRecord.equals(prev)
640 && edgeRecord.getLane().getParentLink().equals(next.getLane().getParentLink()))
641 {
642 for (Lane adjLane : edgeRecord.getLane().accessibleAdjacentLanesPhysical(
643 LateralDirectionality.RIGHT, gtuType, edgeRecord.getDirection()))
644 {
645 if (adjLane.equals(next.getLane()))
646 {
647 edgeRecord.setRight(next, gtuType);
648 next.setLeft(edgeRecord, gtuType);
649 }
650 }
651 }
652 }
653 }
654 else
655 {
656 prev.setRight(next, gtuType);
657 if (nextLane
658 .accessibleAdjacentLanesPhysical(LateralDirectionality.LEFT, gtuType, prev.getDirection())
659 .contains(prev.getLane()))
660 {
661 next.setLeft(prev, gtuType);
662 }
663 for (RollingLaneStructureRecord edgeRecord : edge)
664 {
665 if (!edgeRecord.equals(prev)
666 && edgeRecord.getLane().getParentLink().equals(next.getLane().getParentLink()))
667 {
668 for (Lane adjLane : edgeRecord.getLane().accessibleAdjacentLanesPhysical(
669 LateralDirectionality.LEFT, gtuType, edgeRecord.getDirection()))
670 {
671 if (adjLane.equals(next.getLane()))
672 {
673 edgeRecord.setLeft(next, gtuType);
674 next.setRight(edgeRecord, gtuType);
675 }
676 }
677 }
678 }
679 }
680 prev = next;
681 adjacentLanes =
682 prev.getLane().accessibleAdjacentLanesPhysical(latDirection, gtuType, prev.getDirection());
683 }
684 else
685 {
686 break;
687 }
688 }
689 }
690 }
691 return nextSet;
692 }
693
694
695
696
697
698 private void retreatUpstreamEdge() throws GTUException
699 {
700 boolean moved = true;
701 Set<RollingLaneStructureRecord> nexts = new LinkedHashSet<>();
702 while (moved)
703 {
704 moved = false;
705 nexts.clear();
706 Iterator<RollingLaneStructureRecord> iterator = this.upstreamEdge.iterator();
707 while (iterator.hasNext())
708 {
709 RollingLaneStructureRecord prev = iterator.next();
710
711 if (!nexts.contains(prev) && prev.getStartDistance().si + prev.getLane().getLength().si < this.up.si)
712 {
713 for (RollingLaneStructureRecord next : prev.getNext())
714 {
715 next.clearPrevList();
716 next.setCutOffStart(this.up.minus(next.getStartDistance()));
717 moved = true;
718 nexts.add(next);
719 RollingLaneStructureRecord lat = next.getLeft();
720 while (lat != null && lat.getPrev().isEmpty())
721 {
722 nexts.add(lat);
723 lat = lat.getLeft();
724 }
725 lat = next.getRight();
726 while (lat != null && lat.getPrev().isEmpty())
727 {
728 nexts.add(lat);
729 lat = lat.getRight();
730 }
731 }
732 prev.clearNextList();
733 removeRecord(prev);
734 iterator.remove();
735 }
736 else
737 {
738 Length cutOff = this.up.minus(prev.getStartDistance());
739 if (cutOff.si > 0)
740 {
741 prev.setCutOffStart(cutOff);
742 }
743 }
744 }
745 this.upstreamEdge.addAll(nexts);
746
747
748 for (RollingLaneStructureRecord record : nexts)
749 {
750 RollingLaneStructureRecord prev = record;
751 for (LateralDirectionality latDirection : new LateralDirectionality[] { LateralDirectionality.LEFT,
752 LateralDirectionality.RIGHT })
753 {
754 while (prev != null)
755 {
756 RollingLaneStructureRecord next = latDirection.isLeft() ? prev.getLeft() : prev.getRight();
757 if (next != null && !this.upstreamEdge.contains(next))
758 {
759 moved |= findUpstreamEdge(next);
760 }
761 prev = next;
762 }
763 }
764 }
765 }
766 }
767
768
769
770
771
772
773
774
775
776 private boolean findUpstreamEdge(final RollingLaneStructureRecord record) throws GTUException
777 {
778 Length cutOff = this.up.minus(record.getStartDistance());
779 boolean moved = false;
780 if (cutOff.gt0())
781 {
782 if (cutOff.lt(record.getLane().getLength()))
783 {
784 record.clearPrevList();
785 record.setCutOffStart(cutOff);
786 this.upstreamEdge.add(record);
787 moved = true;
788 }
789 else
790 {
791 if (this.relativeLanes.containsKey(record))
792 {
793
794 removeRecord(record);
795 }
796 for (RollingLaneStructureRecord next : record.getNext())
797 {
798 moved |= findUpstreamEdge(next);
799 }
800 }
801 }
802 return moved;
803 }
804
805
806
807
808
809
810
811
812 private void expandDownstreamEdge(final GTUType gtuType, final double fractionalPosition, final Route route)
813 throws GTUException
814 {
815 this.ignoreSet.clear();
816 for (LaneStructureRecord record : this.downstreamEdge)
817 {
818 this.ignoreSet.add(record.getLane());
819 }
820 Set<RollingLaneStructureRecord> nextSet = new LinkedHashSet<>();
821 Set<RollingLaneStructureRecord> splitSet = new LinkedHashSet<>();
822 boolean expand = true;
823 while (expand)
824 {
825 expand = false;
826
827
828
829 Set<Link> linksToExpandFrom = new LinkedHashSet<>();
830 Iterator<RollingLaneStructureRecord> iterator = this.downstreamEdge.iterator();
831 while (iterator.hasNext())
832 {
833 RollingLaneStructureRecord record = iterator.next();
834 if (record.getStartDistance().si + record.getLane().getLength().si < this.down.si)
835 {
836 linksToExpandFrom.add(record.getLane().getParentLink());
837 }
838 }
839 iterator = this.downstreamEdge.iterator();
840 Set<RollingLaneStructureRecord> modifiedEdge = new LinkedHashSet<>(this.downstreamEdge);
841 while (iterator.hasNext())
842 {
843 RollingLaneStructureRecord record = iterator.next();
844 Map<Lane, GTUDirectionality> nexts = record.getLane().downstreamLanes(record.getDirection(), gtuType);
845 if (!linksToExpandFrom.contains(record.getLane().getParentLink()))
846 {
847
848 record.setCutOffEnd(this.down.minus(record.getStartDistance()));
849 for (Lane nextLane : nexts.keySet())
850 {
851 this.ignoreSet.add(nextLane);
852 }
853 }
854 else
855 {
856
857
858
859 LaneDirection nextLaneDirection =
860 new LaneDirection(record.getLane(), record.getDirection()).getNextLaneDirection(this.containingGtu);
861
862 record.clearCutOffEnd();
863 iterator.remove();
864 for (Lane nextLane : nexts.keySet())
865 {
866 if (nextLaneDirection != null
867 && nextLane.getParentLink().equals(nextLaneDirection.getLane().getParentLink())
868 && !nextLane.equals(nextLaneDirection.getLane()))
869 {
870
871 continue;
872 }
873 RelativeLane relativeLane = this.relativeLanes.get(record);
874 GTUDirectionality dir = nexts.get(nextLane);
875 RollingLaneStructureRecord next = constructRecord(nextLane, dir, record, RecordLink.DOWN, relativeLane);
876 this.ignoreSet.add(nextLane);
877 next.updateStartDistance(fractionalPosition, this);
878 record.addNext(next);
879 next.addPrev(record);
880 connectLaterally(next, gtuType, modifiedEdge);
881
882 int from = route == null ? 0 : route.indexOf(next.getFromNode());
883 int to = route == null ? 1 : route.indexOf(next.getToNode());
884 if (to < 0 || to - from != 1)
885 {
886
887 splitSet.add(next);
888 }
889 else
890 {
891
892 nextSet.add(next);
893 }
894 modifiedEdge.add(next);
895
896 Set<RollingLaneStructureRecord> set = new HashSet<>();
897 set.add(next);
898 expandUpstreamMerge(set, gtuType, fractionalPosition, route);
899 }
900 }
901 }
902 this.downstreamEdge.addAll(nextSet);
903 expand |= !nextSet.isEmpty();
904 nextSet.clear();
905
906
907 expandDownstreamSplit(splitSet, gtuType, fractionalPosition, route);
908 splitSet.clear();
909
910
911 Set<RollingLaneStructureRecord> lateralSet =
912 expandLateral(this.downstreamEdge, RecordLink.LATERAL_END, gtuType, fractionalPosition);
913 nextSet.addAll(lateralSet);
914 expandUpstreamMerge(lateralSet, gtuType, fractionalPosition, route);
915
916
917 this.downstreamEdge.addAll(nextSet);
918 expand |= !nextSet.isEmpty();
919 nextSet.clear();
920 }
921 }
922
923
924
925
926
927
928
929
930
931 private void expandDownstreamSplit(final Set<RollingLaneStructureRecord> set, final GTUType gtuType,
932 final double fractionalPosition, final Route route) throws GTUException
933 {
934 Map<RollingLaneStructureRecord, Length> prevs = new LinkedHashMap<>();
935 Map<RollingLaneStructureRecord, Length> nexts = new LinkedHashMap<>();
936 for (RollingLaneStructureRecord record : set)
937 {
938 prevs.put(record, record.getStartDistance().plus(this.downSplit));
939 }
940 while (!prevs.isEmpty())
941 {
942 for (RollingLaneStructureRecord prev : prevs.keySet())
943 {
944 Map<Lane, GTUDirectionality> nextLanes = prev.getLane().downstreamLanes(prev.getDirection(), gtuType);
945 RelativeLane relativeLane = this.relativeLanes.get(prev);
946 for (Lane nextLane : nextLanes.keySet())
947 {
948 GTUDirectionality dir = nextLanes.get(nextLane);
949 Node fromNode =
950 dir.isPlus() ? nextLane.getParentLink().getStartNode() : nextLane.getParentLink().getEndNode();
951 Node toNode =
952 dir.isPlus() ? nextLane.getParentLink().getEndNode() : nextLane.getParentLink().getStartNode();
953 int from = route.indexOf(fromNode);
954 int to = route.indexOf(toNode);
955 if (from == -1 || to == -1 || to - from != 1)
956 {
957 RollingLaneStructureRecord next = constructRecord(nextLane, dir, prev, RecordLink.DOWN, relativeLane);
958 next.updateStartDistance(fractionalPosition, this);
959 next.addPrev(prev);
960 prev.addNext(next);
961 connectLaterally(next, gtuType, nexts.keySet());
962 Length downHere = prevs.get(prev);
963 if (next.getStartDistance().si > downHere.si)
964 {
965 next.setCutOffEnd(downHere.minus(next.getStartDistance()));
966 }
967 else
968 {
969 nexts.put(next, downHere);
970 }
971 }
972 }
973 }
974 prevs = nexts;
975 nexts = new LinkedHashMap<>();
976 }
977 }
978
979
980
981
982
983
984
985
986
987 private void expandUpstreamMerge(final Set<RollingLaneStructureRecord> set, final GTUType gtuType,
988 final double fractionalPosition, final Route route) throws GTUException
989 {
990 Map<RollingLaneStructureRecord, Length> prevs = new LinkedHashMap<>();
991 Map<RollingLaneStructureRecord, Length> nexts = new LinkedHashMap<>();
992 for (RollingLaneStructureRecord record : set)
993 {
994 prevs.put(record, record.getStartDistance().plus(this.upMerge));
995 }
996 while (!prevs.isEmpty())
997 {
998 for (RollingLaneStructureRecord prev : prevs.keySet())
999 {
1000 Map<Lane, GTUDirectionality> nextLanes = prev.getLane().upstreamLanes(prev.getDirection(), gtuType);
1001 boolean anyAdded = false;
1002 for (Lane nextLane : nextLanes.keySet())
1003 {
1004 GTUDirectionality dir = nextLanes.get(nextLane);
1005 Node fromNode =
1006 dir.isPlus() ? nextLane.getParentLink().getStartNode() : nextLane.getParentLink().getEndNode();
1007 Node toNode =
1008 dir.isPlus() ? nextLane.getParentLink().getEndNode() : nextLane.getParentLink().getStartNode();
1009 int from = route == null ? 0 : route.indexOf(fromNode);
1010 int to = route == null ? 1 : route.indexOf(toNode);
1011
1012
1013 if (from == -1 || to == -1 || to - from != 1)
1014 {
1015 anyAdded = true;
1016 RelativeLane relativeLane = this.relativeLanes.get(prev);
1017 RollingLaneStructureRecord next =
1018 constructRecord(nextLane, nextLanes.get(nextLane), prev, RecordLink.UP, relativeLane);
1019 next.updateStartDistance(fractionalPosition, this);
1020 next.addNext(prev);
1021 prev.addPrev(next);
1022 connectLaterally(next, gtuType, nexts.keySet());
1023 Length upHere = prevs.get(prev);
1024 if (next.getStartDistance().si < upHere.si)
1025 {
1026 next.setCutOffStart(upHere.minus(next.getStartDistance()));
1027 this.upstreamEdge.add(next);
1028 }
1029 else
1030 {
1031 nexts.put(next, upHere);
1032 }
1033 }
1034 }
1035 if (!anyAdded && !set.contains(prev))
1036 {
1037 this.upstreamEdge.add(prev);
1038 }
1039 }
1040 prevs = nexts;
1041 nexts = new LinkedHashMap<>();
1042 }
1043 }
1044
1045
1046
1047
1048
1049
1050
1051 private void connectLaterally(final RollingLaneStructureRecord record, final GTUType gtuType,
1052 final Set<RollingLaneStructureRecord> nextSet)
1053 {
1054 for (RollingLaneStructureRecord other : nextSet)
1055 {
1056 for (LateralDirectionality latDirection : new LateralDirectionality[] { LateralDirectionality.LEFT,
1057 LateralDirectionality.RIGHT })
1058 {
1059 if ((latDirection.isLeft() ? other.getLeft() : other.getRight()) == null)
1060 {
1061 for (Lane otherLane : other.getLane().accessibleAdjacentLanesPhysical(latDirection, gtuType,
1062 other.getDirection()))
1063 {
1064 if (otherLane.equals(record.getLane()))
1065 {
1066 if (latDirection.isLeft())
1067 {
1068 other.setLeft(record, gtuType);
1069 record.setRight(other, gtuType);
1070 }
1071 else
1072 {
1073 other.setRight(record, gtuType);
1074 record.setLeft(other, gtuType);
1075 }
1076 }
1077 }
1078 }
1079 }
1080 }
1081 }
1082
1083
1084
1085
1086
1087
1088
1089
1090
1091
1092 private RollingLaneStructureRecord constructRecord(final Lane lane, final GTUDirectionality direction,
1093 final RollingLaneStructureRecord startDistanceSource, final RecordLink recordLink, final RelativeLane relativeLane)
1094 {
1095 RollingLaneStructureRecord record = new RollingLaneStructureRecord(lane, direction, startDistanceSource, recordLink);
1096 if (!this.relativeLaneMap.containsKey(relativeLane))
1097 {
1098 this.relativeLaneMap.put(relativeLane, new LinkedHashSet<>());
1099 }
1100 this.relativeLaneMap.get(relativeLane).add(record);
1101 this.relativeLanes.put(record, relativeLane);
1102 return record;
1103 }
1104
1105
1106 @Override
1107 public final LaneStructureRecord getRootRecord()
1108 {
1109 return this.root.get();
1110 }
1111
1112
1113
1114
1115
1116 public final LaneStructureRecord getRootRecord(final Time time)
1117 {
1118 return this.root.get(time);
1119 }
1120
1121
1122 @Override
1123 public final SortedSet<RelativeLane> getExtendedCrossSection()
1124 {
1125 return this.firstRecords.navigableKeySet();
1126 }
1127
1128
1129
1130
1131
1132
1133
1134 @Override
1135 public final RollingLaneStructureRecord getFirstRecord(final RelativeLane lane)
1136 {
1137 if (this.firstRecords.containsKey(lane))
1138 {
1139 return this.firstRecords.get(lane);
1140 }
1141
1142 RelativeLane rel = RelativeLane.CURRENT;
1143 int dMin = Integer.MAX_VALUE;
1144 for (RelativeLane relLane : this.crossSectionRecords.keySet())
1145 {
1146 if (relLane.getLateralDirectionality().equals(lane.getLateralDirectionality()))
1147 {
1148 int d = lane.getNumLanes() - relLane.getNumLanes();
1149 if (d < dMin)
1150 {
1151 rel = relLane;
1152 d = dMin;
1153 }
1154 }
1155 }
1156 RollingLaneStructureRecord record = this.crossSectionRecords.get(rel);
1157
1158 while (rel.getNumLanes() < lane.getNumLanes())
1159 {
1160 RollingLaneStructureRecord adj = lane.getLateralDirectionality().isLeft() ? record.getLeft() : record.getRight();
1161 if (adj != null)
1162 {
1163 rel = lane.getLateralDirectionality().isLeft() ? rel.getLeft() : rel.getRight();
1164 record = adj;
1165 }
1166 else if (!record.getNext().isEmpty())
1167 {
1168 LaneDirection laneDir =
1169 new LaneDirection(record.getLane(), record.getDirection()).getNextLaneDirection(this.containingGtu);
1170 if (laneDir == null)
1171 {
1172 record = null;
1173 break;
1174 }
1175 RollingLaneStructureRecord chosenNext = null;
1176 for (RollingLaneStructureRecord next : record.getNext())
1177 {
1178 if (next.getLane().equals(laneDir.getLane()))
1179 {
1180 chosenNext = next;
1181 break;
1182 }
1183 }
1184 Throw.when(chosenNext == null, RuntimeException.class,
1185 "Unexpected exception while deriving first record not on the cross-section.");
1186 record = chosenNext;
1187 }
1188 else
1189 {
1190
1191 record = null;
1192 break;
1193 }
1194 }
1195 if (record != null)
1196 {
1197
1198 while (record.getPrev().size() == 1 && record.getStartDistance().gt0())
1199 {
1200 record = record.getPrev().get(0);
1201 }
1202 this.firstRecords.put(lane, record);
1203 }
1204 return record;
1205 }
1206
1207
1208
1209
1210
1211
1212
1213
1214
1215
1216
1217 @Override
1218 public final <T extends LaneBasedObject> Map<RelativeLane, SortedSet<Entry<T>>> getDownstreamObjects(final Class<T> clazz,
1219 final LaneBasedGTU gtu, final RelativePosition.TYPE pos) throws GTUException
1220 {
1221 Map<RelativeLane, SortedSet<Entry<T>>> out = new HashMap<>();
1222 for (RelativeLane relativeLane : this.relativeLaneMap.keySet())
1223 {
1224 out.put(relativeLane, getDownstreamObjects(relativeLane, clazz, gtu, pos));
1225 }
1226 return out;
1227 }
1228
1229
1230
1231
1232
1233
1234
1235
1236
1237
1238
1239
1240 @Override
1241 @SuppressWarnings("unchecked")
1242 public final <T extends LaneBasedObject> SortedSet<Entry<T>> getDownstreamObjects(final RelativeLane lane,
1243 final Class<T> clazz, final LaneBasedGTU gtu, final RelativePosition.TYPE pos) throws GTUException
1244 {
1245 LaneStructureRecord record = getFirstRecord(lane);
1246 SortedSet<Entry<T>> set = new TreeSet<>();
1247 if (record != null)
1248 {
1249 double ds = gtu.getRelativePositions().get(pos).getDx().si - gtu.getReference().getDx().si;
1250 if (record.isDownstreamBranch())
1251 {
1252
1253 Length minimumPosition;
1254 Length maximumPosition;
1255 if (record.getDirection().isPlus())
1256 {
1257 minimumPosition = Length.createSI(ds - record.getStartDistance().si);
1258 maximumPosition = Length.createSI(record.getLane().getLength().si);
1259 }
1260 else
1261 {
1262 minimumPosition = Length.ZERO;
1263 maximumPosition = Length.createSI(record.getLane().getLength().si + record.getStartDistance().si - ds);
1264 }
1265
1266 for (LaneBasedObject object : record.getLane().getLaneBasedObjects(minimumPosition, maximumPosition))
1267 {
1268 if (clazz.isAssignableFrom(object.getClass())
1269 && ((record.getDirection().isPlus() && object.getDirection().isForwardOrBoth())
1270 || (record.getDirection().isMinus() && object.getDirection().isBackwardOrBoth())))
1271 {
1272
1273 double distance = record.getDistanceToPosition(object.getLongitudinalPosition()).si - ds;
1274 if (distance <= this.lookAhead.si)
1275 {
1276 set.add(new Entry<>(Length.createSI(distance), (T) object));
1277 }
1278 }
1279 }
1280 }
1281 getDownstreamObjectsRecursive(set, record, clazz, ds);
1282 }
1283 return set;
1284 }
1285
1286
1287
1288
1289
1290
1291
1292
1293
1294
1295
1296
1297
1298 @Override
1299 public final <T extends LaneBasedObject> SortedSet<Entry<T>> getDownstreamObjectsOnRoute(final RelativeLane lane,
1300 final Class<T> clazz, final LaneBasedGTU gtu, final RelativePosition.TYPE pos, final Route route)
1301 throws GTUException
1302 {
1303 SortedSet<Entry<T>> set = getDownstreamObjects(lane, clazz, gtu, pos);
1304 if (route != null)
1305 {
1306 Iterator<Entry<T>> iterator = set.iterator();
1307 while (iterator.hasNext())
1308 {
1309 Entry<T> entry = iterator.next();
1310 CrossSectionLink link = entry.getLaneBasedObject().getLane().getParentLink();
1311 if (!route.contains(link.getStartNode()) || !route.contains(link.getEndNode())
1312 || Math.abs(route.indexOf(link.getStartNode()) - route.indexOf(link.getEndNode())) != 1)
1313 {
1314 iterator.remove();
1315 }
1316 }
1317 }
1318 return set;
1319 }
1320
1321
1322
1323
1324
1325
1326
1327
1328
1329 @SuppressWarnings("unchecked")
1330 private <T extends LaneBasedObject> void getDownstreamObjectsRecursive(final SortedSet<Entry<T>> set,
1331 final LaneStructureRecord record, final Class<T> clazz, final double ds)
1332 {
1333 if (record.getNext().isEmpty() || record.getNext().get(0).getStartDistance().gt(this.lookAhead))
1334 {
1335 return;
1336 }
1337 for (LaneStructureRecord next : record.getNext())
1338 {
1339 if (next.isDownstreamBranch())
1340 {
1341 List<LaneBasedObject> list = next.getLane().getLaneBasedObjects();
1342 int iStart, di;
1343 if (record.getDirection().isPlus())
1344 {
1345 iStart = 0;
1346 di = 1;
1347 }
1348 else
1349 {
1350 iStart = list.size() - 1;
1351 di = -1;
1352 }
1353 for (int i = iStart; i >= 0 & i < list.size(); i += di)
1354 {
1355 LaneBasedObject object = list.get(i);
1356 if (clazz.isAssignableFrom(object.getClass())
1357 && ((record.getDirection().isPlus() && object.getDirection().isForwardOrBoth())
1358 || (record.getDirection().isMinus() && object.getDirection().isBackwardOrBoth())))
1359 {
1360
1361 double distance = next.getDistanceToPosition(object.getLongitudinalPosition()).si - ds;
1362 if (distance <= this.lookAhead.si)
1363 {
1364 set.add(new Entry<>(Length.createSI(distance), (T) object));
1365 }
1366 else
1367 {
1368 return;
1369 }
1370 }
1371 }
1372 }
1373 getDownstreamObjectsRecursive(set, next, clazz, ds);
1374 }
1375 }
1376
1377
1378
1379
1380
1381
1382
1383
1384
1385
1386
1387
1388 @Override
1389 public final <T extends LaneBasedObject> Map<RelativeLane, SortedSet<Entry<T>>> getDownstreamObjectsOnRoute(
1390 final Class<T> clazz, final LaneBasedGTU gtu, final RelativePosition.TYPE pos, final Route route)
1391 throws GTUException
1392 {
1393 Map<RelativeLane, SortedSet<Entry<T>>> out = new HashMap<>();
1394 for (RelativeLane relativeLane : this.relativeLaneMap.keySet())
1395 {
1396 out.put(relativeLane, getDownstreamObjectsOnRoute(relativeLane, clazz, gtu, pos, route));
1397 }
1398 return out;
1399 }
1400
1401
1402
1403
1404
1405
1406
1407
1408
1409
1410
1411
1412 @Override
1413 @SuppressWarnings("unchecked")
1414 public final <T extends LaneBasedObject> SortedSet<Entry<T>> getUpstreamObjects(final RelativeLane lane,
1415 final Class<T> clazz, final LaneBasedGTU gtu, final RelativePosition.TYPE pos) throws GTUException
1416 {
1417 SortedSet<Entry<T>> set = new TreeSet<>();
1418 LaneStructureRecord record = this.getFirstRecord(lane);
1419 if (record.getStartDistance().gt0())
1420 {
1421 return set;
1422 }
1423 Length ds = gtu.getReference().getDx().minus(gtu.getRelativePositions().get(pos).getDx());
1424
1425 Length minimumPosition;
1426 Length maximumPosition;
1427 if (record.getDirection().isPlus())
1428 {
1429 minimumPosition = Length.ZERO;
1430 maximumPosition = record.getStartDistance().neg().minus(ds);
1431 }
1432 else
1433 {
1434 minimumPosition = record.getLane().getLength().plus(record.getStartDistance()).plus(ds);
1435 maximumPosition = record.getLane().getLength();
1436 }
1437 Length distance;
1438 for (LaneBasedObject object : record.getLane().getLaneBasedObjects(minimumPosition, maximumPosition))
1439 {
1440 if (clazz.isAssignableFrom(object.getClass())
1441 && ((record.getDirection().isPlus() && object.getDirection().isForwardOrBoth())
1442 || (record.getDirection().isMinus() && object.getDirection().isBackwardOrBoth())))
1443 {
1444 distance = record.getDistanceToPosition(object.getLongitudinalPosition()).neg().minus(ds);
1445
1446 set.add(new Entry<>(distance, (T) object));
1447 }
1448 }
1449 getUpstreamObjectsRecursive(set, record, clazz, ds);
1450 return set;
1451 }
1452
1453
1454
1455
1456
1457
1458
1459
1460
1461 @SuppressWarnings("unchecked")
1462 private <T extends LaneBasedObject> void getUpstreamObjectsRecursive(final SortedSet<Entry<T>> set,
1463 final LaneStructureRecord record, final Class<T> clazz, final Length ds)
1464 {
1465 for (LaneStructureRecord prev : record.getPrev())
1466 {
1467 Length distance;
1468 for (LaneBasedObject object : prev.getLane().getLaneBasedObjects())
1469 {
1470 if (clazz.isAssignableFrom(object.getClass())
1471 && ((record.getDirection().isPlus() && object.getDirection().isForwardOrBoth())
1472 || (record.getDirection().isMinus() && object.getDirection().isBackwardOrBoth())))
1473 {
1474 distance = prev.getDistanceToPosition(object.getLongitudinalPosition()).neg().minus(ds);
1475
1476 set.add(new Entry<>(distance, (T) object));
1477 }
1478 }
1479 getUpstreamObjectsRecursive(set, prev, clazz, ds);
1480 }
1481 }
1482
1483
1484 @Override
1485 public final String toString()
1486 {
1487 return "LaneStructure [rootLSR=" + this.root + "]";
1488 }
1489
1490
1491
1492
1493
1494
1495
1496
1497
1498 public class AnimationAccess
1499 {
1500
1501
1502
1503 @SuppressWarnings("synthetic-access")
1504 public TreeMap<RelativeLane, RollingLaneStructureRecord> getCrossSectionRecords()
1505 {
1506 return RollingLaneStructure.this.crossSectionRecords;
1507 }
1508
1509
1510
1511
1512 @SuppressWarnings("synthetic-access")
1513 public Set<RollingLaneStructureRecord> getUpstreamEdge()
1514 {
1515 return RollingLaneStructure.this.upstreamEdge;
1516 }
1517
1518
1519
1520
1521 @SuppressWarnings("synthetic-access")
1522 public Set<RollingLaneStructureRecord> getDownstreamEdge()
1523 {
1524 return RollingLaneStructure.this.downstreamEdge;
1525 }
1526 }
1527 }