View Javadoc
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   * This class contains most of the code that is needed to run a lane based GTU. <br>
77   * The starting point of a LaneBasedTU is that it can be in <b>multiple lanes</b> at the same time. This can be due to a lane
78   * change (lateral), or due to crossing a link (front of the GTU is on another Lane than rear of the GTU). If a Lane is shorter
79   * than the length of the GTU (e.g. when we do node expansion on a crossing, this is very well possible), a GTU could occupy
80   * dozens of Lanes at the same time.
81   * <p>
82   * When calculating a headway, the GTU has to look in successive lanes. When Lanes (or underlying CrossSectionLinks) diverge,
83   * the headway algorithms have to look at multiple Lanes and return the minimum headway in each of the Lanes. When the Lanes (or
84   * underlying CrossSectionLinks) converge, "parallel" traffic is not taken into account in the headway calculation. Instead, gap
85   * acceptance algorithms or their equivalent should guide the merging behavior.
86   * <p>
87   * To decide its movement, an AbstractLaneBasedGTU applies its car following algorithm and lane change algorithm to set the
88   * acceleration and any lane change operation to perform. It then schedules the triggers that will add it to subsequent lanes
89   * and remove it from current lanes as needed during the time step that is has committed to. Finally, it re-schedules its next
90   * movement evaluation with the simulator.
91   * <p>
92   * Copyright (c) 2013-2022 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
93   * BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
94   * <p>
95   * @version $Revision: 1408 $, $LastChangedDate: 2015-09-24 15:17:25 +0200 (Thu, 24 Sep 2015) $, by $Author: pknoppers $,
96   *          initial version Oct 22, 2014 <br>
97   * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
98   * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
99   */
100 public abstract class AbstractLaneBasedGTU extends AbstractGTU implements LaneBasedGTU
101 {
102     /** */
103     private static final long serialVersionUID = 20140822L;
104 
105     /**
106      * Fractional longitudinal positions of the reference point of the GTU on one or more links at the start of the current
107      * operational plan. Because the reference point of the GTU might not be on all the links the GTU is registered on, the
108      * fractional longitudinal positions can be more than one, or less than zero.
109      */
110     private HistoricalMap<Link, Double> fractionalLinkPositions;
111 
112     /**
113      * The lanes the GTU is registered on. Each lane has to have its link registered in the fractionalLinkPositions as well to
114      * keep consistency. Each link from the fractionalLinkPositions can have one or more Lanes on which the vehicle is
115      * registered. This is a list to improve reproducibility: The 'oldest' lanes on which the vehicle is registered are at the
116      * front of the list, the later ones more to the back.
117      */
118     private final HistoricalMap<Lane, GTUDirectionality> currentLanes;
119 
120     /** Maps that we enter when initiating a lane change, but we may not actually enter given a deviative plan. */
121     private final Set<Lane> enteredLanes = new LinkedHashSet<>();
122 
123     /** Pending leave triggers for each lane. */
124     private Map<Lane, List<SimEventInterface<Duration>>> pendingLeaveTriggers = new LinkedHashMap<>();
125 
126     /** Pending enter triggers for each lane. */
127     private Map<Lane, List<SimEventInterface<Duration>>> pendingEnterTriggers = new LinkedHashMap<>();
128 
129     /** Event to finalize lane change. */
130     private SimEventInterface<Duration> finalizeLaneChangeEvent = null;
131 
132     /** Cached desired speed. */
133     private Speed cachedDesiredSpeed;
134 
135     /** Time desired speed was cached. */
136     private Time desiredSpeedTime;
137 
138     /** Cached car-following acceleration. */
139     private Acceleration cachedCarFollowingAcceleration;
140 
141     /** Time car-following acceleration was cached. */
142     private Time carFollowingAccelerationTime;
143 
144     /** The object to lock to make the GTU thread safe. */
145     private Object lock = new Object();
146 
147     /** The threshold distance for differences between initial locations of the GTU on different lanes. */
148     @SuppressWarnings("checkstyle:visibilitymodifier")
149     public static Length initialLocationThresholdDifference = new Length(1.0, LengthUnit.MILLIMETER);
150 
151     /** Turn indicator status. */
152     private final Historical<TurnIndicatorStatus> turnIndicatorStatus;
153 
154     /** Caching on or off. */
155     // TODO: should be indicated with a Parameter
156     public static boolean CACHING = true;
157 
158     /** cached position count. */
159     // TODO: can be removed after testing period
160     public static int CACHED_POSITION = 0;
161 
162     /** cached position count. */
163     // TODO: can be removed after testing period
164     public static int NON_CACHED_POSITION = 0;
165 
166     /** Vehicle model. */
167     private VehicleModel vehicleModel = VehicleModel.MINMAX;
168 
169     /**
170      * Construct a Lane Based GTU.
171      * @param id String; the id of the GTU
172      * @param gtuType GTUType; the type of GTU, e.g. TruckType, CarType, BusType
173      * @param network OTSRoadNetwork; the network that the GTU is initially registered in
174      * @throws GTUException when initial values are not correct
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      * @param strategicalPlanner LaneBasedStrategicalPlanner; the strategical planner (e.g., route determination) to use
188      * @param initialLongitudinalPositions Set&lt;DirectedLanePosition&gt;; the initial positions of the car on one or more
189      *            lanes with their directions
190      * @param initialSpeed Speed; the initial speed of the car on the lane
191      * @throws NetworkException when the GTU cannot be placed on the given lane
192      * @throws SimRuntimeException when the move method cannot be scheduled
193      * @throws GTUException when initial values are not correct
194      * @throws OTSGeometryException when the initial path is wrong
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             // Throw.when(lastPoint != null && pos.getLocation().distance(lastPoint) > initialLocationThresholdDifference.si,
208             // GTUException.class, "initial locations for GTU have distance > " + initialLocationThresholdDifference);
209             lastPoint = pos.getLocation();
210         }
211         DirectedPoint initialLocation = lastPoint;
212 
213         // Give the GTU a 1 micrometer long operational plan, or a stand-still plan, so the first move and events will work
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         // register the GTU on the lanes
236         for (DirectedLanePosition directedLanePosition : initialLongitudinalPositions)
237         {
238             Lane lane = directedLanePosition.getLane();
239             addLaneToGtu(lane, directedLanePosition.getPosition(), directedLanePosition.getGtuDirection()); // enter lane part 1
240         }
241 
242         // init event
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         // register the GTU on the lanes
250         for (DirectedLanePosition directedLanePosition : initialLongitudinalPositions)
251         {
252             Lane lane = directedLanePosition.getLane();
253             lane.addGTU(this, directedLanePosition.getPosition()); // enter lane part 2
254         }
255 
256         // initiate the actual move
257         super.init(strategicalPlanner, initialLocation, initialSpeed);
258 
259         this.referencePositionTime = Double.NaN; // remove cache, it may be invalid as the above init results in a lane change
260 
261     }
262 
263     /**
264      * {@inheritDoc} All lanes the GTU is on will be left.
265      */
266     @Override
267     public void setParent(final GTU gtu) throws GTUException
268     {
269         for (Lane lane : new LinkedHashSet<>(this.currentLanes.keySet())) // copy for concurrency problems
270         {
271             leaveLane(lane);
272         }
273         super.setParent(gtu);
274     }
275 
276     /**
277      * Reinitializes the GTU on the network using the existing strategical planner and zero speed.
278      * @param initialLongitudinalPositions Set&lt;DirectedLanePosition&gt;; initial position
279      * @throws NetworkException when the GTU cannot be placed on the given lane
280      * @throws SimRuntimeException when the move method cannot be scheduled
281      * @throws GTUException when initial values are not correct
282      * @throws OTSGeometryException when the initial path is wrong
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      * Hack method. TODO remove and solve better
292      * @return safe to change
293      * @throws GTUException on error
294      */
295     public final boolean isSafeToChange() throws GTUException
296     {
297         return this.fractionalLinkPositions.get(getReferencePosition().getLane().getParentLink()) > 0.0;
298     }
299 
300     /**
301      * insert GTU at a certain position. This can happen at setup (first initialization), and after a lane change of the GTU.
302      * The relative position that will be registered is the referencePosition (dx, dy, dz) = (0, 0, 0). Front and rear positions
303      * are relative towards this position.
304      * @param lane Lane; the lane to add to the list of lanes on which the GTU is registered.
305      * @param gtuDirection GTUDirectionality; the direction of the GTU on the lane (which can be bidirectional). If the GTU has
306      *            a positive speed, it is moving in this direction.
307      * @param position Length; the position on the lane.
308      * @throws GTUException when positioning the GTU on the lane causes a problem
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      * Registers the lane at the GTU. Only works at the start of a operational plan.
323      * @param lane Lane; the lane to add to the list of lanes on which the GTU is registered.
324      * @param gtuDirection GTUDirectionality; the direction of the GTU on the lane (which can be bidirectional). If the GTU has
325      *            a positive speed, it is moving in this direction.
326      * @param position Length; the position on the lane.
327      * @throws GTUException when positioning the GTU on the lane causes a problem
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         // if the GTU is already registered on a lane of the same link, do not change its fractional position, as
339         // this might lead to a "jump".
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      * Part of 'enterLane' which registers the GTU with the lane so the lane can report its GTUs.
349      * @param lane Lane; lane
350      * @param position Length; position
351      * @throws GTUException on exception
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      * Unregister the GTU from a lane.
378      * @param lane Lane; the lane to remove from the list of lanes on which the GTU is registered.
379      * @throws GTUException when leaveLane should not be called
380      */
381     @SuppressWarnings("checkstyle:designforextension")
382     public void leaveLane(final Lane lane) throws GTUException
383     {
384         leaveLane(lane, false);
385     }
386 
387     /**
388      * Leave a lane but do not complain about having no lanes left when beingDestroyed is true.
389      * @param lane Lane; the lane to leave
390      * @param beingDestroyed boolean; if true, no complaints about having no lanes left
391      * @throws GTUException in case leaveLane should not be called
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         // check if there are any lanes for this link left. If not, remove the link.
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      * Removes and cancels events for the given lane.
422      * @param lane Lane; lane
423      * @param triggers Map&lt;Lane, List&lt;SimEventInterface&lt;Duration&gt;&gt;&gt;; map to use
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     /** {@inheritDoc} */
448     @Override
449     public void changeLaneInstantaneously(final LateralDirectionality laneChangeDirection) throws GTUException
450     {
451 
452         // from info
453         DirectedLanePosition from = getReferencePosition();
454 
455         // keep a copy of the lanes and directions (!)
456         Set<Lane> lanesToBeRemoved = new LinkedHashSet<>(this.currentLanes.keySet());
457 
458         // store the new positions
459         // start with current link position, these will be overwritten, except if from a lane no adjacent lane is found, i.e.
460         // changing over a continuous line when probably the reference point is past the line
461         Map<Link, Double> newLinkPositionsLC = new LinkedHashMap<>(this.fractionalLinkPositions);
462 
463         // obtain position on lane adjacent to reference lane and enter lanes upstream/downstream from there
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         // update the positions on the lanes we are registered on
475         this.fractionalLinkPositions.clear();
476         this.fractionalLinkPositions.putAll(newLinkPositionsLC);
477 
478         // leave the from lanes
479         for (Lane lane : lanesToBeRemoved)
480         {
481             leaveLane(lane);
482         }
483 
484         // stored positions no longer valid
485         this.referencePositionTime = Double.NaN;
486         this.cachedPositions.clear();
487 
488         // fire event
489         this.fireTimedEvent(LaneBasedGTU.LANE_CHANGE_EVENT, new Object[] {getId(), laneChangeDirection, from},
490                 getSimulator().getSimulatorTime());
491 
492     }
493 
494     /**
495      * Enters lanes upstream and downstream of the new location after an instantaneous lane change.
496      * @param lane LaneDirection; considered lane
497      * @param position Length; position to add GTU at
498      * @param newLinkPositionsLC Map&lt;Link, Double&gt;; new link fractions to store
499      * @param planLength Length; length of plan, to consider fractions at start
500      * @param lanesToBeRemoved Set&lt;Lane&gt;; lanes to leave, from which lanes are removed when entered (such that they arent
501      *            then left)
502      * @param dir int; below 0 for upstream, above 0 for downstream, 0 for both
503      * @throws GTUException on exception
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         // upstream
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                             // multiple upstream lanes could belong to the same link, we pick an arbitrary lane
538                             // (a conflict should solve this)
539                             upLane = nextUp;
540                             break;
541                         }
542                     }
543                     if (upLane == null)
544                     {
545                         // the rear is on an upstream section we weren't before the lane change, due to curvature, we pick an
546                         // arbitrary lane (a conflict should solve this)
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         // downstream
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      * Register on lanes in target lane.
590      * @param laneChangeDirection LateralDirectionality; direction of lane change
591      * @throws GTUException exception
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      * Performs the finalization of a lane change by leaving the from lanes.
632      * @param laneChangeDirection LateralDirectionality; direction of lane change
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             // find lanes to leave as they have an adjacent lane the GTU is also on in the lane change direction
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             // some lanes registered to the GTU may be downstream of a split and have no adjacent lane, find longitudinally
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                     // if the reference point is in between two lanes, this recognizes the lane upstream of the gap
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             // should not happen, lane was obtained from GTU
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     /** {@inheritDoc} */
722     @Override
723     public void setFinalizeLaneChangeEvent(final SimEventInterface<Duration> event)
724     {
725         this.finalizeLaneChangeEvent = event;
726     }
727 
728     /** {@inheritDoc} */
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     /** {@inheritDoc} */
738     @Override
739     @SuppressWarnings("checkstyle:designforextension")
740     protected boolean move(final DirectedPoint fromLocation)
741             throws SimRuntimeException, GTUException, OperationalPlanException, NetworkException, ParameterException
742     {
743         // DirectedPoint currentPoint = getLocation(); // used for "jump" detection that is also commented out
744         // Only carry out move() if we still have lane(s) to drive on.
745         // Note: a (Sink) trigger can have 'destroyed' us between the previous evaluation step and this one.
746         if (this.currentLanes.isEmpty())
747         {
748             destroy();
749             return false; // Done; do not re-schedule execution of this move method.
750         }
751 
752         // remove enter events
753         // WS: why?
754         // for (Lane lane : this.pendingEnterTriggers.keySet())
755         // {
756         // System.out.println("GTU " + getId() + " is canceling event on lane " + lane.getFullId());
757         // List<SimEventInterface<Duration>> events = this.pendingEnterTriggers.get(lane);
758         // for (SimEventInterface<Duration> event : events)
759         // {
760         // // also unregister from lane
761         // this.currentLanes.remove(lane);
762         // getSimulator().cancelEvent(event);
763         // }
764         // }
765         // this.pendingEnterTriggers.clear();
766 
767         // get distance covered in previous plan, to aid a shift in link fraction (from which a plan moves onwards)
768         Length covered;
769         if (getOperationalPlan() instanceof LaneBasedOperationalPlan
770                 && ((LaneBasedOperationalPlan) getOperationalPlan()).isDeviative())
771         {
772             // traveled distance as difference between start and current position on reference lane
773             // note that for a deviative plan the traveled distance along the path is not valuable here
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             // Note that distance is valid as the reference lane can not change (and location of previous plan is start location
782             // of current plan). Only instantaneous lane changes can do that, which do not result in deviative plans.
783         }
784         else
785         {
786             covered = getOperationalPlan().getTraveledDistance(getSimulator().getSimulatorAbsTime());
787         }
788 
789         // generate the next operational plan and carry it out
790         // in case of an instantaneous lane change, fractionalLinkPositions will be accordingly adjusted to the new lane
791         super.move(fromLocation);
792 
793         // update the positions on the lanes we are registered on
794         // WS: this was previously done using fractions calculated before super.move() based on the GTU position, but an
795         // instantaneous lane change while e.g. the nose is on the next lane which is curved, results in a different fraction on
796         // the next link (the GTU doesn't stretch or shrink)
797         Map<Link, Double> newLinkFractions = new LinkedHashMap<>(this.fractionalLinkPositions);
798         Set<Link> done = new LinkedHashSet<>();
799         // WS: this used to be on all current lanes, skipping links already processed, but 'covered' regards the reference lane
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         // DirectedPoint currentPointAfterMove = getLocation();
827         // if (currentPoint.distance(currentPointAfterMove) > 0.1)
828         // {
829         // System.err.println(this.getId() + " jumped");
830         // }
831         // schedule triggers and determine when to enter lanes with front and leave lanes with rear
832         scheduleEnterLeaveTriggers();
833         return false;
834     }
835 
836     /**
837      * Recursive update of link fractions based on a moved distance.
838      * @param lane Lane; current lane, start with reference lane
839      * @param newLinkFractions Map&lt;Link, Double&gt;; map to put new fractions in
840      * @param done Set&lt;Link&gt;; links to skip as link are already done
841      * @param prevs boolean; whether to loop to the previous or next lanes, regardless of driving direction
842      * @param covered Length; covered distance along the reference lane
843      * @param isReferenceLane boolean; whether this lane is the reference lane (to skip in second call)
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                 // can not happen as we check that the lane is in the currentLanes
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     /** {@inheritDoc} */
875     @Override
876     public final Map<Lane, Length> positions(final RelativePosition relativePosition) throws GTUException
877     {
878         return positions(relativePosition, getSimulator().getSimulatorAbsTime());
879     }
880 
881     /** {@inheritDoc} */
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     /** {@inheritDoc} */
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      * Return the longitudinal position that the indicated relative position of this GTU would have if it were to change to
902      * another Lane with a / the current CrossSectionLink. This point may be before the begin or after the end of the link of
903      * the projection lane of the GTU. This preserves the length of the GTU.
904      * @param projectionLane Lane; the lane onto which the position of this GTU must be projected
905      * @param relativePosition RelativePosition; the point on this GTU that must be projected
906      * @param when Time; the time for which to project the position of this GTU
907      * @return Length; the position of this GTU in the projectionLane
908      * @throws GTUException when projectionLane it not in any of the CrossSectionLink that the GTU is on
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      * Return the longitudinal position on the projection lane that has the same fractional position on one of the current lanes
937      * of the indicated relative position. This preserves the fractional positions of all relative positions of the GTU.
938      * @param projectionLane Lane; the lane onto which the position of this GTU must be projected
939      * @param relativePosition RelativePosition; the point on this GTU that must be projected
940      * @param when Time; the time for which to project the position of this GTU
941      * @return Length; the position of this GTU in the projectionLane
942      * @throws GTUException when projectionLane it not in any of the CrossSectionLink that the GTU is on
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     /** caching of time field for last stored position(s). */
965     private double cachePositionsTime = Double.NaN;
966 
967     /** caching of last stored position(s). */
968     private Map<Integer, Length> cachedPositions = new LinkedHashMap<>();
969 
970     /** {@inheritDoc} */
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                 // PK verify the result; uncomment if you don't trust the cache
983                 // this.cachedPositions.clear();
984                 // Length difficultWay = position(lane, relativePosition, when);
985                 // if (Math.abs(l.si - difficultWay.si) > 0.00001)
986                 // {
987                 // System.err.println("Whoops: cache returns bad value for GTU " + getId());
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                     // deviative LaneBasedOperationalPlan, i.e. the GTU is not on a center line
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                         // the point does not project fractionally to this lane, it has to be up- or downstream of the lane
1039 
1040                         // simple heuristic to decide if we first look upstream or downstream
1041                         boolean upstream = this.fractionalLinkPositions.get(lane.getParentLink()) < 0.0 ? true : false;
1042 
1043                         // use loop up to 2 times (for loop creates 'loc not initialized' warning)
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                             // distance can be positive on an upstream lane due to a loop
1051                             if (!Double.isNaN(distance))
1052                             {
1053                                 if (i == 1 && !Double.isNaN(loc))
1054                                 {
1055                                     // loc was determined in both loops, this constitutes a lane-loop, select nearest
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                                     // loc was determined in second loop
1065                                     loc = upstream ? -distance : distance + lane.getLength().si;
1066                                 }
1067                             }
1068                             else if (!Double.isNaN(loc))
1069                             {
1070                                 // loc was determined in first loop
1071                                 break;
1072                             }
1073                             else if (i == 1)
1074                             {
1075                                 // loc was determined in neither loop
1076                                 // Lane change ended while moving to next link. The source lanes are left and for a leave-lane
1077                                 // event the position is required. This may depend on upstream or downstream lanes as the
1078                                 // reference position is projected to that lane. But if we already left that lane, we can't use
1079                                 // it. We thus use ENDPOINT fallback instead.
1080                                 loc = lane.getLength().si * lane.getCenterLine().projectFractional(null, null, p.x, p.y,
1081                                         FractionalFallback.ENDPOINT);
1082                                 break;
1083                             }
1084                             // try other direction
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     /** Set of lane to attempt when determining the location with a deviative lane change. */
1116     // private Set<Lane> otherLanesToConsider;
1117 
1118     /**
1119      * In case of a deviative operational plan (not on the center lines), positions are projected fractionally to the center
1120      * lines. For points upstream or downstream of a lane, fractional projection is not valid. In such cases we need to project
1121      * the position to an upstream or downstream lane instead, and adjust length along the center lines.
1122      * @param lane Lane; lane to determine the position on
1123      * @param when Time; time
1124      * @param upstream boolean; whether to check upstream (or downstream)
1125      * @param distance double; cumulative distance in recursive search, starts at 0.0
1126      * @param point DirectedPoint; absolute point of GTU to be projected to center line
1127      * @param otherLanesToConsider Set&lt;Lane&gt;; lanes to consider
1128      * @return Length; position on lane being &lt;0 or &gt;{@code lane.getLength()}
1129      * @throws GTUException if GTU is not on the lane
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()); // safe copy
1136         nextLanes.retainAll(otherLanesToConsider); // as we delete here
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         // TODO When requesting the position at the end of the plan, which will be on a further lane, this lane is not yet
1151         // part of the lanes in the current operational plan. This can be upstream or downstream depending on the direction of
1152         // travel. We might check whether getDirection(lane)=DIR_PLUS and upstream=false, or getDirection(lane)=DIR_MINUS and
1153         // upstream=true, to then use LaneDirection.getNextLaneDirection(this) to obtain the next lane. This is only required if
1154         // nextLanes originally had more than 1 lane, otherwise we can simply use that one lane. Problem is that the search
1155         // might go on far or even eternally (on a circular network), as projection simply keeps failing because the GTU is
1156         // actually towards the other longitudinal direction. Hence, the heuristic used before this method is called should
1157         // change and first always search against the direction of travel, and only consider lanes in currentLanes, while the
1158         // consecutive search in the direction of travel should then always find a point. We could build in a counter to prevent
1159         // a hanging software.
1160         if (nextLanes.size() == 0)
1161         {
1162             return Double.NaN; // point must be in the other direction
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     /** Time of reference position cache. */
1178     private double referencePositionTime = Double.NaN;
1179 
1180     /** Cached reference position. */
1181     private DirectedLanePosition cachedReferencePosition = null;
1182 
1183     /** {@inheritDoc} */
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                 // TODO widest lane in case we are registered on more than one lane with the reference point?
1202                 // TODO lane that leads to our location or not if we are registered on parallel lanes?
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))// && getOperationalPlan() instanceof LaneBasedOperationalPlan
1226             // && ((LaneBasedOperationalPlan) getOperationalPlan()).isDeviative())
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         // for (Lane lane : this.currentLanes.keySet())
1245         // {
1246         // Length relativePosition = position(lane, RelativePosition.REFERENCE_POSITION);
1247         // System.err
1248         // .println(String.format("Lane %s of Link %s: absolute position %s, relative position %5.1f%%", lane.getId(),
1249         // lane.getParentLink().getId(), relativePosition, relativePosition.si * 100 / lane.getLength().si));
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      * Schedule the triggers for this GTU that are going to happen until the next evaluation time. Also schedule the
1256      * registration and deregistration of lanes when the vehicle enters or leaves them, at the exact right time. <br>
1257      * Note: when the GTU makes a lane change, the vehicle will be registered for both lanes during the entire maneuver.
1258      * @throws NetworkException on network inconsistency
1259      * @throws SimRuntimeException should never happen
1260      * @throws GTUException when a branch is reached where the GTU does not know where to go next
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         // Figure out which lanes this GTU will enter with its FRONT, and schedule the lane enter events
1279         Map<Lane, GTUDirectionality> lanesCopy = new LinkedHashMap<>(this.currentLanes);
1280         Iterator<Lane> it = lanesCopy.keySet().iterator();
1281         Lane enteredLane = null;
1282         // LateralDirectionality forceSide = LateralDirectionality.NONE;
1283         while (it.hasNext() || enteredLane != null) // use a copy because this.currentLanes can change
1284         {
1285             // next lane from 'lanesCopy', or asses the lane we just entered as it may be very short and also passed fully
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             // skip if already on next lane
1302             if (!Collections.disjoint(this.currentLanes.keySet(),
1303                     lane.downstreamLanes(laneDir, getGTUType()).keySet().toCollection()))
1304             {
1305                 continue;
1306             }
1307 
1308             // schedule triggers on this lane
1309             double referenceStartSI = this.fractionalLinkPositions.get(lane.getParentLink()) * lane.getLength().getSI();
1310             // referenceStartSI is position of reference of GTU on current lane
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                     // A sink should delete the GTU, or a lane change should end, before reaching the end of the lane
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                     // GTU front is more than lane length, so end location can not be extracted from the lane, let's move then
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                         // TODO: this escape was in timeAtPoint, where it was changed to return null for leave lane events
1369                         enterTime = Time.instantiateSI(getOperationalPlan().getEndTime().si - 1e-9);
1370                         // -1e-9 prevents that next move() reschedules enter
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         // Figure out which lanes this GTU will exit with its BACK, and schedule the lane exit events
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                         // code below will leave entered lanes if exitTime is null, make this so if NaN results due to the lane
1447                         // end being beyond the plan (rather than the GTU never having been there, but being registered there
1448                         // upon lane change initiation)
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                 // This lane was entered when initiating the lane change due to a fractional calculation. Now, the deviative
1469                 // plan indicates we will never reach this location.
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     /** {@inheritDoc} */
1481     @Override
1482     public final Map<Lane, Double> fractionalPositions(final RelativePosition relativePosition) throws GTUException
1483     {
1484         return fractionalPositions(relativePosition, getSimulator().getSimulatorAbsTime());
1485     }
1486 
1487     /** {@inheritDoc} */
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     /** {@inheritDoc} */
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     /** {@inheritDoc} */
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     /** {@inheritDoc} */
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      * Add enter trigger.
1530      * @param lane Lane; lane
1531      * @param event SimEventInterface&lt;Duration&gt;; event
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      * Sets a vehicle model.
1546      * @param vehicleModel VehicleModel; vehicle model
1547      */
1548     public void setVehicleModel(final VehicleModel vehicleModel)
1549     {
1550         this.vehicleModel = vehicleModel;
1551     }
1552 
1553     /** {@inheritDoc} */
1554     @Override
1555     public VehicleModel getVehicleModel()
1556     {
1557         return this.vehicleModel;
1558     }
1559 
1560     /** {@inheritDoc} */
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             // ignore. not important at destroy
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()); // Operate on a copy of the key
1579                                                                                  // set
1580             for (Lane lane : laneSet)
1581             {
1582                 try
1583                 {
1584                     leaveLane(lane, true);
1585                 }
1586                 catch (GTUException e)
1587                 {
1588                     // ignore. not important at destroy
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     /** {@inheritDoc} */
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     /** {@inheritDoc} */
1624     @Override
1625     public final LaneBasedStrategicalPlanner getStrategicalPlanner()
1626     {
1627         return (LaneBasedStrategicalPlanner) super.getStrategicalPlanner();
1628     }
1629 
1630     /** {@inheritDoc} */
1631     @Override
1632     public final LaneBasedStrategicalPlanner getStrategicalPlanner(final Time time)
1633     {
1634         return (LaneBasedStrategicalPlanner) super.getStrategicalPlanner(time);
1635     }
1636 
1637     /** {@inheritDoc} */
1638     @Override
1639     public RoadNetwork getNetwork()
1640     {
1641         return (RoadNetwork) super.getPerceivableContext();
1642     }
1643 
1644     /** {@inheritDoc} */
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                 // Throw.whenNull(infra, "InfrastructurePerception is required to determine the desired speed.");
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     /** {@inheritDoc} */
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             // speed
1681             EgoPerception<?, ?> ego = perception.getPerceptionCategoryOrNull(EgoPerception.class);
1682             Throw.whenNull(ego, "EgoPerception is required to determine the speed.");
1683             Speed speed = ego.getSpeed();
1684             // speed limit info
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             // leaders
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             // obtain
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     /** {@inheritDoc} */
1702     @Override
1703     public final TurnIndicatorStatus getTurnIndicatorStatus()
1704     {
1705         return this.turnIndicatorStatus.get();
1706     }
1707 
1708     /** {@inheritDoc} */
1709     @Override
1710     public final TurnIndicatorStatus getTurnIndicatorStatus(final Time time)
1711     {
1712         return this.turnIndicatorStatus.get(time);
1713     }
1714 
1715     /** {@inheritDoc} */
1716     @Override
1717     public final void setTurnIndicatorStatus(final TurnIndicatorStatus turnIndicatorStatus)
1718     {
1719         this.turnIndicatorStatus.set(turnIndicatorStatus);
1720     }
1721 
1722     /** {@inheritDoc} */
1723     @Override
1724     @SuppressWarnings("checkstyle:designforextension")
1725     public String toString()
1726     {
1727         return String.format("GTU " + getId());
1728     }
1729 
1730 }