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