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