1 package org.opentrafficsim.road.gtu.lane.perception.categories;
2
3 import java.util.ArrayList;
4 import java.util.Collection;
5 import java.util.HashMap;
6 import java.util.HashSet;
7 import java.util.LinkedHashSet;
8 import java.util.List;
9 import java.util.Map;
10 import java.util.Set;
11
12 import org.djunits.unit.LengthUnit;
13 import org.djunits.value.vdouble.scalar.Acceleration;
14 import org.djunits.value.vdouble.scalar.Length;
15 import org.djunits.value.vdouble.scalar.Speed;
16 import org.djunits.value.vdouble.scalar.Time;
17 import org.opentrafficsim.base.TimeStampedObject;
18 import org.opentrafficsim.base.parameters.ParameterException;
19 import org.opentrafficsim.base.parameters.ParameterTypeLength;
20 import org.opentrafficsim.base.parameters.ParameterTypes;
21 import org.opentrafficsim.core.gtu.GTUDirectionality;
22 import org.opentrafficsim.core.gtu.GTUException;
23 import org.opentrafficsim.core.gtu.RelativePosition;
24 import org.opentrafficsim.core.network.LateralDirectionality;
25 import org.opentrafficsim.core.network.NetworkException;
26 import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
27 import org.opentrafficsim.road.gtu.lane.perception.LanePerception;
28 import org.opentrafficsim.road.gtu.lane.perception.headway.AbstractHeadwayGTU;
29 import org.opentrafficsim.road.gtu.lane.perception.headway.GTUStatus;
30 import org.opentrafficsim.road.gtu.lane.perception.headway.Headway;
31 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayDistance;
32 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayGTUSimple;
33 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayObject;
34 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayTrafficLight;
35 import org.opentrafficsim.road.gtu.lane.tactical.AbstractLaneBasedTacticalPlanner;
36 import org.opentrafficsim.road.gtu.lane.tactical.LanePathInfo;
37 import org.opentrafficsim.road.network.lane.Lane;
38 import org.opentrafficsim.road.network.lane.LaneDirection;
39 import org.opentrafficsim.road.network.lane.object.LaneBasedObject;
40 import org.opentrafficsim.road.network.lane.object.trafficlight.TrafficLight;
41
42 import nl.tudelft.simulation.language.Throw;
43
44
45
46
47
48
49
50
51
52
53
54 public class DirectDefaultSimplePerception extends LaneBasedAbstractPerceptionCategory implements DefaultSimplePerception
55 {
56
57
58 private static final long serialVersionUID = 20160811L;
59
60
61 protected static final ParameterTypeLength LOOKAHEAD = ParameterTypes.LOOKAHEAD;
62
63
64 protected static final ParameterTypeLength LOOKBACKOLD = ParameterTypes.LOOKBACKOLD;
65
66
67 private TimeStampedObject<Headway> forwardHeadwayGTU;
68
69
70 private TimeStampedObject<Headway> forwardHeadwayObject;
71
72
73 private TimeStampedObject<Headway> backwardHeadway;
74
75
76 private TimeStampedObject<Speed> speedLimit;
77
78
79 private TimeStampedObject<Map<Lane, Set<Lane>>> accessibleAdjacentLanesLeft;
80
81
82 private TimeStampedObject<Map<Lane, Set<Lane>>> accessibleAdjacentLanesRight;
83
84
85 private TimeStampedObject<Collection<Headway>> parallelHeadwaysLeft;
86
87
88 private TimeStampedObject<Collection<Headway>> parallelHeadwaysRight;
89
90
91 private TimeStampedObject<Collection<Headway>> neighboringHeadwaysLeft;
92
93
94 private TimeStampedObject<Collection<Headway>> neighboringHeadwaysRight;
95
96
97 private TimeStampedObject<LanePathInfo> lanePathInfo;
98
99
100
101
102 public DirectDefaultSimplePerception(final LanePerception perception)
103 {
104 super(perception);
105 }
106
107
108 @Override
109 public final void updateLanePathInfo() throws GTUException, NetworkException, ParameterException
110 {
111 Time timestamp = getTimestamp();
112 this.lanePathInfo = new TimeStampedObject<>(
113 AbstractLaneBasedTacticalPlanner.buildLanePathInfo(getGtu(), getGtu().getParameters().getParameter(LOOKAHEAD)),
114 timestamp);
115 }
116
117
118 @Override
119 public final void updateForwardHeadwayGTU() throws GTUException, NetworkException, ParameterException
120 {
121 Time timestamp = getTimestamp();
122 if (this.lanePathInfo == null || this.lanePathInfo.getTimestamp().ne(timestamp))
123 {
124 updateLanePathInfo();
125 }
126 Length maximumForwardHeadway = getGtu().getParameters().getParameter(LOOKAHEAD);
127 this.forwardHeadwayGTU = new TimeStampedObject<>(forwardHeadway(maximumForwardHeadway, true), timestamp);
128 }
129
130
131 @Override
132 public final void updateForwardHeadwayObject() throws GTUException, NetworkException, ParameterException
133 {
134 Time timestamp = getTimestamp();
135 if (this.lanePathInfo == null || this.lanePathInfo.getTimestamp().ne(timestamp))
136 {
137 updateLanePathInfo();
138 }
139 Length maximumForwardHeadway = getGtu().getParameters().getParameter(LOOKAHEAD);
140 this.forwardHeadwayObject = new TimeStampedObject<>(forwardHeadway(maximumForwardHeadway, false), timestamp);
141 }
142
143
144 @Override
145 public final void updateBackwardHeadway() throws GTUException, ParameterException, NetworkException
146 {
147 Time timestamp = getTimestamp();
148 Length maximumReverseHeadway = getGtu().getParameters().getParameter(LOOKBACKOLD);
149 this.backwardHeadway = new TimeStampedObject<>(backwardHeadway(maximumReverseHeadway), timestamp);
150 }
151
152
153 @Override
154 public final void updateAccessibleAdjacentLanesLeft() throws GTUException
155 {
156 Time timestamp = getTimestamp();
157 Map<Lane, Set<Lane>> accessibleAdjacentLanesMap = new HashMap<>();
158 for (Lane lane : getGtu().positions(getGtu().getReference()).keySet())
159 {
160 Set<Lane> adjacentLanes = new HashSet<>(1);
161 adjacentLanes.addAll(lane.accessibleAdjacentLanesLegal(LateralDirectionality.LEFT, getGtu().getGTUType(),
162 getGtu().getDirection(lane)));
163 accessibleAdjacentLanesMap.put(lane, adjacentLanes);
164 }
165 this.accessibleAdjacentLanesLeft = new TimeStampedObject<>(accessibleAdjacentLanesMap, timestamp);
166 }
167
168
169 @Override
170 public final void updateAccessibleAdjacentLanesRight() throws GTUException
171 {
172 Time timestamp = getTimestamp();
173 Map<Lane, Set<Lane>> accessibleAdjacentLanesMap = new HashMap<>();
174 for (Lane lane : getGtu().positions(getGtu().getReference()).keySet())
175 {
176 Set<Lane> adjacentLanes = new HashSet<>(1);
177 adjacentLanes.addAll(lane.accessibleAdjacentLanesLegal(LateralDirectionality.RIGHT, getGtu().getGTUType(),
178 getGtu().getDirection(lane)));
179 accessibleAdjacentLanesMap.put(lane, adjacentLanes);
180 }
181 this.accessibleAdjacentLanesRight = new TimeStampedObject<>(accessibleAdjacentLanesMap, timestamp);
182 }
183
184
185 @Override
186 public final void updateNeighboringHeadwaysLeft() throws GTUException, ParameterException, NetworkException
187 {
188 Time timestamp = getTimestamp();
189 if (this.accessibleAdjacentLanesLeft == null || !timestamp.equals(this.accessibleAdjacentLanesLeft.getTimestamp()))
190 {
191 updateAccessibleAdjacentLanesLeft();
192 }
193
194 if (this.parallelHeadwaysLeft == null || !timestamp.equals(this.parallelHeadwaysLeft.getTimestamp()))
195 {
196 updateParallelHeadwaysLeft();
197 }
198
199
200 Length maximumForwardHeadway = getGtu().getParameters().getParameter(LOOKAHEAD);
201 Length maximumReverseHeadway = getGtu().getParameters().getParameter(LOOKBACKOLD);
202 this.neighboringHeadwaysLeft = new TimeStampedObject<>(
203 collectNeighborLaneTraffic(LateralDirectionality.LEFT, timestamp, maximumForwardHeadway, maximumReverseHeadway),
204 timestamp);
205 }
206
207
208 @Override
209 public final void updateNeighboringHeadwaysRight() throws GTUException, ParameterException, NetworkException
210 {
211 Time timestamp = getTimestamp();
212 if (this.accessibleAdjacentLanesRight == null || !timestamp.equals(this.accessibleAdjacentLanesRight.getTimestamp()))
213 {
214 updateAccessibleAdjacentLanesRight();
215 }
216
217 if (this.parallelHeadwaysRight == null || !timestamp.equals(this.parallelHeadwaysRight.getTimestamp()))
218 {
219 updateParallelHeadwaysRight();
220 }
221
222
223 Length maximumForwardHeadway = getGtu().getParameters().getParameter(LOOKAHEAD);
224 Length maximumReverseHeadway = getGtu().getParameters().getParameter(LOOKBACKOLD);
225 this.neighboringHeadwaysRight = new TimeStampedObject<>(collectNeighborLaneTraffic(LateralDirectionality.RIGHT,
226 timestamp, maximumForwardHeadway, maximumReverseHeadway), timestamp);
227 }
228
229
230 @Override
231 public final void updateNeighboringHeadways(final LateralDirectionality lateralDirection)
232 throws GTUException, ParameterException, NetworkException
233 {
234 if (lateralDirection.equals(LateralDirectionality.LEFT))
235 {
236 updateNeighboringHeadwaysLeft();
237 }
238 else
239 {
240 updateNeighboringHeadwaysRight();
241 }
242 }
243
244
245 @Override
246 public final void updateParallelHeadwaysLeft() throws GTUException
247 {
248 Time timestamp = getTimestamp();
249 if (this.accessibleAdjacentLanesLeft == null || !timestamp.equals(this.accessibleAdjacentLanesLeft.getTimestamp()))
250 {
251 updateAccessibleAdjacentLanesLeft();
252 }
253 Set<Headway> parallelHeadwaySet = new HashSet<>();
254 for (Lane lane : this.accessibleAdjacentLanesLeft.getObject().keySet())
255 {
256 for (Lane adjacentLane : this.accessibleAdjacentLanesLeft.getObject().get(lane))
257 {
258 parallelHeadwaySet.addAll(parallel(adjacentLane, timestamp));
259 }
260 }
261 this.parallelHeadwaysLeft = new TimeStampedObject<>(parallelHeadwaySet, timestamp);
262 }
263
264
265 @Override
266 public final void updateParallelHeadwaysRight() throws GTUException
267 {
268 Time timestamp = getTimestamp();
269 if (this.accessibleAdjacentLanesRight == null || !timestamp.equals(this.accessibleAdjacentLanesRight.getTimestamp()))
270 {
271 updateAccessibleAdjacentLanesRight();
272 }
273 Set<Headway> parallelHeadwaySet = new HashSet<>();
274 for (Lane lane : this.accessibleAdjacentLanesRight.getObject().keySet())
275 {
276 for (Lane adjacentLane : this.accessibleAdjacentLanesRight.getObject().get(lane))
277 {
278 parallelHeadwaySet.addAll(parallel(adjacentLane, timestamp));
279 }
280 }
281 this.parallelHeadwaysRight = new TimeStampedObject<>(parallelHeadwaySet, timestamp);
282 }
283
284
285 @Override
286 public final void updateParallelHeadways(final LateralDirectionality lateralDirection) throws GTUException
287 {
288 if (lateralDirection.equals(LateralDirectionality.LEFT))
289 {
290 updateParallelHeadwaysLeft();
291 }
292 else
293 {
294 updateParallelHeadwaysRight();
295 }
296 }
297
298
299 @Override
300 public final void updateSpeedLimit() throws GTUException, NetworkException
301 {
302 Time timestamp = getTimestamp();
303
304 Lane lane = getGtu().getReferencePosition().getLane();
305 this.speedLimit = new TimeStampedObject<>(lane.getSpeedLimit(getGtu().getGTUType()), timestamp);
306 }
307
308
309 @Override
310 public final LanePathInfo getLanePathInfo()
311 {
312 return this.lanePathInfo.getObject();
313 }
314
315
316 @Override
317 public final Headway getForwardHeadwayGTU()
318 {
319 return this.forwardHeadwayGTU.getObject();
320 }
321
322
323 @Override
324 public final Headway getForwardHeadwayObject()
325 {
326 return this.forwardHeadwayObject.getObject();
327 }
328
329
330 @Override
331 public final Headway getBackwardHeadway()
332 {
333 return this.backwardHeadway.getObject();
334 }
335
336
337 @Override
338 public final Map<Lane, Set<Lane>> getAccessibleAdjacentLanesLeft()
339 {
340 return this.accessibleAdjacentLanesLeft.getObject();
341 }
342
343
344 @Override
345 public final Map<Lane, Set<Lane>> getAccessibleAdjacentLanesRight()
346 {
347 return this.accessibleAdjacentLanesRight.getObject();
348 }
349
350
351 @Override
352 public final Map<Lane, Set<Lane>> getAccessibleAdjacentLanes(final LateralDirectionality lateralDirection)
353 {
354 return lateralDirection.equals(LateralDirectionality.LEFT) ? this.accessibleAdjacentLanesLeft.getObject()
355 : this.accessibleAdjacentLanesRight.getObject();
356 }
357
358
359 @Override
360 public final Collection<Headway> getNeighboringHeadwaysLeft()
361 {
362 return this.neighboringHeadwaysLeft.getObject();
363 }
364
365
366 @Override
367 public final Collection<Headway> getNeighboringHeadwaysRight()
368 {
369 return this.neighboringHeadwaysRight.getObject();
370 }
371
372
373 @Override
374 public final Collection<Headway> getNeighboringHeadways(final LateralDirectionality lateralDirection)
375 {
376 return lateralDirection.equals(LateralDirectionality.LEFT) ? this.neighboringHeadwaysLeft.getObject()
377 : this.neighboringHeadwaysRight.getObject();
378 }
379
380
381 @Override
382 public final Collection<Headway> getParallelHeadwaysLeft()
383 {
384 return this.parallelHeadwaysLeft.getObject();
385 }
386
387
388 @Override
389 public final Collection<Headway> getParallelHeadwaysRight()
390 {
391 return this.parallelHeadwaysRight.getObject();
392 }
393
394
395 @Override
396 public final Collection<Headway> getParallelHeadways(final LateralDirectionality lateralDirection)
397 {
398 return lateralDirection.equals(LateralDirectionality.LEFT) ? this.parallelHeadwaysLeft.getObject()
399 : this.parallelHeadwaysRight.getObject();
400 }
401
402
403 @Override
404 public final Speed getSpeedLimit()
405 {
406 return this.speedLimit.getObject();
407 }
408
409
410
411
412 public final TimeStampedObject<Headway> getTimeStampedForwardHeadwayGTU()
413 {
414 return this.forwardHeadwayGTU;
415 }
416
417
418
419
420 public final TimeStampedObject<Headway> getTimeStampedForwardHeadwayObject()
421 {
422 return this.forwardHeadwayObject;
423 }
424
425
426
427
428 public final TimeStampedObject<Headway> getTimeStampedBackwardHeadway()
429 {
430 return this.backwardHeadway;
431 }
432
433
434
435
436 public final TimeStampedObject<Map<Lane, Set<Lane>>> getTimeStampedAccessibleAdjacentLanesLeft()
437 {
438 return this.accessibleAdjacentLanesLeft;
439 }
440
441
442
443
444 public final TimeStampedObject<Map<Lane, Set<Lane>>> getTimeStampedAccessibleAdjacentLanesRight()
445 {
446 return this.accessibleAdjacentLanesRight;
447 }
448
449
450
451
452
453 public final TimeStampedObject<Map<Lane, Set<Lane>>> getTimeStampedAccessibleAdjacentLanes(
454 final LateralDirectionality lateralDirection)
455 {
456 return lateralDirection.equals(LateralDirectionality.LEFT) ? this.accessibleAdjacentLanesLeft
457 : this.accessibleAdjacentLanesRight;
458 }
459
460
461
462
463
464
465 public final TimeStampedObject<Collection<Headway>> getTimeStampedNeighboringHeadwaysLeft()
466 {
467 return this.neighboringHeadwaysLeft;
468 }
469
470
471
472
473
474
475 public final TimeStampedObject<Collection<Headway>> getTimeStampedNeighboringHeadwaysRight()
476 {
477 return this.neighboringHeadwaysRight;
478 }
479
480
481
482
483
484
485
486 public final TimeStampedObject<Collection<Headway>> getTimeStampedNeighboringHeadways(
487 final LateralDirectionality lateralDirection)
488 {
489 return lateralDirection.equals(LateralDirectionality.LEFT) ? this.neighboringHeadwaysLeft
490 : this.neighboringHeadwaysRight;
491 }
492
493
494
495
496
497 public final TimeStampedObject<Collection<Headway>> getTimeStampedParallelHeadwaysLeft()
498 {
499 return this.parallelHeadwaysLeft;
500 }
501
502
503
504
505
506 public final TimeStampedObject<Collection<Headway>> getTimeStampedParallelHeadwaysRight()
507 {
508 return this.parallelHeadwaysRight;
509 }
510
511
512
513
514
515
516 public final TimeStampedObject<Collection<Headway>> getTimeStampedParallelHeadways(
517 final LateralDirectionality lateralDirection)
518 {
519 return lateralDirection.equals(LateralDirectionality.LEFT) ? this.parallelHeadwaysLeft : this.parallelHeadwaysRight;
520 }
521
522
523
524
525 public final TimeStampedObject<Speed> getTimeStampedSpeedLimit()
526 {
527 return this.speedLimit;
528 }
529
530
531
532
533
534 public final TimeStampedObject<LanePathInfo> getTimeStampedLanePathInfo()
535 {
536 return this.lanePathInfo;
537 }
538
539
540 @Override
541 public final Lane bestAccessibleAdjacentLane(final Lane currentLane, final LateralDirectionality lateralDirection,
542 final Length longitudinalPosition)
543 {
544 Set<Lane> candidates = getAccessibleAdjacentLanes(lateralDirection).get(currentLane);
545 if (candidates == null || candidates.isEmpty())
546 {
547 return null;
548 }
549 if (candidates.size() == 1)
550 {
551 return candidates.iterator().next();
552 }
553
554 Lane bestLane = null;
555 double widestSeen = Double.NEGATIVE_INFINITY;
556 for (Lane lane : candidates)
557 {
558 if (lane.getWidth(longitudinalPosition).getSI() > widestSeen)
559 {
560 widestSeen = lane.getWidth(longitudinalPosition).getSI();
561 bestLane = lane;
562 }
563 }
564 return bestLane;
565 }
566
567
568 @Override
569 public final String toString()
570 {
571 return "DirectDefaultSimplePerception";
572 }
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595 private Headway forwardHeadway(final Length maxDistance, final boolean gtu) throws GTUException, NetworkException
596 {
597 LanePathInfo lpi = getLanePathInfo();
598 return forwardHeadway(lpi, maxDistance, gtu);
599 }
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620 private Headway forwardHeadway(final LanePathInfo lpi, final Length maxDistance, final boolean gtu)
621 throws GTUException, NetworkException
622 {
623 Throw.when(maxDistance.le0(), GTUException.class, "forwardHeadway: maxDistance should be positive");
624
625 int ldIndex = 0;
626 LaneDirection ld = lpi.getReferenceLaneDirection();
627 double gtuPosFrontSI = lpi.getReferencePosition().si;
628 if (lpi.getReferenceLaneDirection().getDirection().isPlus())
629 {
630 gtuPosFrontSI += getGtu().getFront().getDx().si;
631 }
632 else
633 {
634 gtuPosFrontSI -= getGtu().getFront().getDx().si;
635 }
636
637
638
639 while ((gtuPosFrontSI > ld.getLane().getLength().si || gtuPosFrontSI < 0.0)
640 && ldIndex < lpi.getLaneDirectionList().size() - 1)
641 {
642 ldIndex++;
643 if (ld.getDirection().isPlus())
644 {
645 gtuPosFrontSI -= ld.getLane().getLength().si;
646 }
647 else
648
649 {
650 gtuPosFrontSI *= -1.0;
651 }
652 if (lpi.getLaneDirectionList().get(ldIndex).getDirection().isMinus())
653 {
654 gtuPosFrontSI = lpi.getLaneDirectionList().get(ldIndex).getLane().getLength().si - gtuPosFrontSI;
655 }
656 ld = lpi.getLaneDirectionList().get(ldIndex);
657 }
658
659 double maxDistanceSI = maxDistance.si;
660 Time time = getGtu().getSimulator().getSimulatorTime();
661
662
663 Headway closest = headwayLane(ld, gtuPosFrontSI, 0.0, time, gtu);
664 if (closest != null)
665 {
666 if (closest.getDistance().si > maxDistanceSI)
667 {
668 return new HeadwayDistance(maxDistanceSI);
669 }
670 return closest;
671 }
672 double cumDistSI = ld.getDirection().isPlus() ? ld.getLane().getLength().si - gtuPosFrontSI : gtuPosFrontSI;
673 for (int i = ldIndex + 1; i < lpi.getLaneDirectionList().size(); i++)
674 {
675 ld = lpi.getLaneDirectionList().get(i);
676 closest = headwayLane(ld, ld.getDirection().isPlus() ? 0.0 : ld.getLane().getLength().si, cumDistSI, time, gtu);
677 if (closest != null)
678 {
679 if (closest.getDistance().si > maxDistanceSI)
680 {
681 return new HeadwayDistance(maxDistanceSI);
682 }
683 return closest;
684 }
685 cumDistSI += ld.getLane().getLength().si;
686 }
687 return new HeadwayDistance(maxDistanceSI);
688 }
689
690
691
692
693
694
695
696
697
698
699
700
701 private Headway headwayLane(final LaneDirection laneDirection, final double startPosSI, final double cumDistSI,
702 final Time now, final boolean gtu) throws GTUException
703 {
704 Lane lane = laneDirection.getLane();
705
706 if (gtu)
707 {
708 LaneBasedGTU laneBasedGTU = lane.getGtuAhead(new Length(startPosSI, LengthUnit.SI), laneDirection.getDirection(),
709 RelativePosition.REAR, now);
710 if (laneBasedGTU == null)
711 {
712 return null;
713 }
714 double gtuDistanceSI = Math.abs(laneBasedGTU.position(lane, laneBasedGTU.getRear()).si - startPosSI);
715 return new HeadwayGTUSimple(laneBasedGTU.getId(), laneBasedGTU.getGTUType(),
716 new Length(cumDistSI + gtuDistanceSI, LengthUnit.SI), laneBasedGTU.getLength(), laneBasedGTU.getWidth(),
717 laneBasedGTU.getSpeed(), laneBasedGTU.getAcceleration(), null, getGtuStatus(laneBasedGTU));
718 }
719
720 else
721
722 {
723 List<LaneBasedObject> laneBasedObjects =
724 lane.getObjectAhead(new Length(startPosSI, LengthUnit.SI), laneDirection.getDirection());
725 if (laneBasedObjects == null)
726 {
727 return null;
728 }
729 double objectDistanceSI = Math.abs(laneBasedObjects.get(0).getLongitudinalPosition().si - startPosSI);
730 LaneBasedObject lbo = laneBasedObjects.get(0);
731
732
733 if (lbo instanceof TrafficLight)
734 {
735 TrafficLight tl = (TrafficLight) lbo;
736 if (tl.getTrafficLightColor().isRed())
737 {
738 if (cumDistSI + objectDistanceSI > breakingDistance(MAX_RED_DECELERATION, getGtu().getSpeed()).si)
739 {
740 return new HeadwayTrafficLight(tl, new Length(cumDistSI + objectDistanceSI, LengthUnit.SI));
741 }
742 return new HeadwayTrafficLight(tl, new Length(cumDistSI + objectDistanceSI, LengthUnit.SI));
743 }
744 if (tl.getTrafficLightColor().isYellow())
745 {
746
747
748
749
750 if (cumDistSI + objectDistanceSI > breakingDistance(MAX_YELLOW_DECELERATION, getGtu().getSpeed()).si)
751 {
752 return new HeadwayTrafficLight(tl, new Length(cumDistSI + objectDistanceSI, LengthUnit.SI));
753 }
754 }
755 if (tl.getTrafficLightColor().isRed())
756 {
757 System.err.println(
758 "Not braking for " + tl.getTrafficLightColor() + " because that would require excessive braking");
759 }
760 return null;
761 }
762
763
764 return new HeadwayObject(laneBasedObjects.get(0).getId(), new Length(cumDistSI + objectDistanceSI, LengthUnit.SI));
765 }
766 }
767
768
769
770
771
772
773
774 private Length breakingDistance(final Acceleration deceleration, final Speed initialSpeed)
775 {
776 double a = -deceleration.si;
777 double brakingTime = initialSpeed.si / a;
778 return new Length(0.5 * a * brakingTime * brakingTime, LengthUnit.SI);
779 }
780
781
782
783
784
785
786 private GTUStatus[] getGtuStatus(final LaneBasedGTU gtu)
787 {
788 if (gtu.getTurnIndicatorStatus().isLeft())
789 {
790 return new GTUStatus[] { GTUStatus.LEFT_TURNINDICATOR };
791 }
792 if (gtu.getTurnIndicatorStatus().isRight())
793 {
794 return new GTUStatus[] { GTUStatus.RIGHT_TURNINDICATOR };
795 }
796 if (gtu.getTurnIndicatorStatus().isHazard())
797 {
798 return new GTUStatus[] { GTUStatus.EMERGENCY_LIGHTS };
799 }
800 return new GTUStatus[0];
801 }
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817 private Headway backwardHeadway(final Length maxDistance) throws GTUException, NetworkException
818 {
819 Throw.when(maxDistance.ge0(), GTUException.class, "backwardHeadway: maxDistance should be negative");
820 Time time = getGtu().getSimulator().getSimulatorTime();
821 double maxDistanceSI = maxDistance.si;
822 Headway foundHeadway = new HeadwayDistance(-maxDistanceSI);
823 for (Lane lane : getGtu().positions(getGtu().getRear()).keySet())
824 {
825 Headway closest = headwayRecursiveBackwardSI(lane, getGtu().getDirection(lane),
826 getGtu().position(lane, getGtu().getRear(), time).getSI(), 0.0, -maxDistanceSI, time);
827 if (closest.getDistance().si < -maxDistanceSI
828 && closest.getDistance().si < foundHeadway.getDistance().si)
829 {
830 foundHeadway = closest;
831 }
832 }
833 if (foundHeadway instanceof AbstractHeadwayGTU)
834 {
835 return new HeadwayGTUSimple(foundHeadway.getId(), ((AbstractHeadwayGTU) foundHeadway).getGtuType(),
836 foundHeadway.getDistance().neg(), foundHeadway.getLength(), ((AbstractHeadwayGTU) foundHeadway).getWidth(),
837 foundHeadway.getSpeed(), foundHeadway.getAcceleration(), null);
838 }
839 if (foundHeadway instanceof HeadwayDistance)
840 {
841 return new HeadwayDistance(foundHeadway.getDistance().neg());
842 }
843
844 throw new GTUException("backwardHeadway not implemented yet for other object types than GTU");
845 }
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863 private Headway headwayRecursiveBackwardSI(final Lane lane, final GTUDirectionality direction, final double lanePositionSI,
864 final double cumDistanceSI, final double maxDistanceSI, final Time when) throws GTUException
865 {
866 LaneBasedGTU otherGTU =
867 lane.getGtuBehind(new Length(lanePositionSI, LengthUnit.SI), direction, RelativePosition.FRONT, when);
868 if (otherGTU != null)
869 {
870 double distanceM = cumDistanceSI + lanePositionSI - otherGTU.position(lane, otherGTU.getFront(), when).getSI();
871 if (distanceM > 0 && distanceM <= maxDistanceSI)
872 {
873 return new HeadwayGTUSimple(otherGTU.getId(), otherGTU.getGTUType(), new Length(distanceM, LengthUnit.SI),
874 otherGTU.getLength(), otherGTU.getWidth(), otherGTU.getSpeed(), otherGTU.getAcceleration(), null);
875 }
876 return new HeadwayDistance(Double.MAX_VALUE);
877 }
878
879
880 if (cumDistanceSI + lanePositionSI < maxDistanceSI)
881 {
882
883 if (lane.prevLanes(getGtu().getGTUType()).size() > 0)
884 {
885 Headway foundMaxGTUDistanceSI = new HeadwayDistance(Double.MAX_VALUE);
886 for (Lane prevLane : lane.prevLanes(getGtu().getGTUType()).keySet())
887 {
888
889 double traveledDistanceSI = cumDistanceSI + lanePositionSI;
890
891 Headway closest = headwayRecursiveBackwardSI(prevLane, direction, prevLane.getLength().getSI(),
892 traveledDistanceSI, maxDistanceSI, when);
893 if (closest.getDistance().si < maxDistanceSI
894 && closest.getDistance().si < foundMaxGTUDistanceSI.getDistance().si)
895 {
896 foundMaxGTUDistanceSI = closest;
897 }
898 }
899 return foundMaxGTUDistanceSI;
900 }
901 }
902
903
904 return new HeadwayDistance(Double.MAX_VALUE);
905 }
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921 private Collection<Headway> parallel(final Lane lane, final Time when) throws GTUException
922 {
923 Collection<Headway> headwayCollection = new LinkedHashSet<>();
924 for (Lane l : getGtu().positions(getGtu().getReference()).keySet())
925 {
926
927 if (l.getParentLink().equals(lane.getParentLink()))
928 {
929
930 double posFractionRef = getGtu().fractionalPosition(l, getGtu().getReference(), when);
931 double posFractionFront = Math.max(0.0, posFractionRef + getGtu().getFront().getDx().si / lane.getLength().si);
932 double posFractionRear = Math.min(1.0, posFractionRef + getGtu().getRear().getDx().si / lane.getLength().si);
933
934
935 double posMin = Math.min(posFractionFront, posFractionRear);
936 double posMax = Math.max(posFractionFront, posFractionRear);
937 for (LaneBasedGTU otherGTU : lane.getGtuList())
938 {
939 if (!otherGTU.equals(this))
940 {
941
942
943
944
945 double gtuFractionRef = otherGTU.fractionalPosition(lane, otherGTU.getReference(), when);
946 double gtuFractionFront =
947 Math.max(0.0, gtuFractionRef + otherGTU.getFront().getDx().si / lane.getLength().si);
948 double gtuFractionRear =
949 Math.min(1.0, gtuFractionRef + otherGTU.getRear().getDx().si / lane.getLength().si);
950 double gtuMin = Math.min(gtuFractionFront, gtuFractionRear);
951 double gtuMax = Math.max(gtuFractionFront, gtuFractionRear);
952 if ((gtuMin >= posMin && gtuMin <= posMax) || (gtuMax >= posMin && gtuMax <= posMax)
953 || (posMin >= gtuMin && posMin <= gtuMax) || (posMax >= gtuMin && posMax <= gtuMax))
954 {
955
956 Length overlapFront = new Length(1.0, LengthUnit.SI);
957 Length overlap = new Length(1.0, LengthUnit.SI);
958 Length overlapRear = new Length(1.0, LengthUnit.SI);
959 headwayCollection.add(new HeadwayGTUSimple(otherGTU.getId(), otherGTU.getGTUType(), overlapFront,
960 overlap, overlapRear, otherGTU.getLength(), otherGTU.getWidth(), otherGTU.getSpeed(),
961 otherGTU.getAcceleration(), null, getGtuStatus(otherGTU)));
962 }
963 }
964 }
965 }
966 }
967 return headwayCollection;
968 }
969
970
971
972
973
974
975
976
977
978
979
980
981 private Collection<Headway> parallel(final LateralDirectionality lateralDirection, final Time when) throws GTUException
982 {
983 Collection<Headway> gtuSet = new LinkedHashSet<>();
984 for (Lane lane : getGtu().positions(getGtu().getReference()).keySet())
985 {
986 for (Lane adjacentLane : getAccessibleAdjacentLanes(lateralDirection).get(lane))
987 {
988 gtuSet.addAll(parallel(adjacentLane, when));
989 }
990 }
991 return gtuSet;
992 }
993
994
995
996
997
998
999
1000
1001
1002
1003
1004
1005
1006 private Collection<Headway> collectNeighborLaneTraffic(final LateralDirectionality directionality, final Time when,
1007 final Length maximumForwardHeadway, final Length maximumReverseHeadway)
1008 throws NetworkException, GTUException, ParameterException
1009 {
1010 Collection<Headway> result = new HashSet<>();
1011 for (Headway p : parallel(directionality, when))
1012 {
1013
1014
1015
1016 result.add(p);
1017 }
1018
1019
1020 for (Lane adjacentLane : getAccessibleAdjacentLanes(directionality).get(getLanePathInfo().getReferenceLane()))
1021 {
1022 LanePathInfo lpiAdjacent = buildLanePathInfoAdjacent(adjacentLane, directionality, when);
1023 Headway leader = forwardHeadway(lpiAdjacent, maximumForwardHeadway, true);
1024 if (null != leader.getId() && !result.contains(leader))
1025 {
1026 result.add(leader);
1027 }
1028 }
1029
1030
1031 Lane lane = getGtu().getReferencePosition().getLane();
1032 for (Lane adjacentLane : getAccessibleAdjacentLanes(directionality).get(lane))
1033 {
1034 Headway follower = headwayRecursiveBackwardSI(adjacentLane, getGtu().getDirection(lane),
1035 getGtu().translatedPosition(adjacentLane, getGtu().getRear(), when).getSI(), 0.0,
1036 -maximumReverseHeadway.getSI(), when);
1037 if (follower instanceof AbstractHeadwayGTU)
1038 {
1039 boolean found = false;
1040 for (Headway headway : result)
1041 {
1042 if (headway.getId().equals(follower.getId()))
1043 {
1044 found = true;
1045 }
1046 }
1047 if (!found)
1048 {
1049 result.add(new HeadwayGTUSimple(follower.getId(), ((AbstractHeadwayGTU) follower).getGtuType(),
1050 follower.getDistance().neg(), follower.getLength(), ((AbstractHeadwayGTU) follower).getWidth(),
1051 follower.getSpeed(), follower.getAcceleration(), null));
1052 }
1053 }
1054 else if (follower instanceof HeadwayDistance)
1055 {
1056 result.add(new HeadwayDistance(follower.getDistance().neg()));
1057 }
1058 else
1059 {
1060 throw new GTUException("collectNeighborLaneTraffic not yet suited to observe obstacles on neighboring lanes");
1061 }
1062 }
1063 return result;
1064 }
1065
1066
1067
1068
1069
1070
1071
1072
1073
1074
1075
1076
1077 private LanePathInfo buildLanePathInfoAdjacent(final Lane adjacentLane, final LateralDirectionality direction,
1078 final Time when) throws GTUException, NetworkException, ParameterException
1079 {
1080 if (this.lanePathInfo == null || this.lanePathInfo.getTimestamp().ne(when))
1081 {
1082 updateLanePathInfo();
1083 }
1084 LanePathInfo lpi = getLanePathInfo();
1085 List<LaneDirection> laneDirectionList = new ArrayList<>();
1086 laneDirectionList.add(new LaneDirection(adjacentLane, lpi.getReferenceLaneDirection().getDirection()));
1087 Length referencePosition = getGtu().translatedPosition(adjacentLane, getGtu().getReference(), when);
1088 for (int i = 1; i < lpi.getLaneDirectionList().size(); i++)
1089 {
1090 LaneDirection ld = lpi.getLaneDirectionList().get(i);
1091 Set<Lane> accessibleLanes = ld.getLane().accessibleAdjacentLanesLegal(direction, getGtu().getGTUType(),
1092 lpi.getReferenceLaneDirection().getDirection());
1093 Lane adjLane = null;
1094 for (Lane lane : accessibleLanes)
1095 {
1096 if (lane.getParentLink().equals(ld.getLane().getParentLink()))
1097 {
1098 adjLane = lane;
1099 }
1100 }
1101 if (adjLane == null)
1102 {
1103 break;
1104 }
1105 laneDirectionList.add(new LaneDirection(adjLane, ld.getDirection()));
1106 }
1107 return new LanePathInfo(null, laneDirectionList, referencePosition);
1108 }
1109
1110 }