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