1 package org.opentrafficsim.road.gtu.lane.perception;
2
3 import java.io.Serializable;
4 import java.util.ArrayList;
5 import java.util.HashSet;
6 import java.util.LinkedHashSet;
7 import java.util.List;
8 import java.util.Set;
9
10 import org.djunits.value.vdouble.scalar.Length;
11 import org.opentrafficsim.core.gtu.GTUDirectionality;
12 import org.opentrafficsim.core.gtu.GTUException;
13 import org.opentrafficsim.core.gtu.GTUType;
14 import org.opentrafficsim.core.gtu.NestedCache;
15 import org.opentrafficsim.core.gtu.Try;
16 import org.opentrafficsim.core.network.LateralDirectionality;
17 import org.opentrafficsim.core.network.Link;
18 import org.opentrafficsim.core.network.NetworkException;
19 import org.opentrafficsim.core.network.Node;
20 import org.opentrafficsim.core.network.route.Route;
21 import org.opentrafficsim.road.network.lane.Lane;
22
23 import nl.tudelft.simulation.language.Throw;
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39 public class RollingLaneStructureRecord implements LaneStructureRecord, Serializable
40 {
41
42 private static final long serialVersionUID = 20160400L;
43
44
45
46 private static NestedCache<Boolean> allowsRouteCache =
47 new NestedCache<>(Lane.class, Route.class, GTUType.class, Boolean.class);
48
49
50 private final Lane lane;
51
52
53 private final GTUDirectionality gtuDirectionality;
54
55
56 private RollingLaneStructureRecord left;
57
58
59 private boolean mayChangeLeft;
60
61
62 private RollingLaneStructureRecord right;
63
64
65 private boolean mayChangeRight;
66
67
68 private Length cutOffEnd = null;
69
70
71 private Length cutOffStart = null;
72
73
74 private Length startDistance;
75
76
77
78
79
80 private List<RollingLaneStructureRecord> nextList = new ArrayList<>();
81
82
83
84
85
86 private List<RollingLaneStructureRecord> prevList = new ArrayList<>();
87
88
89 private RollingLaneStructureRecord source;
90
91
92 private RecordLink sourceLink;
93
94
95 private final Set<RollingLaneStructureRecord> dependentRecords = new LinkedHashSet<>();
96
97
98
99
100
101
102
103
104 public RollingLaneStructureRecord(final Lane lane, final GTUDirectionality direction,
105 final RollingLaneStructureRecord startDistanceSource, final RecordLink recordLink)
106 {
107 this.lane = lane;
108 this.gtuDirectionality = direction;
109 this.source = startDistanceSource;
110 this.sourceLink = recordLink;
111 if (startDistanceSource != null)
112 {
113 startDistanceSource.dependentRecords.add(this);
114 }
115 }
116
117
118
119
120
121
122 public RollingLaneStructureRecord(final Lane lane, final GTUDirectionality direction, final Length startDistance)
123 {
124 this.lane = lane;
125 this.gtuDirectionality = direction;
126 this.startDistance = startDistance;
127
128 this.source = null;
129 this.sourceLink = null;
130 }
131
132
133 @Override
134 public Length getLength()
135 {
136 return getLane().getLength();
137 }
138
139
140
141
142
143
144 final void changeStartDistanceSource(final RollingLaneStructureRecord startDistanceSource, final RecordLink recordLink)
145 {
146
147 if (this.source != null)
148 {
149 this.source.dependentRecords.remove(this);
150 }
151
152 this.source = startDistanceSource;
153 this.sourceLink = recordLink;
154 if (this.source != null)
155 {
156 this.source.dependentRecords.add(this);
157 }
158 }
159
160
161
162
163
164
165
166 final void updateStartDistance(final double fractionalPosition, final RollingLaneStructure laneStructure)
167 {
168 this.startDistance = this.sourceLink.calculateStartDistance(this.source, this, fractionalPosition);
169 for (RollingLaneStructureRecord record : this.dependentRecords)
170 {
171 record.updateStartDistance(fractionalPosition, laneStructure);
172 }
173 }
174
175
176
177
178
179 final RollingLaneStructureRecord getStartDistanceSource()
180 {
181 return this.source;
182 }
183
184
185 @Override
186 public final Node getFromNode()
187 {
188 return this.gtuDirectionality.isPlus() ? this.lane.getParentLink().getStartNode()
189 : this.lane.getParentLink().getEndNode();
190 }
191
192
193 @Override
194 public final Node getToNode()
195 {
196 return this.gtuDirectionality.isPlus() ? this.lane.getParentLink().getEndNode()
197 : this.lane.getParentLink().getStartNode();
198 }
199
200
201
202
203
204 @Deprecated
205 public final boolean isLinkSplit()
206 {
207 if (isCutOffEnd())
208 {
209
210 return false;
211 }
212 Set<Node> toNodes = new HashSet<>();
213 LaneStructureRecord lsr = this;
214 while (lsr != null)
215 {
216 for (LaneStructureRecord next : lsr.getNext())
217 {
218 toNodes.add(next.getToNode());
219 }
220 lsr = lsr.getLeft();
221 }
222 lsr = this.getRight();
223 while (lsr != null)
224 {
225 for (LaneStructureRecord next : lsr.getNext())
226 {
227 toNodes.add(next.getToNode());
228 }
229 lsr = lsr.getRight();
230 }
231 return toNodes.size() > 1;
232 }
233
234
235
236
237
238 public final boolean isLinkMerge()
239 {
240 if (isCutOffStart())
241 {
242
243 return false;
244 }
245 Set<Node> fromNodes = new HashSet<>();
246 LaneStructureRecord lsr = this;
247 while (lsr != null)
248 {
249 for (LaneStructureRecord prev : lsr.getPrev())
250 {
251 fromNodes.add(prev.getFromNode());
252 }
253 lsr = lsr.getLeft();
254 }
255 lsr = this.getRight();
256 while (lsr != null)
257 {
258 for (LaneStructureRecord prev : lsr.getPrev())
259 {
260 fromNodes.add(prev.getFromNode());
261 }
262 lsr = lsr.getRight();
263 }
264 return fromNodes.size() > 1;
265 }
266
267
268 @Override
269 public final boolean allowsRoute(final Route route, final GTUType gtuType) throws NetworkException
270 {
271 return allowsRoute(route, gtuType, false);
272 }
273
274
275 @Override
276 public final boolean allowsRouteAtEnd(final Route route, final GTUType gtuType) throws NetworkException
277 {
278 return allowsRoute(route, gtuType, true);
279 }
280
281
282
283
284
285
286
287
288
289 private boolean allowsRoute(final Route route, final GTUType gtuType, final boolean end) throws NetworkException
290 {
291 return allowsRouteCache.getValue(() -> Try.assign(() -> allowsRoute0(route, gtuType, end), "no destination"), this.lane,
292 route, gtuType, end);
293 }
294
295
296
297
298
299
300
301
302
303 private boolean allowsRoute0(final Route route, final GTUType gtuType, final boolean end) throws NetworkException
304 {
305
306
307 if (route == null)
308 {
309 return true;
310 }
311
312
313 int from = route.indexOf(getFromNode());
314 int to = route.indexOf(getToNode());
315 if (from == -1 || to == -1 || from != to - 1)
316 {
317 return leadsToRoute(route, gtuType, null);
318 }
319
320
321 Set<LaneStructureRecord> currentSet = new LinkedHashSet<>();
322 Set<LaneStructureRecord> nextSet = new LinkedHashSet<>();
323 currentSet.add(this);
324
325 boolean firstLoop = true;
326 while (!currentSet.isEmpty())
327 {
328
329 if (!firstLoop || end)
330 {
331
332 for (LaneStructureRecord laneRecord : currentSet)
333 {
334 to = route.indexOf(laneRecord.getToNode());
335 if (to == route.getNodes().size() - 2)
336 {
337
338 for (Link link : laneRecord.getToNode().nextLinks(gtuType, laneRecord.getLane().getParentLink()))
339 {
340 if (link.getLinkType().isConnector())
341 {
342 if ((link.getStartNode().equals(laneRecord.getToNode())
343 && link.getEndNode().equals(route.destinationNode()))
344 || (link.getEndNode().equals(laneRecord.getToNode())
345 && link.getStartNode().equals(route.destinationNode())))
346 {
347 return true;
348 }
349 }
350 }
351 }
352 for (LaneStructureRecord next : laneRecord.getNext())
353 {
354 if (next.getToNode().equals(route.destinationNode()))
355 {
356
357 return true;
358 }
359 if (route.indexOf(next.getToNode()) == to + 1)
360 {
361 nextSet.add(next);
362 }
363 }
364 }
365 currentSet = nextSet;
366 nextSet = new LinkedHashSet<>();
367 }
368 firstLoop = false;
369
370
371 nextSet.addAll(currentSet);
372 for (LaneStructureRecord laneRecord : currentSet)
373 {
374 while (laneRecord.legalLeft() && !nextSet.contains(laneRecord.getLeft()))
375 {
376 nextSet.add(laneRecord.getLeft());
377 laneRecord = laneRecord.getLeft();
378 }
379 }
380 for (LaneStructureRecord laneRecord : currentSet)
381 {
382 while (laneRecord.legalRight() && !nextSet.contains(laneRecord.getRight()))
383 {
384 nextSet.add(laneRecord.getRight());
385 laneRecord = laneRecord.getRight();
386 }
387 }
388
389
390 if (nextSet.isEmpty())
391 {
392 return false;
393 }
394
395
396 int nLanesOnNextLink = 0;
397 LaneStructureRecord nextRecord = nextSet.iterator().next();
398 for (Lane l : nextRecord.getLane().getParentLink().getLanes())
399 {
400 if (l.getLaneType().getDirectionality(gtuType).getDirectionalities().contains(nextRecord.getDirection()))
401 {
402 nLanesOnNextLink++;
403 }
404 }
405 if (nextSet.size() == nLanesOnNextLink)
406 {
407
408 return true;
409 }
410
411 currentSet = nextSet;
412 nextSet = new LinkedHashSet<>();
413
414 }
415
416
417 return false;
418 }
419
420
421
422
423
424
425
426
427
428 private boolean leadsToRoute(final Route route, final GTUType gtuType, final LaneStructureRecord original)
429 throws NetworkException
430 {
431 if (original == this)
432 {
433 return false;
434 }
435 if (original != null && allowsRoute(route, gtuType))
436 {
437 return true;
438 }
439
440 for (LaneStructureRecord record : getNext())
441 {
442 boolean leadsTo =
443 ((RollingLaneStructureRecord) record).leadsToRoute(route, gtuType, original == null ? this : original);
444 if (leadsTo)
445 {
446 return true;
447 }
448 }
449 return false;
450 }
451
452
453 @Override
454 public final RollingLaneStructureRecord getLeft()
455 {
456 return this.left;
457 }
458
459
460
461
462
463 public final void setLeft(final RollingLaneStructureRecord leftRecord, final GTUType gtuType)
464 {
465 this.left = leftRecord;
466 this.mayChangeLeft = getLane().accessibleAdjacentLanesLegal(LateralDirectionality.LEFT, gtuType, this.gtuDirectionality)
467 .contains(leftRecord.getLane());
468 if (getLane().getFullId().equals("1023.FORWARD3") && !this.mayChangeLeft)
469 {
470 System.out.println("Lane 1023.FORWARD3 allows left:" + this.mayChangeLeft);
471 }
472 }
473
474
475 @Override
476 public final boolean legalLeft()
477 {
478 return this.mayChangeLeft;
479 }
480
481
482 @Override
483 public final boolean physicalLeft()
484 {
485 return this.left != null;
486 }
487
488
489 @Override
490 public final RollingLaneStructureRecord getRight()
491 {
492 return this.right;
493 }
494
495
496
497
498
499
500 public final void setRight(final RollingLaneStructureRecord rightRecord, final GTUType gtuType)
501 {
502 this.right = rightRecord;
503 this.mayChangeRight =
504 getLane().accessibleAdjacentLanesLegal(LateralDirectionality.RIGHT, gtuType, this.gtuDirectionality)
505 .contains(rightRecord.getLane());
506 }
507
508
509 @Override
510 public final boolean legalRight()
511 {
512 return this.mayChangeRight;
513 }
514
515
516 @Override
517 public final boolean physicalRight()
518 {
519 return this.right != null;
520 }
521
522
523 @Override
524 public final List<RollingLaneStructureRecord> getNext()
525 {
526 return this.nextList;
527 }
528
529
530
531
532 final void clearNextList()
533 {
534 this.nextList.clear();
535 }
536
537
538
539
540
541 public final void addNext(final RollingLaneStructureRecord next) throws GTUException
542 {
543 Throw.when(this.cutOffEnd != null, GTUException.class,
544 "Cannot add next records to a record that was cut-off at the end.");
545 this.nextList.add(next);
546 }
547
548
549 @Override
550 public final List<RollingLaneStructureRecord> getPrev()
551 {
552 return this.prevList;
553 }
554
555
556
557
558 final void clearPrevList()
559 {
560 this.prevList.clear();
561 }
562
563
564
565
566
567 public final void addPrev(final RollingLaneStructureRecord prev) throws GTUException
568 {
569 Throw.when(this.cutOffStart != null, GTUException.class,
570 "Cannot add previous records to a record that was cut-off at the start.");
571 this.prevList.add(prev);
572 }
573
574
575
576
577
578
579 public final void setCutOffEnd(final Length cutOffEnd) throws GTUException
580 {
581 Throw.when(!this.nextList.isEmpty(), GTUException.class,
582 "Setting lane record with cut-off end, but there are next records.");
583 this.cutOffEnd = cutOffEnd;
584 }
585
586
587
588
589
590
591 public final void setCutOffStart(final Length cutOffStart) throws GTUException
592 {
593 Throw.when(!this.prevList.isEmpty(), GTUException.class,
594 "Setting lane record with cut-off start, but there are previous records.");
595 this.cutOffStart = cutOffStart;
596 }
597
598
599 @Override
600 public final boolean isCutOffEnd()
601 {
602 return this.cutOffEnd != null;
603 }
604
605
606 @Override
607 public final boolean isCutOffStart()
608 {
609 return this.cutOffStart != null;
610 }
611
612
613
614
615
616 public final Length getCutOffEnd()
617 {
618 return this.cutOffEnd;
619 }
620
621
622
623
624
625 public final Length getCutOffStart()
626 {
627 return this.cutOffStart;
628 }
629
630
631
632
633 public final void clearCutOffEnd()
634 {
635 this.cutOffEnd = null;
636 }
637
638
639
640
641 public final void clearCutOffStart()
642 {
643 this.cutOffStart = null;
644 }
645
646
647 @Override
648 public final boolean isDeadEnd()
649 {
650 return this.cutOffEnd == null && this.nextList.isEmpty();
651 }
652
653
654 @Override
655 public final Lane getLane()
656 {
657 return this.lane;
658 }
659
660
661 @Override
662 public final GTUDirectionality getDirection()
663 {
664 return this.gtuDirectionality;
665 }
666
667
668 @Override
669 public final Length getStartDistance()
670 {
671 return this.startDistance;
672 }
673
674
675 @Override
676 public boolean isDownstreamBranch()
677 {
678
679 return !RecordLink.UP.equals(this.sourceLink) && !RecordLink.LATERAL_END.equals(this.sourceLink);
680 }
681
682
683 @Override
684 public final String toString()
685 {
686
687 String s;
688 if (this.source == null)
689 {
690 s = "o";
691 }
692 else if (this.source == this.left)
693 {
694 s = "^";
695 }
696 else if (this.source == this.right)
697 {
698 s = "v";
699 }
700 else if (this.prevList.contains(this.source))
701 {
702 s = "<";
703 }
704 else if (this.nextList.contains(this.source))
705 {
706 s = ">";
707 }
708 else
709 {
710 s = "?";
711 }
712 return "LaneStructureRecord [lane=" + this.lane + " (" + s + "), direction=" + this.gtuDirectionality + "]";
713 }
714
715
716
717
718
719
720
721
722
723
724
725
726
727 public enum RecordLink
728 {
729
730
731 UP
732 {
733
734 @Override
735 public Length calculateStartDistance(final RollingLaneStructureRecord startDistanceSource,
736 final RollingLaneStructureRecord self, final double fractionalPosition)
737 {
738 return startDistanceSource.getStartDistance().minus(self.getLane().getLength());
739 }
740 },
741
742
743 DOWN
744 {
745
746 @Override
747 public Length calculateStartDistance(final RollingLaneStructureRecord startDistanceSource,
748 final RollingLaneStructureRecord self, final double fractionalPosition)
749 {
750 return startDistanceSource.getStartDistance().plus(startDistanceSource.getLane().getLength());
751 }
752 },
753
754
755 LATERAL_END
756 {
757
758 @Override
759 public Length calculateStartDistance(final RollingLaneStructureRecord startDistanceSource,
760 final RollingLaneStructureRecord self, final double fractionalPosition)
761 {
762 return startDistanceSource.getStartDistance().plus(startDistanceSource.getLane().getLength())
763 .minus(self.getLane().getLength());
764 }
765 },
766
767
768 LATERAL_START
769 {
770
771 @Override
772 public Length calculateStartDistance(final RollingLaneStructureRecord startDistanceSource,
773 final RollingLaneStructureRecord self, final double fractionalPosition)
774 {
775 return startDistanceSource.getStartDistance();
776 }
777 },
778
779
780 CROSS
781 {
782
783 @Override
784 public Length calculateStartDistance(final RollingLaneStructureRecord startDistanceSource,
785 final RollingLaneStructureRecord self, final double fractionalPosition)
786 {
787 return self.getLane().getLength().multiplyBy(fractionalPosition).neg();
788 }
789 };
790
791
792
793
794
795
796
797
798 public abstract Length calculateStartDistance(RollingLaneStructureRecord startDistanceSource,
799 RollingLaneStructureRecord self, double fractionalPosition);
800
801 }
802
803 }