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