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