1 package org.opentrafficsim.road.gtu.lane;
2
3 import java.util.ArrayList;
4 import java.util.Collections;
5 import java.util.HashMap;
6 import java.util.HashSet;
7 import java.util.Iterator;
8 import java.util.LinkedHashMap;
9 import java.util.LinkedHashSet;
10 import java.util.List;
11 import java.util.Map;
12 import java.util.Set;
13
14 import javax.media.j3d.Bounds;
15 import javax.vecmath.Point3d;
16
17 import org.djunits.unit.DurationUnit;
18 import org.djunits.unit.LengthUnit;
19 import org.djunits.value.vdouble.scalar.Acceleration;
20 import org.djunits.value.vdouble.scalar.Duration;
21 import org.djunits.value.vdouble.scalar.Length;
22 import org.djunits.value.vdouble.scalar.Speed;
23 import org.djunits.value.vdouble.scalar.Time;
24 import org.djutils.exceptions.Throw;
25 import org.djutils.exceptions.Try;
26 import org.opentrafficsim.base.parameters.ParameterException;
27 import org.opentrafficsim.core.dsol.OTSSimulatorInterface;
28 import org.opentrafficsim.core.geometry.OTSGeometryException;
29 import org.opentrafficsim.core.geometry.OTSLine3D;
30 import org.opentrafficsim.core.geometry.OTSLine3D.FractionalFallback;
31 import org.opentrafficsim.core.geometry.OTSPoint3D;
32 import org.opentrafficsim.core.gtu.AbstractGTU;
33 import org.opentrafficsim.core.gtu.GTU;
34 import org.opentrafficsim.core.gtu.GTUDirectionality;
35 import org.opentrafficsim.core.gtu.GTUException;
36 import org.opentrafficsim.core.gtu.GTUType;
37 import org.opentrafficsim.core.gtu.RelativePosition;
38 import org.opentrafficsim.core.gtu.TurnIndicatorStatus;
39 import org.opentrafficsim.core.gtu.perception.EgoPerception;
40 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan;
41 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlanBuilder;
42 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlanException;
43 import org.opentrafficsim.core.network.LateralDirectionality;
44 import org.opentrafficsim.core.network.Link;
45 import org.opentrafficsim.core.network.NetworkException;
46 import org.opentrafficsim.core.perception.Historical;
47 import org.opentrafficsim.core.perception.HistoricalValue;
48 import org.opentrafficsim.core.perception.HistoryManager;
49 import org.opentrafficsim.core.perception.collections.HistoricalLinkedHashMap;
50 import org.opentrafficsim.core.perception.collections.HistoricalMap;
51 import org.opentrafficsim.road.gtu.lane.perception.LanePerception;
52 import org.opentrafficsim.road.gtu.lane.perception.PerceptionCollectable;
53 import org.opentrafficsim.road.gtu.lane.perception.RelativeLane;
54 import org.opentrafficsim.road.gtu.lane.perception.categories.DefaultSimplePerception;
55 import org.opentrafficsim.road.gtu.lane.perception.categories.InfrastructurePerception;
56 import org.opentrafficsim.road.gtu.lane.perception.categories.neighbors.NeighborsPerception;
57 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayGTU;
58 import org.opentrafficsim.road.gtu.lane.plan.operational.LaneBasedOperationalPlan;
59 import org.opentrafficsim.road.gtu.strategical.LaneBasedStrategicalPlanner;
60 import org.opentrafficsim.road.network.OTSRoadNetwork;
61 import org.opentrafficsim.road.network.RoadNetwork;
62 import org.opentrafficsim.road.network.lane.CrossSectionElement;
63 import org.opentrafficsim.road.network.lane.CrossSectionLink;
64 import org.opentrafficsim.road.network.lane.DirectedLanePosition;
65 import org.opentrafficsim.road.network.lane.Lane;
66 import org.opentrafficsim.road.network.lane.LaneDirection;
67 import org.opentrafficsim.road.network.speed.SpeedLimitInfo;
68 import org.opentrafficsim.road.network.speed.SpeedLimitTypes;
69
70 import nl.tudelft.simulation.dsol.SimRuntimeException;
71 import nl.tudelft.simulation.dsol.formalisms.eventscheduling.SimEvent;
72 import nl.tudelft.simulation.dsol.formalisms.eventscheduling.SimEventInterface;
73 import nl.tudelft.simulation.dsol.logger.SimLogger;
74 import nl.tudelft.simulation.dsol.simtime.SimTimeDoubleUnit;
75 import nl.tudelft.simulation.language.d3.BoundingBox;
76 import nl.tudelft.simulation.language.d3.DirectedPoint;
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103 public abstract class AbstractLaneBasedGTU extends AbstractGTU implements LaneBasedGTU
104 {
105
106 private static final long serialVersionUID = 20140822L;
107
108
109 private final CollisionDetector collisionDetector;
110
111
112
113
114
115
116 private HistoricalMap<Link, Double> fractionalLinkPositions;
117
118
119
120
121
122
123
124 private final HistoricalMap<Lane, GTUDirectionality> currentLanes;
125
126
127 private Map<Lane, List<SimEventInterface<SimTimeDoubleUnit>>> pendingLeaveTriggers = new HashMap<>();
128
129
130 private Map<Lane, List<SimEventInterface<SimTimeDoubleUnit>>> pendingEnterTriggers = new HashMap<>();
131
132
133 private SimEventInterface<SimTimeDoubleUnit> finalizeLaneChangeEvent = null;
134
135
136 private Speed cachedDesiredSpeed;
137
138
139 private Time desiredSpeedTime;
140
141
142 private Acceleration cachedCarFollowingAcceleration;
143
144
145 private Time carFollowingAccelerationTime;
146
147
148 private Object lock = new Object();
149
150
151 @SuppressWarnings("checkstyle:visibilitymodifier")
152 public static Length initialLocationThresholdDifference = new Length(1.0, LengthUnit.MILLIMETER);
153
154
155 private final Historical<TurnIndicatorStatus> turnIndicatorStatus;
156
157
158
159 public static boolean CACHING = true;
160
161
162
163 public static int CACHED_POSITION = 0;
164
165
166
167 public static int NON_CACHED_POSITION = 0;
168
169
170 private VehicleModel vehicleModel = VehicleModel.MINMAX;
171
172
173
174
175
176
177
178
179
180 public AbstractLaneBasedGTU(final String id, final GTUType gtuType, final OTSSimulatorInterface simulator,
181 final OTSRoadNetwork network) throws GTUException
182 {
183 super(id, gtuType, simulator, network);
184 HistoryManager historyManager = simulator.getReplication().getHistoryManager(simulator);
185 this.fractionalLinkPositions = new HistoricalLinkedHashMap<>(historyManager);
186 this.currentLanes = new HistoricalLinkedHashMap<>(historyManager);
187 this.turnIndicatorStatus = new HistoricalValue<>(historyManager, TurnIndicatorStatus.NOTPRESENT);
188 this.collisionDetector = new CollisionDetector(id);
189 }
190
191
192
193
194
195
196
197
198
199
200
201 @SuppressWarnings("checkstyle:designforextension")
202 public void init(final LaneBasedStrategicalPlanner strategicalPlanner,
203 final Set<DirectedLanePosition> initialLongitudinalPositions, final Speed initialSpeed)
204 throws NetworkException, SimRuntimeException, GTUException, OTSGeometryException
205 {
206 Throw.when(null == initialLongitudinalPositions, GTUException.class, "InitialLongitudinalPositions is null");
207 Throw.when(0 == initialLongitudinalPositions.size(), GTUException.class, "InitialLongitudinalPositions is empty set");
208
209 DirectedPoint lastPoint = null;
210 for (DirectedLanePosition pos : initialLongitudinalPositions)
211 {
212
213
214 lastPoint = pos.getLocation();
215 }
216 DirectedPoint initialLocation = lastPoint;
217
218
219 Time now = getSimulator().getSimulatorTime();
220 try
221 {
222 if (initialSpeed.si < OperationalPlan.DRIFTING_SPEED_SI)
223 {
224 this.operationalPlan
225 .set(new OperationalPlan(this, initialLocation, now, new Duration(1E-6, DurationUnit.SECOND)));
226 }
227 else
228 {
229 OTSPoint3D p2 = new OTSPoint3D(initialLocation.x + 1E-6 * Math.cos(initialLocation.getRotZ()),
230 initialLocation.y + 1E-6 * Math.sin(initialLocation.getRotZ()), initialLocation.z);
231 OTSLine3D path = new OTSLine3D(new OTSPoint3D(initialLocation), p2);
232 this.operationalPlan.set(OperationalPlanBuilder.buildConstantSpeedPlan(this, path, now, initialSpeed));
233 }
234 }
235 catch (OperationalPlanException e)
236 {
237 throw new RuntimeException("Initial operational plan could not be created.", e);
238 }
239
240
241 for (DirectedLanePosition directedLanePosition : initialLongitudinalPositions)
242 {
243 Lane lane = directedLanePosition.getLane();
244 addLaneToGtu(lane, directedLanePosition.getPosition(), directedLanePosition.getGtuDirection());
245 }
246
247
248 DirectedLanePosition referencePosition = getReferencePosition();
249 fireTimedEvent(LaneBasedGTU.LANEBASED_INIT_EVENT,
250 new Object[] {getId(), initialLocation, getLength(), getWidth(), referencePosition.getLane(),
251 referencePosition.getPosition(), referencePosition.getGtuDirection(), getGTUType()},
252 getSimulator().getSimulatorTime());
253
254
255 for (DirectedLanePosition directedLanePosition : initialLongitudinalPositions)
256 {
257 Lane lane = directedLanePosition.getLane();
258 lane.addGTU(this, directedLanePosition.getPosition());
259 }
260
261
262 super.init(strategicalPlanner, initialLocation, initialSpeed);
263
264 this.referencePositionTime = Double.NaN;
265
266 }
267
268
269
270
271 @Override
272 public void setParent(final GTU gtu) throws GTUException
273 {
274 for (Lane lane : new HashSet<>(this.currentLanes.keySet()))
275 {
276 leaveLane(lane);
277 }
278 super.setParent(gtu);
279 }
280
281
282
283
284
285
286
287
288
289 public void reinit(final Set<DirectedLanePosition> initialLongitudinalPositions)
290 throws NetworkException, SimRuntimeException, GTUException, OTSGeometryException
291 {
292 init(getStrategicalPlanner(), initialLongitudinalPositions, Speed.ZERO);
293 }
294
295
296
297
298
299
300 public final boolean isSafeToChange() throws GTUException
301 {
302 return this.fractionalLinkPositions.get(getReferencePosition().getLane().getParentLink()) > 0.0;
303 }
304
305
306 @Override
307 @SuppressWarnings("checkstyle:designforextension")
308 public void enterLane(final Lane lane, final Length position, final GTUDirectionality gtuDirection) throws GTUException
309 {
310 if (lane == null || gtuDirection == null || position == null)
311 {
312 throw new GTUException("enterLane - one of the arguments is null");
313 }
314 addLaneToGtu(lane, position, gtuDirection);
315 addGtuToLane(lane, position);
316 }
317
318
319
320
321
322
323
324
325
326 private void addLaneToGtu(final Lane lane, final Length position, final GTUDirectionality gtuDirection) throws GTUException
327 {
328 if (this.currentLanes.containsKey(lane))
329 {
330 System.err.println(this + " is already registered on lane: " + lane + " at fractional position "
331 + this.fractionalPosition(lane, RelativePosition.REFERENCE_POSITION) + " intended position is " + position
332 + " length of lane is " + lane.getLength());
333 return;
334 }
335
336
337 if (!this.fractionalLinkPositions.containsKey(lane.getParentLink()))
338 {
339 this.fractionalLinkPositions.put(lane.getParentLink(), lane.fraction(position));
340 }
341 this.currentLanes.put(lane, gtuDirection);
342 }
343
344
345
346
347
348
349
350 protected void addGtuToLane(final Lane lane, final Length position) throws GTUException
351 {
352 List<SimEventInterface<SimTimeDoubleUnit>> pending = this.pendingEnterTriggers.get(lane);
353 if (null != pending)
354 {
355 for (SimEventInterface<SimTimeDoubleUnit> event : pending)
356 {
357 if (event.getAbsoluteExecutionTime().get().ge(getSimulator().getSimulatorTime()))
358 {
359 boolean result = getSimulator().cancelEvent(event);
360 if (!result && event.getAbsoluteExecutionTime().get().ne(getSimulator().getSimulatorTime()))
361 {
362 System.err.println("addLaneToGtu, trying to remove event: NOTHING REMOVED -- result=" + result
363 + ", simTime=" + getSimulator().getSimulatorTime() + ", eventTime="
364 + event.getAbsoluteExecutionTime().get());
365 }
366 }
367 }
368 this.pendingEnterTriggers.remove(lane);
369 }
370 lane.addGTU(this, position);
371 }
372
373
374 @Override
375 @SuppressWarnings("checkstyle:designforextension")
376 public void leaveLane(final Lane lane) throws GTUException
377 {
378 leaveLane(lane, false);
379 }
380
381
382
383
384
385
386
387 @SuppressWarnings("checkstyle:designforextension")
388 public void leaveLane(final Lane lane, final boolean beingDestroyed) throws GTUException
389 {
390 Length position = position(lane, getReference());
391 this.currentLanes.remove(lane);
392 removePendingEvents(lane, this.pendingLeaveTriggers);
393 removePendingEvents(lane, this.pendingEnterTriggers);
394
395 boolean found = false;
396 for (Lane l : this.currentLanes.keySet())
397 {
398 if (l.getParentLink().equals(lane.getParentLink()))
399 {
400 found = true;
401 }
402 }
403 if (!found)
404 {
405 this.fractionalLinkPositions.remove(lane.getParentLink());
406 }
407 lane.removeGTU(this, !found, position);
408 if (this.currentLanes.size() == 0 && !beingDestroyed)
409 {
410 System.err.println("leaveLane: lanes.size() = 0 for GTU " + getId());
411 }
412 }
413
414
415
416
417
418
419 private void removePendingEvents(final Lane lane, final Map<Lane, List<SimEventInterface<SimTimeDoubleUnit>>> triggers)
420 {
421 List<SimEventInterface<SimTimeDoubleUnit>> pending = triggers.get(lane);
422 if (null != pending)
423 {
424 for (SimEventInterface<SimTimeDoubleUnit> event : pending)
425 {
426 if (event.getAbsoluteExecutionTime().get().ge(getSimulator().getSimulatorTime()))
427 {
428 boolean result = getSimulator().cancelEvent(event);
429 if (!result && event.getAbsoluteExecutionTime().get().ne(getSimulator().getSimulatorTime()))
430 {
431 System.err.println("leaveLane, trying to remove event: NOTHING REMOVED -- result=" + result
432 + ", simTime=" + getSimulator().getSimulatorTime() + ", eventTime="
433 + event.getAbsoluteExecutionTime().get());
434 }
435 }
436 }
437 triggers.remove(lane);
438 }
439 }
440
441
442 @Override
443 public void changeLaneInstantaneously(final LateralDirectionality laneChangeDirection) throws GTUException
444 {
445
446
447 DirectedLanePosition from = getReferencePosition();
448
449
450 Set<Lane> lanesToBeRemoved = new LinkedHashSet<>(this.currentLanes.keySet());
451
452
453
454
455 Map<Link, Double> newLinkPositionsLC = new LinkedHashMap<>(this.fractionalLinkPositions);
456
457
458 Set<Lane> adjLanes = from.getLane().accessibleAdjacentLanesPhysical(laneChangeDirection, getGTUType(),
459 this.currentLanes.get(from.getLane()));
460 Lane adjLane = adjLanes.iterator().next();
461 Length position = adjLane.position(from.getLane().fraction(from.getPosition()));
462 GTUDirectionality direction = getDirection(from.getLane());
463 Length planLength = Try.assign(() -> getOperationalPlan().getTraveledDistance(getSimulator().getSimulatorTime()),
464 "Exception while determining plan length.");
465 enterLaneRecursive(new LaneDirection(adjLane, direction), position, newLinkPositionsLC, planLength, lanesToBeRemoved,
466 0);
467
468
469 this.fractionalLinkPositions.clear();
470 this.fractionalLinkPositions.putAll(newLinkPositionsLC);
471
472
473 for (Lane lane : lanesToBeRemoved)
474 {
475 leaveLane(lane);
476 }
477
478
479 this.referencePositionTime = Double.NaN;
480 this.cachedPositions.clear();
481
482
483 this.fireTimedEvent(LaneBasedGTU.LANE_CHANGE_EVENT, new Object[] {getId(), laneChangeDirection, from},
484 getSimulator().getSimulatorTime());
485
486 }
487
488
489
490
491
492
493
494
495
496
497
498
499 private void enterLaneRecursive(final LaneDirection lane, final Length position, final Map<Link, Double> newLinkPositionsLC,
500 final Length planLength, final Set<Lane> lanesToBeRemoved, final int dir) throws GTUException
501 {
502 enterLane(lane.getLane(), position, lane.getDirection());
503 lanesToBeRemoved.remove(lane);
504 Length adjusted = lane.getDirection().isPlus() ? position.minus(planLength) : position.plus(planLength);
505 newLinkPositionsLC.put(lane.getLane().getParentLink(), adjusted.si / lane.getLength().si);
506
507
508 if (dir < 1)
509 {
510 Length rear = lane.getDirection().isPlus() ? position.plus(getRear().getDx()) : position.minus(getRear().getDx());
511 Length before = null;
512 if (lane.getDirection().isPlus() && rear.si < 0.0)
513 {
514 before = rear.neg();
515 }
516 else if (lane.getDirection().isMinus() && rear.si > lane.getLength().si)
517 {
518 before = rear.minus(lane.getLength());
519 }
520 if (before != null)
521 {
522 GTUDirectionality upDir = lane.getDirection();
523 Map<Lane, GTUDirectionality> upstream = lane.getLane().upstreamLanes(upDir, getGTUType());
524 if (!upstream.isEmpty())
525 {
526 Lane upLane = null;
527 for (Lane nextUp : upstream.keySet())
528 {
529 if (newLinkPositionsLC.containsKey(nextUp.getParentLink()))
530 {
531
532
533 upLane = nextUp;
534 break;
535 }
536 }
537 if (upLane == null)
538 {
539
540
541
542 upLane = upstream.keySet().iterator().next();
543 }
544 upDir = upstream.get(upLane);
545 LaneDirection next = new LaneDirection(upLane, upDir);
546 Length nextPos = upDir.isPlus() ? next.getLength().minus(before).minus(getRear().getDx())
547 : before.plus(getRear().getDx());
548 enterLaneRecursive(next, nextPos, newLinkPositionsLC, planLength, lanesToBeRemoved, -1);
549 }
550 }
551 }
552
553
554 if (dir > -1)
555 {
556 Length front =
557 lane.getDirection().isPlus() ? position.plus(getFront().getDx()) : position.minus(getFront().getDx());
558 Length passed = null;
559 if (lane.getDirection().isPlus() && front.si > lane.getLength().si)
560 {
561 passed = front.minus(lane.getLength());
562 }
563 else if (lane.getDirection().isMinus() && front.si < 0.0)
564 {
565 passed = front.neg();
566 }
567 if (passed != null)
568 {
569 LaneDirection next = lane.getNextLaneDirection(this);
570 Length nextPos = next.getDirection().isPlus() ? passed.minus(getFront().getDx())
571 : next.getLength().minus(passed).plus(getFront().getDx());
572 enterLaneRecursive(next, nextPos, newLinkPositionsLC, planLength, lanesToBeRemoved, 1);
573 }
574 }
575 }
576
577
578
579
580
581
582 @SuppressWarnings("checkstyle:designforextension")
583 public void initLaneChange(final LateralDirectionality laneChangeDirection) throws GTUException
584 {
585 Map<Lane, GTUDirectionality> lanesCopy = new LinkedHashMap<>(this.currentLanes);
586 Map<Lane, Double> fractionalLanePositions = new HashMap<>();
587 for (Lane lane : lanesCopy.keySet())
588 {
589 fractionalLanePositions.put(lane, fractionalPosition(lane, getReference()));
590 }
591 int numRegistered = 0;
592 for (Lane lane : lanesCopy.keySet())
593 {
594 Set<Lane> laneSet = lane.accessibleAdjacentLanesLegal(laneChangeDirection, getGTUType(), getDirection(lane));
595 if (laneSet.size() > 0)
596 {
597 numRegistered++;
598 Lane adjacentLane = laneSet.iterator().next();
599 enterLane(adjacentLane, adjacentLane.getLength().multiplyBy(fractionalLanePositions.get(lane)),
600 lanesCopy.get(lane));
601 }
602 }
603 Throw.when(numRegistered == 0, GTUException.class, "Gtu %s starting %s lane change, but no adjacent lane found.",
604 getId(), laneChangeDirection);
605 }
606
607
608
609
610
611 @SuppressWarnings("checkstyle:designforextension")
612 protected void finalizeLaneChange(final LateralDirectionality laneChangeDirection)
613 {
614 Map<Lane, GTUDirectionality> lanesCopy = new LinkedHashMap<>(this.currentLanes);
615 Set<Lane> lanesToBeRemoved = new LinkedHashSet<>();
616 Lane fromLane = null;
617 Length fromPosition = null;
618 GTUDirectionality fromDirection = null;
619 try
620 {
621
622 for (Lane lane : lanesCopy.keySet())
623 {
624 Iterator<Lane> iterator =
625 lane.accessibleAdjacentLanesPhysical(laneChangeDirection, getGTUType(), getDirection(lane)).iterator();
626 if (iterator.hasNext() && lanesCopy.keySet().contains(iterator.next()))
627 {
628 lanesToBeRemoved.add(lane);
629 }
630 }
631
632 boolean added = true;
633 while (added)
634 {
635 added = false;
636 Set<Lane> lanesToAlsoBeRemoved = new LinkedHashSet<>();
637 for (Lane lane : lanesToBeRemoved)
638 {
639 GTUDirectionality direction = getDirection(lane);
640 for (Lane nextLane : direction.isPlus() ? lane.nextLanes(getGTUType()).keySet()
641 : lane.prevLanes(getGTUType()).keySet())
642 {
643 if (lanesCopy.containsKey(nextLane) && !lanesToBeRemoved.contains(nextLane))
644 {
645 added = true;
646 lanesToAlsoBeRemoved.add(nextLane);
647 }
648 }
649 }
650 lanesToBeRemoved.addAll(lanesToAlsoBeRemoved);
651 }
652 double nearest = Double.POSITIVE_INFINITY;
653 for (Lane lane : lanesToBeRemoved)
654 {
655 Length pos = position(lane, RelativePosition.REFERENCE_POSITION);
656 if (0.0 <= pos.si && pos.si <= lane.getLength().si)
657 {
658 fromLane = lane;
659 fromPosition = pos;
660 fromDirection = getDirection(lane);
661 }
662 else if (fromLane == null && (getDirection(lane).isPlus() ? pos.si > lane.getLength().si : pos.le0()))
663 {
664
665 double distance = getDirection(lane).isPlus() ? pos.si - lane.getLength().si : -pos.si;
666 if (distance < nearest)
667 {
668 nearest = distance;
669 fromLane = lane;
670 fromPosition = pos;
671 fromDirection = getDirection(lane);
672 }
673 }
674 leaveLane(lane);
675 }
676 this.referencePositionTime = Double.NaN;
677 this.finalizeLaneChangeEvent = null;
678 }
679 catch (GTUException exception)
680 {
681
682 throw new RuntimeException("position on lane not possible", exception);
683 }
684 Throw.when(fromLane == null, RuntimeException.class, "No from lane for lane change event.");
685 DirectedLanePosition from;
686 try
687 {
688 from = new DirectedLanePosition(fromLane, fromPosition, fromDirection);
689 }
690 catch (GTUException exception)
691 {
692 throw new RuntimeException(exception);
693 }
694 this.fireTimedEvent(LaneBasedGTU.LANE_CHANGE_EVENT, new Object[] {getId(), laneChangeDirection, from},
695 getSimulator().getSimulatorTime());
696 }
697
698
699 @Override
700 public void setFinalizeLaneChangeEvent(final SimEventInterface<SimTimeDoubleUnit> event)
701 {
702 this.finalizeLaneChangeEvent = event;
703 }
704
705
706 @Override
707 public final GTUDirectionality getDirection(final Lane lane) throws GTUException
708 {
709 Throw.when(!this.currentLanes.containsKey(lane), GTUException.class, "getDirection: Lanes %s does not contain %s",
710 this.currentLanes.keySet(), lane);
711 return this.currentLanes.get(lane);
712 }
713
714
715 @Override
716 @SuppressWarnings("checkstyle:designforextension")
717 protected void move(final DirectedPoint fromLocation)
718 throws SimRuntimeException, GTUException, OperationalPlanException, NetworkException, ParameterException
719 {
720
721
722
723 if (this.currentLanes.isEmpty())
724 {
725 destroy();
726 return;
727 }
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745 Length covered;
746 if (getOperationalPlan() instanceof LaneBasedOperationalPlan
747 && ((LaneBasedOperationalPlan) getOperationalPlan()).isDeviative())
748 {
749
750
751 LaneBasedOperationalPlan plan = (LaneBasedOperationalPlan) getOperationalPlan();
752 DirectedLanePosition ref = getReferencePosition();
753 covered = ref.getGtuDirection().isPlus()
754 ? position(ref.getLane(), getReference())
755 .minus(position(ref.getLane(), getReference(), plan.getStartTime()))
756 : position(ref.getLane(), getReference(), plan.getStartTime())
757 .minus(position(ref.getLane(), getReference()));
758
759
760 }
761 else
762 {
763 covered = getOperationalPlan().getTraveledDistance(getSimulator().getSimulatorTime());
764 }
765
766
767
768 super.move(fromLocation);
769
770
771
772
773
774 Map<Link, Double> newLinkFractions = new LinkedHashMap<>(this.fractionalLinkPositions);
775 Set<Link> done = new LinkedHashSet<>();
776
777 updateLinkFraction(getReferencePosition().getLane(), newLinkFractions, done, false, covered, true);
778 updateLinkFraction(getReferencePosition().getLane(), newLinkFractions, done, true, covered, true);
779 this.fractionalLinkPositions.clear();
780 this.fractionalLinkPositions.putAll(newLinkFractions);
781
782 DirectedLanePosition dlp = getReferencePosition();
783 fireTimedEvent(
784 LaneBasedGTU.LANEBASED_MOVE_EVENT, new Object[] {getId(), fromLocation, getSpeed(), getAcceleration(),
785 getTurnIndicatorStatus(), getOdometer(), dlp.getLane(), dlp.getPosition(), dlp.getGtuDirection()},
786 getSimulator().getSimulatorTime());
787
788 if (getOperationalPlan().getAcceleration(Duration.ZERO).si < -10
789 && getOperationalPlan().getSpeed(Duration.ZERO).si > 2.5)
790 {
791 System.err.println("GTU: " + getId() + " - getOperationalPlan().getAcceleration(Duration.ZERO).si < -10)");
792 System.err.println("Lanes in current plan: " + this.currentLanes.keySet());
793 if (getTacticalPlanner().getPerception().contains(DefaultSimplePerception.class))
794 {
795 DefaultSimplePerception p =
796 getTacticalPlanner().getPerception().getPerceptionCategory(DefaultSimplePerception.class);
797 System.err.println("HeadwayGTU: " + p.getForwardHeadwayGTU());
798 System.err.println("HeadwayObject: " + p.getForwardHeadwayObject());
799 }
800 }
801
802
803
804
805
806
807 scheduleEnterLeaveTriggers();
808 }
809
810
811
812
813
814
815
816
817
818
819 private void updateLinkFraction(final Lane lane, final Map<Link, Double> newLinkFractions, final Set<Link> done,
820 final boolean prevs, final Length covered, final boolean isReferenceLane)
821 {
822 if (!prevs || !isReferenceLane)
823 {
824 if (done.contains(lane.getParentLink()) || !this.currentLanes.containsKey(lane))
825 {
826 return;
827 }
828 double sign;
829 try
830 {
831 sign = getDirection(lane).isPlus() ? 1.0 : -1.0;
832 }
833 catch (GTUException exception)
834 {
835
836 throw new RuntimeException("Unexpected exception: trying to obtain direction on lane.", exception);
837 }
838 newLinkFractions.put(lane.getParentLink(),
839 this.fractionalLinkPositions.get(lane.getParentLink()) + sign * covered.si / lane.getLength().si);
840 done.add(lane.getParentLink());
841 }
842 for (Lane nextLane : (prevs ? lane.prevLanes(getGTUType()) : lane.nextLanes(getGTUType())).keySet())
843 {
844 updateLinkFraction(nextLane, newLinkFractions, done, prevs, covered, false);
845 }
846 }
847
848
849 @Override
850 public final Map<Lane, Length> positions(final RelativePosition relativePosition) throws GTUException
851 {
852 return positions(relativePosition, getSimulator().getSimulatorTime());
853 }
854
855
856 @Override
857 public final Map<Lane, Length> positions(final RelativePosition relativePosition, final Time when) throws GTUException
858 {
859 Map<Lane, Length> positions = new LinkedHashMap<>();
860 for (Lane lane : this.currentLanes.keySet())
861 {
862 positions.put(lane, position(lane, relativePosition, when));
863 }
864 return positions;
865 }
866
867
868 @Override
869 public final Length position(final Lane lane, final RelativePosition relativePosition) throws GTUException
870 {
871 return position(lane, relativePosition, getSimulator().getSimulatorTime());
872 }
873
874
875 @Override
876 @SuppressWarnings("checkstyle:designforextension")
877 public Length translatedPosition(final Lane projectionLane, final RelativePosition relativePosition, final Time when)
878 throws GTUException
879 {
880 CrossSectionLink link = projectionLane.getParentLink();
881 for (CrossSectionElement cse : link.getCrossSectionElementList())
882 {
883 if (cse instanceof Lane)
884 {
885 Lane cseLane = (Lane) cse;
886 if (null != this.currentLanes.get(cseLane))
887 {
888 double fractionalPosition = fractionalPosition(cseLane, RelativePosition.REFERENCE_POSITION, when);
889 Length pos = new Length(projectionLane.getLength().getSI() * fractionalPosition, LengthUnit.SI);
890 if (this.currentLanes.get(cseLane).isPlus())
891 {
892 return pos.plus(relativePosition.getDx());
893 }
894 return pos.minus(relativePosition.getDx());
895 }
896 }
897 }
898 throw new GTUException(this + " is not on any lane of Link " + link);
899 }
900
901
902 @Override
903 @SuppressWarnings("checkstyle:designforextension")
904 public Length projectedPosition(final Lane projectionLane, final RelativePosition relativePosition, final Time when)
905 throws GTUException
906 {
907 CrossSectionLink link = projectionLane.getParentLink();
908 for (CrossSectionElement cse : link.getCrossSectionElementList())
909 {
910 if (cse instanceof Lane)
911 {
912 Lane cseLane = (Lane) cse;
913 if (null != this.currentLanes.get(cseLane))
914 {
915 double fractionalPosition = fractionalPosition(cseLane, relativePosition, when);
916 return new Length(projectionLane.getLength().getSI() * fractionalPosition, LengthUnit.SI);
917 }
918 }
919 }
920 throw new GTUException(this + " is not on any lane of Link " + link);
921 }
922
923
924 private double cachePositionsTime = Double.NaN;
925
926
927 private Map<Integer, Length> cachedPositions = new HashMap<>();
928
929
930 @Override
931 @SuppressWarnings("checkstyle:designforextension")
932 public Length position(final Lane lane, final RelativePosition relativePosition, final Time when) throws GTUException
933 {
934 int cacheIndex = 0;
935 if (CACHING)
936 {
937 cacheIndex = 17 * lane.hashCode() + relativePosition.hashCode();
938 Length l;
939 if (when.si == this.cachePositionsTime && (l = this.cachedPositions.get(cacheIndex)) != null)
940 {
941
942
943
944
945
946
947
948 CACHED_POSITION++;
949 return l;
950 }
951 if (when.si != this.cachePositionsTime)
952 {
953 this.cachedPositions.clear();
954 this.cachePositionsTime = when.si;
955 }
956 }
957 NON_CACHED_POSITION++;
958
959 synchronized (this.lock)
960 {
961 double longitudinalPosition;
962 try
963 {
964 longitudinalPosition = lane.positionSI(this.fractionalLinkPositions.get(when).get(lane.getParentLink()));
965 }
966 catch (NullPointerException exception)
967 {
968 throw exception;
969 }
970 double loc = Double.NaN;
971 try
972 {
973 OperationalPlan plan = getOperationalPlan(when);
974 if (!(plan instanceof LaneBasedOperationalPlan) || !((LaneBasedOperationalPlan) plan).isDeviative())
975 {
976 if (this.currentLanes.get(when).get(lane).isPlus())
977 {
978 loc = longitudinalPosition + plan.getTraveledDistanceSI(when) + relativePosition.getDx().si;
979 }
980 else
981 {
982 loc = longitudinalPosition - plan.getTraveledDistanceSI(when) - relativePosition.getDx().si;
983 }
984 }
985 else
986 {
987
988 DirectedPoint p = plan.getLocation(when, relativePosition);
989 double f = lane.getCenterLine().projectFractional(null, null, p.x, p.y, FractionalFallback.NaN);
990 if (!Double.isNaN(f))
991 {
992 loc = f * lane.getLength().si;
993 }
994 else
995 {
996
997
998
999 boolean upstream = this.fractionalLinkPositions.get(lane.getParentLink()) < 0.0 ? true : false;
1000
1001
1002 int i = 0;
1003 while (true)
1004 {
1005 Set<Lane> otherLanesToConsider = new LinkedHashSet<>();
1006 otherLanesToConsider.addAll(this.currentLanes.keySet());
1007 double distance = getDistanceAtOtherLane(lane, when, upstream, 0.0, p, otherLanesToConsider);
1008
1009 if (!Double.isNaN(distance))
1010 {
1011 if (i == 1 && !Double.isNaN(loc))
1012 {
1013
1014 double loc2 = upstream ? -distance : distance + lane.getLength().si;
1015 double d1 = loc < 0.0 ? -loc : loc - lane.getLength().si;
1016 double d2 = loc2 < 0.0 ? -loc2 : loc2 - lane.getLength().si;
1017 loc = d1 < d2 ? loc : loc2;
1018 break;
1019 }
1020 else
1021 {
1022
1023 loc = upstream ? -distance : distance + lane.getLength().si;
1024 }
1025 }
1026 else if (!Double.isNaN(loc))
1027 {
1028
1029 break;
1030 }
1031 else if (i == 1)
1032 {
1033
1034
1035
1036
1037
1038 loc = lane.getLength().si * lane.getCenterLine().projectFractional(null, null, p.x, p.y,
1039 FractionalFallback.ENDPOINT);
1040 break;
1041 }
1042
1043 i++;
1044 upstream = !upstream;
1045 }
1046 }
1047 }
1048 }
1049 catch (NullPointerException e)
1050 {
1051 throw new GTUException("lanesCurrentOperationalPlan or fractionalLinkPositions is null", e);
1052 }
1053 catch (Exception e)
1054 {
1055 System.err.println(toString());
1056 System.err.println(this.currentLanes.get(when));
1057 System.err.println(this.fractionalLinkPositions.get(when));
1058 throw new GTUException(e);
1059 }
1060 if (Double.isNaN(loc))
1061 {
1062 System.out.println("loc is NaN");
1063 }
1064 Length length = Length.createSI(loc);
1065 if (CACHING)
1066 {
1067 this.cachedPositions.put(cacheIndex, length);
1068 }
1069 return length;
1070 }
1071 }
1072
1073
1074
1075
1076
1077
1078
1079
1080
1081
1082
1083
1084
1085
1086
1087
1088
1089 private double getDistanceAtOtherLane(final Lane lane, final Time when, final boolean upstream, final double distance,
1090 final DirectedPoint point, final Set<Lane> otherLanesToConsider) throws GTUException
1091 {
1092 Set<Lane> nextLanes = new HashSet<>(upstream == getDirection(lane).isPlus() ? lane.prevLanes(getGTUType()).keySet()
1093 : lane.nextLanes(getGTUType()).keySet());
1094 nextLanes.retainAll(otherLanesToConsider);
1095 if (!upstream && nextLanes.size() > 1)
1096 {
1097 LaneDirection laneDir = new LaneDirection(lane, getDirection(lane)).getNextLaneDirection(this);
1098 if (nextLanes.contains(laneDir.getLane()))
1099 {
1100 nextLanes.clear();
1101 nextLanes.add(laneDir.getLane());
1102 }
1103 else
1104 {
1105 SimLogger.always().error("Distance on downstream lane could not be determined.");
1106 }
1107 }
1108
1109
1110
1111
1112
1113
1114
1115
1116
1117
1118 if (nextLanes.size() == 0)
1119 {
1120 return Double.NaN;
1121 }
1122 Throw.when(nextLanes.size() > 1, IllegalStateException.class,
1123 "A position (%s) of GTU %s is not on any of the current registered lanes.", point, this.getId());
1124 Lane nextLane = nextLanes.iterator().next();
1125 otherLanesToConsider.remove(lane);
1126 double f = nextLane.getCenterLine().projectFractional(null, null, point.x, point.y, FractionalFallback.NaN);
1127 if (Double.isNaN(f))
1128 {
1129 return getDistanceAtOtherLane(nextLane, when, upstream, distance + nextLane.getLength().si, point,
1130 otherLanesToConsider);
1131 }
1132 return distance + (upstream == this.currentLanes.get(nextLane).isPlus() ? 1.0 - f : f) * nextLane.getLength().si;
1133 }
1134
1135
1136 private double referencePositionTime = Double.NaN;
1137
1138
1139 private DirectedLanePosition cachedReferencePosition = null;
1140
1141
1142 @Override
1143 @SuppressWarnings("checkstyle:designforextension")
1144 public DirectedLanePosition getReferencePosition() throws GTUException
1145 {
1146 if (this.referencePositionTime == getSimulator().getSimulatorTime().si)
1147 {
1148 return this.cachedReferencePosition;
1149 }
1150 boolean anyOnLink = false;
1151 Lane refLane = null;
1152 double closest = Double.POSITIVE_INFINITY;
1153 double minEps = Double.POSITIVE_INFINITY;
1154 for (Lane lane : this.currentLanes.keySet())
1155 {
1156 double fraction = fractionalPosition(lane, getReference());
1157 if (fraction >= 0.0 && fraction <= 1.0)
1158 {
1159 anyOnLink = true;
1160
1161
1162 if (refLane == null)
1163 {
1164 refLane = lane;
1165 }
1166 else
1167 {
1168 DirectedPoint loc = getLocation();
1169 double f = lane.getCenterLine().projectFractional(null, null, loc.x, loc.y, FractionalFallback.ENDPOINT);
1170 double distance = loc.distance(lane.getCenterLine().getLocationFractionExtended(f));
1171 if (refLane != null && Double.isInfinite(closest))
1172 {
1173 f = refLane.getCenterLine().projectFractional(null, null, loc.x, loc.y, FractionalFallback.ENDPOINT);
1174 closest = loc.distance(refLane.getCenterLine().getLocationFractionExtended(f));
1175 }
1176 if (distance < closest)
1177 {
1178 refLane = lane;
1179 closest = distance;
1180 }
1181 }
1182 }
1183 else if (!anyOnLink && Double.isInfinite(closest) && getOperationalPlan() instanceof LaneBasedOperationalPlan
1184 && ((LaneBasedOperationalPlan) getOperationalPlan()).isDeviative())
1185 {
1186 double eps = (fraction > 1.0 ? lane.getCenterLine().getLast() : lane.getCenterLine().getFirst())
1187 .distanceSI(new OTSPoint3D(getLocation()));
1188 if (eps < minEps)
1189 {
1190 minEps = eps;
1191 refLane = lane;
1192 }
1193 }
1194 }
1195 if (refLane != null)
1196 {
1197 this.cachedReferencePosition =
1198 new DirectedLanePosition(refLane, position(refLane, getReference()), this.getDirection(refLane));
1199 this.referencePositionTime = getSimulator().getSimulatorTime().si;
1200 return this.cachedReferencePosition;
1201 }
1202
1203
1204
1205
1206
1207
1208
1209 throw new GTUException("The reference point of GTU " + this + " is not on any of the lanes on which it is registered");
1210 }
1211
1212
1213
1214
1215
1216
1217
1218
1219
1220 @SuppressWarnings("checkstyle:designforextension")
1221 protected void scheduleEnterLeaveTriggers() throws NetworkException, SimRuntimeException, GTUException
1222 {
1223 LaneBasedOperationalPlan plan = null;
1224 double moveSI;
1225 if (getOperationalPlan() instanceof LaneBasedOperationalPlan)
1226 {
1227 plan = (LaneBasedOperationalPlan) getOperationalPlan();
1228 moveSI = plan.getTotalLengthAlongLane(this).si;
1229 }
1230 else
1231 {
1232 moveSI = getOperationalPlan().getTotalLength().si;
1233 }
1234
1235
1236 Map<Lane, GTUDirectionality> lanesCopy = new LinkedHashMap<>(this.currentLanes);
1237 Iterator<Lane> it = lanesCopy.keySet().iterator();
1238 Lane enteredLane = null;
1239 while (it.hasNext() || enteredLane != null)
1240 {
1241
1242 Lane lane;
1243 GTUDirectionality laneDir;
1244 if (enteredLane == null)
1245 {
1246 lane = it.next();
1247 laneDir = lanesCopy.get(lane);
1248 }
1249 else
1250 {
1251 lane = enteredLane;
1252 laneDir = this.currentLanes.get(lane);
1253 }
1254 double sign = laneDir.isPlus() ? 1.0 : -1.0;
1255 enteredLane = null;
1256
1257
1258 if (!Collections.disjoint(this.currentLanes.keySet(), lane.downstreamLanes(laneDir, getGTUType()).keySet()))
1259 {
1260 continue;
1261 }
1262
1263
1264 double referenceStartSI = this.fractionalLinkPositions.get(lane.getParentLink()) * lane.getLength().getSI();
1265
1266 if (laneDir.isPlus())
1267 {
1268 lane.scheduleSensorTriggers(this, referenceStartSI, moveSI);
1269 }
1270 else
1271 {
1272 lane.scheduleSensorTriggers(this, referenceStartSI - moveSI, moveSI);
1273 }
1274
1275 double nextFrontPosSI = referenceStartSI + sign * (moveSI + getFront().getDx().si);
1276 Lane nextLane = null;
1277 GTUDirectionality nextDirection = null;
1278 Length refPosAtLastTimestep = null;
1279 DirectedPoint end = null;
1280 if (laneDir.isPlus() ? nextFrontPosSI > lane.getLength().si : nextFrontPosSI < 0.0)
1281 {
1282 LaneDirection next = new LaneDirection(lane, laneDir).getNextLaneDirection(this);
1283 if (null == next)
1284 {
1285
1286 return;
1287 }
1288 nextLane = next.getLane();
1289 nextDirection = next.getDirection();
1290 double endPos = laneDir.isPlus() ? lane.getLength().si - getFront().getDx().si : getFront().getDx().si;
1291 Lane endLane = lane;
1292 GTUDirectionality endLaneDir = laneDir;
1293 while (endLaneDir.isPlus() ? endPos < 0.0 : endPos > endLane.getLength().si)
1294 {
1295
1296 Map<Lane, GTUDirectionality> map = endLane.upstreamLanes(endLaneDir, getGTUType());
1297 map.keySet().retainAll(this.currentLanes.keySet());
1298 double remain = endLaneDir.isPlus() ? -endPos : endPos - endLane.getLength().si;
1299 endLane = map.keySet().iterator().next();
1300 endLaneDir = map.get(endLane);
1301 endPos = endLaneDir.isPlus() ? endLane.getLength().si - remain : remain;
1302 }
1303 end = endLane.getCenterLine().getLocationExtendedSI(endPos);
1304 if (laneDir.isPlus())
1305 {
1306 refPosAtLastTimestep = nextDirection.isPlus() ? Length.createSI(referenceStartSI - lane.getLength().si)
1307 : Length.createSI(nextLane.getLength().si - referenceStartSI + lane.getLength().si);
1308 }
1309 else
1310 {
1311 refPosAtLastTimestep = nextDirection.isPlus() ? Length.createSI(-referenceStartSI)
1312 : Length.createSI(nextLane.getLength().si + referenceStartSI);
1313 }
1314 }
1315
1316 if (end != null)
1317 {
1318 Time enterTime = getOperationalPlan().timeAtPoint(end, false);
1319 if (enterTime != null)
1320 {
1321 addLaneToGtu(nextLane, refPosAtLastTimestep, nextDirection);
1322 enteredLane = nextLane;
1323 Length coveredDistance;
1324 if (plan == null || !plan.isDeviative())
1325 {
1326 try
1327 {
1328 coveredDistance = getOperationalPlan().getTraveledDistance(enterTime);
1329 }
1330 catch (OperationalPlanException exception)
1331 {
1332 throw new RuntimeException("Enter time of lane beyond plan.", exception);
1333 }
1334 }
1335 else
1336 {
1337 coveredDistance = plan.getDistanceAlongLane(this, end);
1338 }
1339 SimEventInterface<SimTimeDoubleUnit> event = getSimulator().scheduleEventAbs(enterTime, this, this,
1340 "addGtuToLane", new Object[] {nextLane, refPosAtLastTimestep.plus(coveredDistance)});
1341 addEnterTrigger(nextLane, event);
1342 }
1343 }
1344 }
1345
1346
1347 for (Lane lane : this.currentLanes.keySet())
1348 {
1349 double referenceStartSI = this.fractionalLinkPositions.get(lane.getParentLink()) * lane.getLength().getSI();
1350 Time exitTime = null;
1351
1352 GTUDirectionality laneDir = getDirection(lane);
1353 double sign = laneDir.isPlus() ? 1.0 : -1.0;
1354
1355 double nextRearPosSI = referenceStartSI + sign * (getRear().getDx().si + moveSI);
1356 if (plan == null || !plan.isDeviative())
1357 {
1358 if (laneDir.isPlus() ? nextRearPosSI > lane.getLength().si : nextRearPosSI < 0.0)
1359 {
1360 exitTime = getOperationalPlan().timeAtDistance(
1361 Length.createSI((laneDir.isPlus() ? lane.getLength().si - referenceStartSI : referenceStartSI)
1362 - getRear().getDx().si));
1363 }
1364 }
1365 else
1366 {
1367 DirectedPoint end = null;
1368 if (laneDir.isPlus() ? nextRearPosSI > lane.getLength().si : nextRearPosSI < 0.0)
1369 {
1370 double endPos = laneDir.isPlus() ? lane.getLength().si - getRear().getDx().si : getRear().getDx().si;
1371 Lane endLane = lane;
1372 GTUDirectionality endLaneDir = laneDir;
1373 while (endLaneDir.isPlus() ? endPos > endLane.getLength().si : endPos < 0.0)
1374 {
1375 Map<Lane, GTUDirectionality> map = endLane.downstreamLanes(endLaneDir, getGTUType());
1376 map.keySet().retainAll(this.currentLanes.keySet());
1377 double remain = endLaneDir.isPlus() ? endPos - endLane.getLength().si : -endPos;
1378 endLane = map.keySet().iterator().next();
1379 endLaneDir = map.get(endLane);
1380 endPos = endLaneDir.isPlus() ? remain : endLane.getLength().si - remain;
1381 }
1382 end = endLane.getCenterLine().getLocationExtendedSI(endPos);
1383 }
1384 if (end != null)
1385 {
1386 exitTime = getOperationalPlan().timeAtPoint(end, false);
1387 }
1388 }
1389
1390 if (exitTime != null && !Double.isNaN(exitTime.si))
1391 {
1392 SimEvent<SimTimeDoubleUnit> event = new SimEvent<>(new SimTimeDoubleUnit(exitTime), this, this, "leaveLane",
1393 new Object[] {lane, new Boolean(false)});
1394 getSimulator().scheduleEvent(event);
1395 addTrigger(lane, event);
1396 }
1397 }
1398 }
1399
1400
1401 @Override
1402 public final Map<Lane, Double> fractionalPositions(final RelativePosition relativePosition) throws GTUException
1403 {
1404 return fractionalPositions(relativePosition, getSimulator().getSimulatorTime());
1405 }
1406
1407
1408 @Override
1409 public final Map<Lane, Double> fractionalPositions(final RelativePosition relativePosition, final Time when)
1410 throws GTUException
1411 {
1412 Map<Lane, Double> positions = new LinkedHashMap<>();
1413 for (Lane lane : this.currentLanes.keySet())
1414 {
1415 positions.put(lane, fractionalPosition(lane, relativePosition, when));
1416 }
1417 return positions;
1418 }
1419
1420
1421 @Override
1422 public final double fractionalPosition(final Lane lane, final RelativePosition relativePosition, final Time when)
1423 throws GTUException
1424 {
1425 return position(lane, relativePosition, when).getSI() / lane.getLength().getSI();
1426 }
1427
1428
1429 @Override
1430 public final double fractionalPosition(final Lane lane, final RelativePosition relativePosition) throws GTUException
1431 {
1432 return position(lane, relativePosition).getSI() / lane.getLength().getSI();
1433 }
1434
1435
1436 @Override
1437 public final void addTrigger(final Lane lane, final SimEventInterface<SimTimeDoubleUnit> event)
1438 {
1439 List<SimEventInterface<SimTimeDoubleUnit>> list = this.pendingLeaveTriggers.get(lane);
1440 if (null == list)
1441 {
1442 list = new ArrayList<>();
1443 }
1444 list.add(event);
1445 this.pendingLeaveTriggers.put(lane, list);
1446 }
1447
1448
1449
1450
1451
1452
1453 private void addEnterTrigger(final Lane lane, final SimEventInterface<SimTimeDoubleUnit> event)
1454 {
1455 List<SimEventInterface<SimTimeDoubleUnit>> list = this.pendingEnterTriggers.get(lane);
1456 if (null == list)
1457 {
1458 list = new ArrayList<>();
1459 }
1460 list.add(event);
1461 this.pendingEnterTriggers.put(lane, list);
1462 }
1463
1464
1465
1466
1467
1468 public void setVehicleModel(final VehicleModel vehicleModel)
1469 {
1470 this.vehicleModel = vehicleModel;
1471 }
1472
1473
1474 @Override
1475 public VehicleModel getVehicleModel()
1476 {
1477 return this.vehicleModel;
1478 }
1479
1480
1481 @Override
1482 @SuppressWarnings("checkstyle:designforextension")
1483 public void destroy()
1484 {
1485 DirectedLanePosition dlp = null;
1486 try
1487 {
1488 dlp = getReferencePosition();
1489 }
1490 catch (GTUException e)
1491 {
1492
1493 }
1494 DirectedPoint location = this.getOperationalPlan() == null ? new DirectedPoint(0.0, 0.0, 0.0) : getLocation();
1495
1496 synchronized (this.lock)
1497 {
1498 Set<Lane> laneSet = new LinkedHashSet<>(this.currentLanes.keySet());
1499
1500 for (Lane lane : laneSet)
1501 {
1502 try
1503 {
1504 leaveLane(lane, true);
1505 }
1506 catch (GTUException e)
1507 {
1508
1509 }
1510 }
1511 }
1512
1513 if (dlp != null && dlp.getLane() != null)
1514 {
1515 Lane referenceLane = dlp.getLane();
1516 fireTimedEvent(LaneBasedGTU.LANEBASED_DESTROY_EVENT,
1517 new Object[] {getId(), location, getOdometer(), referenceLane, dlp.getPosition(), dlp.getGtuDirection()},
1518 getSimulator().getSimulatorTime());
1519 }
1520 else
1521 {
1522 fireTimedEvent(LaneBasedGTU.LANEBASED_DESTROY_EVENT,
1523 new Object[] {getId(), location, getOdometer(), null, Length.ZERO, null},
1524 getSimulator().getSimulatorTime());
1525 }
1526 if (this.finalizeLaneChangeEvent != null)
1527 {
1528 getSimulator().cancelEvent(this.finalizeLaneChangeEvent);
1529 }
1530
1531 super.destroy();
1532 }
1533
1534
1535 @Override
1536 public final Bounds getBounds()
1537 {
1538 double dx = 0.5 * getLength().doubleValue();
1539 double dy = 0.5 * getWidth().doubleValue();
1540 return new BoundingBox(new Point3d(-dx, -dy, 0.0), new Point3d(dx, dy, 0.0));
1541 }
1542
1543
1544 @Override
1545 public final LaneBasedStrategicalPlanner getStrategicalPlanner()
1546 {
1547 return (LaneBasedStrategicalPlanner) super.getStrategicalPlanner();
1548 }
1549
1550
1551 @Override
1552 public final LaneBasedStrategicalPlanner getStrategicalPlanner(final Time time)
1553 {
1554 return (LaneBasedStrategicalPlanner) super.getStrategicalPlanner(time);
1555 }
1556
1557
1558 @Override
1559 public RoadNetwork getNetwork()
1560 {
1561 return (RoadNetwork) super.getPerceivableContext();
1562 }
1563
1564
1565 @Override
1566 public Speed getDesiredSpeed()
1567 {
1568 Time simTime = getSimulator().getSimulatorTime();
1569 if (this.desiredSpeedTime == null || this.desiredSpeedTime.si < simTime.si)
1570 {
1571 InfrastructurePerception infra =
1572 getTacticalPlanner().getPerception().getPerceptionCategoryOrNull(InfrastructurePerception.class);
1573 SpeedLimitInfo speedInfo;
1574 if (infra == null)
1575 {
1576 speedInfo = new SpeedLimitInfo();
1577 speedInfo.addSpeedInfo(SpeedLimitTypes.MAX_VEHICLE_SPEED, getMaximumSpeed());
1578 }
1579 else
1580 {
1581
1582 speedInfo = infra.getSpeedLimitProspect(RelativeLane.CURRENT).getSpeedLimitInfo(Length.ZERO);
1583 }
1584 this.cachedDesiredSpeed =
1585 Try.assign(() -> getTacticalPlanner().getCarFollowingModel().desiredSpeed(getParameters(), speedInfo),
1586 "Parameter exception while obtaining the desired speed.");
1587 this.desiredSpeedTime = simTime;
1588 }
1589 return this.cachedDesiredSpeed;
1590 }
1591
1592
1593 @Override
1594 public Acceleration getCarFollowingAcceleration()
1595 {
1596 Time simTime = getSimulator().getSimulatorTime();
1597 if (this.carFollowingAccelerationTime == null || this.carFollowingAccelerationTime.si < simTime.si)
1598 {
1599 LanePerception perception = getTacticalPlanner().getPerception();
1600
1601 EgoPerception<?, ?> ego = perception.getPerceptionCategoryOrNull(EgoPerception.class);
1602 Throw.whenNull(ego, "EgoPerception is required to determine the speed.");
1603 Speed speed = ego.getSpeed();
1604
1605 InfrastructurePerception infra = perception.getPerceptionCategoryOrNull(InfrastructurePerception.class);
1606 Throw.whenNull(infra, "InfrastructurePerception is required to determine the desired speed.");
1607 SpeedLimitInfo speedInfo = infra.getSpeedLimitProspect(RelativeLane.CURRENT).getSpeedLimitInfo(Length.ZERO);
1608
1609 NeighborsPerception neighbors = perception.getPerceptionCategoryOrNull(NeighborsPerception.class);
1610 Throw.whenNull(neighbors, "NeighborsPerception is required to determine the car-following acceleration.");
1611 PerceptionCollectable<HeadwayGTU, LaneBasedGTU> leaders = neighbors.getLeaders(RelativeLane.CURRENT);
1612
1613 if (!leaders.isEmpty())
1614 {
1615 leaders.collect(this.collisionDetector);
1616 }
1617
1618 this.cachedCarFollowingAcceleration =
1619 Try.assign(() -> getTacticalPlanner().getCarFollowingModel().followingAcceleration(getParameters(), speed,
1620 speedInfo, leaders), "Parameter exception while obtaining the desired speed.");
1621 this.carFollowingAccelerationTime = simTime;
1622 }
1623 return this.cachedCarFollowingAcceleration;
1624 }
1625
1626
1627 @Override
1628 public final TurnIndicatorStatus getTurnIndicatorStatus()
1629 {
1630 return this.turnIndicatorStatus.get();
1631 }
1632
1633
1634 @Override
1635 public final TurnIndicatorStatus getTurnIndicatorStatus(final Time time)
1636 {
1637 return this.turnIndicatorStatus.get(time);
1638 }
1639
1640
1641 @Override
1642 public final void setTurnIndicatorStatus(final TurnIndicatorStatus turnIndicatorStatus)
1643 {
1644 this.turnIndicatorStatus.set(turnIndicatorStatus);
1645 }
1646
1647
1648 @Override
1649 @SuppressWarnings("checkstyle:designforextension")
1650 public String toString()
1651 {
1652 return String.format("GTU " + getId());
1653 }
1654
1655 }