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 javax.media.j3d.Bounds;
13  import javax.vecmath.Point3d;
14  
15  import org.djunits.unit.DirectionUnit;
16  import org.djunits.unit.DurationUnit;
17  import org.djunits.unit.LengthUnit;
18  import org.djunits.unit.PositionUnit;
19  import org.djunits.value.vdouble.scalar.Acceleration;
20  import org.djunits.value.vdouble.scalar.Duration;
21  import org.djunits.value.vdouble.scalar.Length;
22  import org.djunits.value.vdouble.scalar.Speed;
23  import org.djunits.value.vdouble.scalar.Time;
24  import org.djutils.exceptions.Throw;
25  import org.djutils.exceptions.Try;
26  import org.djutils.immutablecollections.ImmutableMap;
27  import org.opentrafficsim.base.parameters.ParameterException;
28  import org.opentrafficsim.core.dsol.OTSSimulatorInterface;
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  import nl.tudelft.simulation.dsol.simtime.SimTimeDoubleUnit;
75  import nl.tudelft.simulation.language.d3.BoundingBox;
76  import nl.tudelft.simulation.language.d3.DirectedPoint;
77  
78  /**
79   * This class contains most of the code that is needed to run a lane based GTU. <br>
80   * 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
81   * 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
82   * 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
83   * dozens of Lanes at the same time.
84   * <p>
85   * When calculating a headway, the GTU has to look in successive lanes. When Lanes (or underlying CrossSectionLinks) diverge,
86   * the headway algorithms have to look at multiple Lanes and return the minimum headway in each of the Lanes. When the Lanes (or
87   * underlying CrossSectionLinks) converge, "parallel" traffic is not taken into account in the headway calculation. Instead, gap
88   * acceptance algorithms or their equivalent should guide the merging behavior.
89   * <p>
90   * To decide its movement, an AbstractLaneBasedGTU applies its car following algorithm and lane change algorithm to set the
91   * acceleration and any lane change operation to perform. It then schedules the triggers that will add it to subsequent lanes
92   * and remove it from current lanes as needed during the time step that is has committed to. Finally, it re-schedules its next
93   * movement evaluation with the simulator.
94   * <p>
95   * Copyright (c) 2013-2020 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
96   * BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
97   * <p>
98   * @version $Revision: 1408 $, $LastChangedDate: 2015-09-24 15:17:25 +0200 (Thu, 24 Sep 2015) $, by $Author: pknoppers $,
99   *          initial version Oct 22, 2014 <br>
100  * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
101  * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
102  */
103 public abstract class AbstractLaneBasedGTU extends AbstractGTU implements LaneBasedGTU
104 {
105     /** */
106     private static final long serialVersionUID = 20140822L;
107 
108     /**
109      * Fractional longitudinal positions of the reference point of the GTU on one or more links at the start of the current
110      * operational plan. Because the reference point of the GTU might not be on all the links the GTU is registered on, the
111      * fractional longitudinal positions can be more than one, or less than zero.
112      */
113     private HistoricalMap<Link, Double> fractionalLinkPositions;
114 
115     /**
116      * The lanes the GTU is registered on. Each lane has to have its link registered in the fractionalLinkPositions as well to
117      * keep consistency. Each link from the fractionalLinkPositions can have one or more Lanes on which the vehicle is
118      * registered. This is a list to improve reproducibility: The 'oldest' lanes on which the vehicle is registered are at the
119      * front of the list, the later ones more to the back.
120      */
121     private final HistoricalMap<Lane, GTUDirectionality> currentLanes;
122 
123     /** Maps that we enter when initiating a lane change, but we may not actually enter given a deviative plan. */
124     private final Set<Lane> enteredLanes = new LinkedHashSet<>();
125 
126     /** Pending leave triggers for each lane. */
127     private Map<Lane, List<SimEventInterface<SimTimeDoubleUnit>>> pendingLeaveTriggers = new LinkedHashMap<>();
128 
129     /** Pending enter triggers for each lane. */
130     private Map<Lane, List<SimEventInterface<SimTimeDoubleUnit>>> pendingEnterTriggers = new LinkedHashMap<>();
131 
132     /** Event to finalize lane change. */
133     private SimEventInterface<SimTimeDoubleUnit> finalizeLaneChangeEvent = null;
134 
135     /** Cached desired speed. */
136     private Speed cachedDesiredSpeed;
137 
138     /** Time desired speed was cached. */
139     private Time desiredSpeedTime;
140 
141     /** Cached car-following acceleration. */
142     private Acceleration cachedCarFollowingAcceleration;
143 
144     /** Time car-following acceleration was cached. */
145     private Time carFollowingAccelerationTime;
146 
147     /** The object to lock to make the GTU thread safe. */
148     private Object lock = new Object();
149 
150     /** The threshold distance for differences between initial locations of the GTU on different lanes. */
151     @SuppressWarnings("checkstyle:visibilitymodifier")
152     public static Length initialLocationThresholdDifference = new Length(1.0, LengthUnit.MILLIMETER);
153 
154     /** Turn indicator status. */
155     private final Historical<TurnIndicatorStatus> turnIndicatorStatus;
156 
157     /** Caching on or off. */
158     // TODO: should be indicated with a Parameter
159     public static boolean CACHING = true;
160 
161     /** cached position count. */
162     // TODO: can be removed after testing period
163     public static int CACHED_POSITION = 0;
164 
165     /** cached position count. */
166     // TODO: can be removed after testing period
167     public static int NON_CACHED_POSITION = 0;
168 
169     /** Vehicle model. */
170     private VehicleModel vehicleModel = VehicleModel.MINMAX;
171 
172     /**
173      * Construct a Lane Based GTU.
174      * @param id String; the id of the GTU
175      * @param gtuType GTUType; the type of GTU, e.g. TruckType, CarType, BusType
176      * @param network OTSRoadNetwork; the network that the GTU is initially registered in
177      * @throws GTUException when initial values are not correct
178      */
179     public AbstractLaneBasedGTU(final String id, final GTUType gtuType, final OTSRoadNetwork network) throws GTUException
180     {
181         super(id, gtuType, network.getSimulator(), network);
182         OTSSimulatorInterface simulator = network.getSimulator();
183         HistoryManager historyManager = simulator.getReplication().getHistoryManager(simulator);
184         this.fractionalLinkPositions = new HistoricalLinkedHashMap<>(historyManager);
185         this.currentLanes = new HistoricalLinkedHashMap<>(historyManager);
186         this.turnIndicatorStatus = new HistoricalValue<>(historyManager, TurnIndicatorStatus.NOTPRESENT);
187     }
188 
189     /**
190      * @param strategicalPlanner LaneBasedStrategicalPlanner; the strategical planner (e.g., route determination) to use
191      * @param initialLongitudinalPositions Set&lt;DirectedLanePosition&gt;; the initial positions of the car on one or more
192      *            lanes with their directions
193      * @param initialSpeed Speed; the initial speed of the car on the lane
194      * @throws NetworkException when the GTU cannot be placed on the given lane
195      * @throws SimRuntimeException when the move method cannot be scheduled
196      * @throws GTUException when initial values are not correct
197      * @throws OTSGeometryException when the initial path is wrong
198      */
199     @SuppressWarnings("checkstyle:designforextension")
200     public void init(final LaneBasedStrategicalPlanner strategicalPlanner,
201             final Set<DirectedLanePosition> initialLongitudinalPositions, final Speed initialSpeed)
202             throws NetworkException, SimRuntimeException, GTUException, OTSGeometryException
203     {
204         Throw.when(null == initialLongitudinalPositions, GTUException.class, "InitialLongitudinalPositions is null");
205         Throw.when(0 == initialLongitudinalPositions.size(), GTUException.class, "InitialLongitudinalPositions is empty set");
206 
207         DirectedPoint lastPoint = null;
208         for (DirectedLanePosition pos : initialLongitudinalPositions)
209         {
210             // Throw.when(lastPoint != null && pos.getLocation().distance(lastPoint) > initialLocationThresholdDifference.si,
211             // GTUException.class, "initial locations for GTU have distance > " + initialLocationThresholdDifference);
212             lastPoint = pos.getLocation();
213         }
214         DirectedPoint initialLocation = lastPoint;
215 
216         // Give the GTU a 1 micrometer long operational plan, or a stand-still plan, so the first move and events will work
217         Time now = getSimulator().getSimulatorTime();
218         try
219         {
220             if (initialSpeed.si < OperationalPlan.DRIFTING_SPEED_SI)
221             {
222                 this.operationalPlan
223                         .set(new OperationalPlan(this, initialLocation, now, new Duration(1E-6, DurationUnit.SECOND)));
224             }
225             else
226             {
227                 OTSPoint3D p2 = new OTSPoint3D(initialLocation.x + 1E-6 * Math.cos(initialLocation.getRotZ()),
228                         initialLocation.y + 1E-6 * Math.sin(initialLocation.getRotZ()), initialLocation.z);
229                 OTSLine3D path = new OTSLine3D(new OTSPoint3D(initialLocation), p2);
230                 this.operationalPlan.set(OperationalPlanBuilder.buildConstantSpeedPlan(this, path, now, initialSpeed));
231             }
232         }
233         catch (OperationalPlanException e)
234         {
235             throw new RuntimeException("Initial operational plan could not be created.", e);
236         }
237 
238         // register the GTU on the lanes
239         for (DirectedLanePosition directedLanePosition : initialLongitudinalPositions)
240         {
241             Lane lane = directedLanePosition.getLane();
242             addLaneToGtu(lane, directedLanePosition.getPosition(), directedLanePosition.getGtuDirection()); // enter lane part 1
243         }
244 
245         // init event
246         DirectedLanePosition referencePosition = getReferencePosition();
247         fireTimedEvent(LaneBasedGTU.LANEBASED_INIT_EVENT,
248                 new Object[] { getId(), initialLocation, getLength(), getWidth(), referencePosition.getLane(),
249                         referencePosition.getPosition(), referencePosition.getGtuDirection(), getGTUType() },
250                 getSimulator().getSimulatorTime());
251 
252         // register the GTU on the lanes
253         for (DirectedLanePosition directedLanePosition : initialLongitudinalPositions)
254         {
255             Lane lane = directedLanePosition.getLane();
256             lane.addGTU(this, directedLanePosition.getPosition()); // enter lane part 2
257         }
258 
259         // initiate the actual move
260         super.init(strategicalPlanner, initialLocation, initialSpeed);
261 
262         this.referencePositionTime = Double.NaN; // remove cache, it may be invalid as the above init results in a lane change
263 
264     }
265 
266     /**
267      * {@inheritDoc} All lanes the GTU is on will be left.
268      */
269     @Override
270     public void setParent(final GTU gtu) throws GTUException
271     {
272         for (Lane lane : new LinkedHashSet<>(this.currentLanes.keySet())) // copy for concurrency problems
273         {
274             leaveLane(lane);
275         }
276         super.setParent(gtu);
277     }
278 
279     /**
280      * Reinitializes the GTU on the network using the existing strategical planner and zero speed.
281      * @param initialLongitudinalPositions Set&lt;DirectedLanePosition&gt;; initial position
282      * @throws NetworkException when the GTU cannot be placed on the given lane
283      * @throws SimRuntimeException when the move method cannot be scheduled
284      * @throws GTUException when initial values are not correct
285      * @throws OTSGeometryException when the initial path is wrong
286      */
287     public void reinit(final Set<DirectedLanePosition> initialLongitudinalPositions)
288             throws NetworkException, SimRuntimeException, GTUException, OTSGeometryException
289     {
290         init(getStrategicalPlanner(), initialLongitudinalPositions, Speed.ZERO);
291     }
292 
293     /**
294      * Hack method. TODO remove and solve better
295      * @return safe to change
296      * @throws GTUException on error
297      */
298     public final boolean isSafeToChange() throws GTUException
299     {
300         return this.fractionalLinkPositions.get(getReferencePosition().getLane().getParentLink()) > 0.0;
301     }
302 
303     /**
304      * insert GTU at a certain position. This can happen at setup (first initialization), and after a lane change of the GTU.
305      * The relative position that will be registered is the referencePosition (dx, dy, dz) = (0, 0, 0). Front and rear positions
306      * are relative towards this position.
307      * @param lane Lane; the lane to add to the list of lanes on which the GTU is registered.
308      * @param gtuDirection GTUDirectionality; the direction of the GTU on the lane (which can be bidirectional). If the GTU has
309      *            a positive speed, it is moving in this direction.
310      * @param position Length; the position on the lane.
311      * @throws GTUException when positioning the GTU on the lane causes a problem
312      */
313     @SuppressWarnings("checkstyle:designforextension")
314     public void enterLane(final Lane lane, final Length position, final GTUDirectionality gtuDirection) throws GTUException
315     {
316         if (lane == null || gtuDirection == null || position == null)
317         {
318             throw new GTUException("enterLane - one of the arguments is null");
319         }
320         addLaneToGtu(lane, position, gtuDirection);
321         addGtuToLane(lane, position);
322     }
323 
324     /**
325      * Registers the lane at the GTU. Only works at the start of a operational plan.
326      * @param lane Lane; the lane to add to the list of lanes on which the GTU is registered.
327      * @param gtuDirection GTUDirectionality; the direction of the GTU on the lane (which can be bidirectional). If the GTU has
328      *            a positive speed, it is moving in this direction.
329      * @param position Length; the position on the lane.
330      * @throws GTUException when positioning the GTU on the lane causes a problem
331      */
332     private void addLaneToGtu(final Lane lane, final Length position, final GTUDirectionality gtuDirection) throws GTUException
333     {
334         if (this.currentLanes.containsKey(lane))
335         {
336             System.err.println(this + " is already registered on lane: " + lane + " at fractional position "
337                     + this.fractionalPosition(lane, RelativePosition.REFERENCE_POSITION) + " intended position is " + position
338                     + " length of lane is " + lane.getLength());
339             return;
340         }
341         // if the GTU is already registered on a lane of the same link, do not change its fractional position, as
342         // this might lead to a "jump".
343         if (!this.fractionalLinkPositions.containsKey(lane.getParentLink()))
344         {
345             this.fractionalLinkPositions.put(lane.getParentLink(), lane.fraction(position));
346         }
347         this.currentLanes.put(lane, gtuDirection);
348     }
349 
350     /**
351      * Part of 'enterLane' which registers the GTU with the lane so the lane can report its GTUs.
352      * @param lane Lane; lane
353      * @param position Length; position
354      * @throws GTUException on exception
355      */
356     protected void addGtuToLane(final Lane lane, final Length position) throws GTUException
357     {
358         List<SimEventInterface<SimTimeDoubleUnit>> pending = this.pendingEnterTriggers.get(lane);
359         if (null != pending)
360         {
361             for (SimEventInterface<SimTimeDoubleUnit> event : pending)
362             {
363                 if (event.getAbsoluteExecutionTime().get().ge(getSimulator().getSimulatorTime()))
364                 {
365                     boolean result = getSimulator().cancelEvent(event);
366                     if (!result && event.getAbsoluteExecutionTime().get().ne(getSimulator().getSimulatorTime()))
367                     {
368                         System.err.println("addLaneToGtu, trying to remove event: NOTHING REMOVED -- result=" + result
369                                 + ", simTime=" + getSimulator().getSimulatorTime() + ", eventTime="
370                                 + event.getAbsoluteExecutionTime().get());
371                     }
372                 }
373             }
374             this.pendingEnterTriggers.remove(lane);
375         }
376         lane.addGTU(this, position);
377     }
378 
379     /**
380      * Unregister the GTU from a lane.
381      * @param lane Lane; the lane to remove from the list of lanes on which the GTU is registered.
382      * @throws GTUException when leaveLane should not be called
383      */
384     @SuppressWarnings("checkstyle:designforextension")
385     public void leaveLane(final Lane lane) throws GTUException
386     {
387         leaveLane(lane, false);
388     }
389 
390     /**
391      * Leave a lane but do not complain about having no lanes left when beingDestroyed is true.
392      * @param lane Lane; the lane to leave
393      * @param beingDestroyed boolean; if true, no complaints about having no lanes left
394      * @throws GTUException in case leaveLane should not be called
395      */
396     @SuppressWarnings("checkstyle:designforextension")
397     public void leaveLane(final Lane lane, final boolean beingDestroyed) throws GTUException
398     {
399         Length position = position(lane, getReference());
400         this.currentLanes.remove(lane);
401         removePendingEvents(lane, this.pendingLeaveTriggers);
402         removePendingEvents(lane, this.pendingEnterTriggers);
403         // check if there are any lanes for this link left. If not, remove the link.
404         boolean found = false;
405         for (Lane l : this.currentLanes.keySet())
406         {
407             if (l.getParentLink().equals(lane.getParentLink()))
408             {
409                 found = true;
410             }
411         }
412         if (!found)
413         {
414             this.fractionalLinkPositions.remove(lane.getParentLink());
415         }
416         lane.removeGTU(this, !found, position);
417         if (this.currentLanes.size() == 0 && !beingDestroyed)
418         {
419             System.err.println("leaveLane: lanes.size() = 0 for GTU " + getId());
420         }
421     }
422 
423     /**
424      * Removes and cancels events for the given lane.
425      * @param lane Lane; lane
426      * @param triggers Map&lt;Lane, List&lt;SimEventInterface&lt;SimTimeDoubleUnit&gt;&gt;&gt;; map to use
427      */
428     private void removePendingEvents(final Lane lane, final Map<Lane, List<SimEventInterface<SimTimeDoubleUnit>>> triggers)
429     {
430         List<SimEventInterface<SimTimeDoubleUnit>> pending = triggers.get(lane);
431         if (null != pending)
432         {
433             for (SimEventInterface<SimTimeDoubleUnit> event : pending)
434             {
435                 if (event.getAbsoluteExecutionTime().get().ge(getSimulator().getSimulatorTime()))
436                 {
437                     boolean result = getSimulator().cancelEvent(event);
438                     if (!result && event.getAbsoluteExecutionTime().get().ne(getSimulator().getSimulatorTime()))
439                     {
440                         System.err.println("leaveLane, trying to remove event: NOTHING REMOVED -- result=" + result
441                                 + ", simTime=" + getSimulator().getSimulatorTime() + ", eventTime="
442                                 + event.getAbsoluteExecutionTime().get());
443                     }
444                 }
445             }
446             triggers.remove(lane);
447         }
448     }
449 
450     /** {@inheritDoc} */
451     @Override
452     public void changeLaneInstantaneously(final LateralDirectionality laneChangeDirection) throws GTUException
453     {
454 
455         // from info
456         DirectedLanePosition from = getReferencePosition();
457 
458         // keep a copy of the lanes and directions (!)
459         Set<Lane> lanesToBeRemoved = new LinkedHashSet<>(this.currentLanes.keySet());
460 
461         // store the new positions
462         // start with current link position, these will be overwritten, except if from a lane no adjacent lane is found, i.e.
463         // changing over a continuous line when probably the reference point is past the line
464         Map<Link, Double> newLinkPositionsLC = new LinkedHashMap<>(this.fractionalLinkPositions);
465 
466         // obtain position on lane adjacent to reference lane and enter lanes upstream/downstream from there
467         Set<Lane> adjLanes = from.getLane().accessibleAdjacentLanesPhysical(laneChangeDirection, getGTUType(),
468                 this.currentLanes.get(from.getLane()));
469         Lane adjLane = adjLanes.iterator().next();
470         Length position = adjLane.position(from.getLane().fraction(from.getPosition()));
471         GTUDirectionality direction = getDirection(from.getLane());
472         Length planLength = Try.assign(() -> getOperationalPlan().getTraveledDistance(getSimulator().getSimulatorTime()),
473                 "Exception while determining plan length.");
474         enterLaneRecursive(new LaneDirection(adjLane, direction), position, newLinkPositionsLC, planLength, lanesToBeRemoved,
475                 0);
476 
477         // update the positions on the lanes we are registered on
478         this.fractionalLinkPositions.clear();
479         this.fractionalLinkPositions.putAll(newLinkPositionsLC);
480 
481         // leave the from lanes
482         for (Lane lane : lanesToBeRemoved)
483         {
484             leaveLane(lane);
485         }
486 
487         // stored positions no longer valid
488         this.referencePositionTime = Double.NaN;
489         this.cachedPositions.clear();
490 
491         // fire event
492         this.fireTimedEvent(LaneBasedGTU.LANE_CHANGE_EVENT, new Object[] { getId(), laneChangeDirection, from },
493                 getSimulator().getSimulatorTime());
494 
495     }
496 
497     /**
498      * Enters lanes upstream and downstream of the new location after an instantaneous lane change.
499      * @param lane LaneDirection; considered lane
500      * @param position Length; position to add GTU at
501      * @param newLinkPositionsLC Map&lt;Link, Double&gt;; new link fractions to store
502      * @param planLength Length; length of plan, to consider fractions at start
503      * @param lanesToBeRemoved Set&lt;Lane&gt;; lanes to leave, from which lanes are removed when entered (such that they arent
504      *            then left)
505      * @param dir int; below 0 for upstream, above 0 for downstream, 0 for both
506      * @throws GTUException on exception
507      */
508     private void enterLaneRecursive(final LaneDirection lane, final Length position, final Map<Link, Double> newLinkPositionsLC,
509             final Length planLength, final Set<Lane> lanesToBeRemoved, final int dir) throws GTUException
510     {
511         enterLane(lane.getLane(), position, lane.getDirection());
512         lanesToBeRemoved.remove(lane);
513         Length adjusted = lane.getDirection().isPlus() ? position.minus(planLength) : position.plus(planLength);
514         newLinkPositionsLC.put(lane.getLane().getParentLink(), adjusted.si / lane.getLength().si);
515 
516         // upstream
517         if (dir < 1)
518         {
519             Length rear = lane.getDirection().isPlus() ? position.plus(getRear().getDx()) : position.minus(getRear().getDx());
520             Length before = null;
521             if (lane.getDirection().isPlus() && rear.si < 0.0)
522             {
523                 before = rear.neg();
524             }
525             else if (lane.getDirection().isMinus() && rear.si > lane.getLength().si)
526             {
527                 before = rear.minus(lane.getLength());
528             }
529             if (before != null)
530             {
531                 GTUDirectionality upDir = lane.getDirection();
532                 ImmutableMap<Lane, GTUDirectionality> upstream = lane.getLane().upstreamLanes(upDir, getGTUType());
533                 if (!upstream.isEmpty())
534                 {
535                     Lane upLane = null;
536                     for (Lane nextUp : upstream.keySet())
537                     {
538                         if (newLinkPositionsLC.containsKey(nextUp.getParentLink()))
539                         {
540                             // multiple upstream lanes could belong to the same link, we pick an arbitrary lane
541                             // (a conflict should solve this)
542                             upLane = nextUp;
543                             break;
544                         }
545                     }
546                     if (upLane == null)
547                     {
548                         // the rear is on an upstream section we weren't before the lane change, due to curvature, we pick an
549                         // arbitrary lane (a conflict should solve this)
550                         upLane = upstream.keySet().iterator().next();
551                     }
552                     if (!this.currentLanes.containsKey(upLane))
553                     {
554                         upDir = upstream.get(upLane);
555                         LaneDirectionk/lane/LaneDirection.html#LaneDirection">LaneDirection next = new LaneDirection(upLane, upDir);
556                         Length nextPos = upDir.isPlus() ? next.getLength().minus(before).minus(getRear().getDx())
557                                 : before.plus(getRear().getDx());
558                         enterLaneRecursive(next, nextPos, newLinkPositionsLC, planLength, lanesToBeRemoved, -1);
559                     }
560                 }
561             }
562         }
563 
564         // downstream
565         if (dir > -1)
566         {
567             Length front =
568                     lane.getDirection().isPlus() ? position.plus(getFront().getDx()) : position.minus(getFront().getDx());
569             Length passed = null;
570             if (lane.getDirection().isPlus() && front.si > lane.getLength().si)
571             {
572                 passed = front.minus(lane.getLength());
573             }
574             else if (lane.getDirection().isMinus() && front.si < 0.0)
575             {
576                 passed = front.neg();
577             }
578             if (passed != null)
579             {
580                 LaneDirection next = lane.getNextLaneDirection(this);
581                 if (!this.currentLanes.containsKey(next.getLane()))
582                 {
583                     Length nextPos = next.getDirection().isPlus() ? passed.minus(getFront().getDx())
584                             : next.getLength().minus(passed).plus(getFront().getDx());
585                     enterLaneRecursive(next, nextPos, newLinkPositionsLC, planLength, lanesToBeRemoved, 1);
586                 }
587             }
588         }
589     }
590 
591     /**
592      * Register on lanes in target lane.
593      * @param laneChangeDirection LateralDirectionality; direction of lane change
594      * @throws GTUException exception
595      */
596     @Override
597     @SuppressWarnings("checkstyle:designforextension")
598     public void initLaneChange(final LateralDirectionality laneChangeDirection) throws GTUException
599     {
600         Map<Lane, GTUDirectionality> lanesCopy = new LinkedHashMap<>(this.currentLanes);
601         Map<Lane, Double> fractionalLanePositions = new LinkedHashMap<>();
602         for (Lane lane : lanesCopy.keySet())
603         {
604             fractionalLanePositions.put(lane, fractionalPosition(lane, getReference()));
605         }
606         int numRegistered = 0;
607         for (Lane lane : lanesCopy.keySet())
608         {
609             Set<Lane> laneSet = lane.accessibleAdjacentLanesLegal(laneChangeDirection, getGTUType(), getDirection(lane));
610             if (laneSet.size() > 0)
611             {
612                 numRegistered++;
613                 Lane adjacentLane = laneSet.iterator().next();
614                 Length position = adjacentLane.getLength().times(fractionalLanePositions.get(lane));
615                 if (lanesCopy.get(lane).isPlus() ? position.lt(lane.getLength().minus(getRear().getDx()))
616                         : position.gt(getFront().getDx().neg()))
617                 {
618                     this.enteredLanes.add(adjacentLane);
619                     enterLane(adjacentLane, position, lanesCopy.get(lane));
620                 }
621                 else
622                 {
623                     System.out.println("Skipping enterLane for GTU " + getId() + " on lane " + lane.getFullId() + " at "
624                             + position + ", lane length = " + lane.getLength() + " rear = " + getRear().getDx() + " front = "
625                             + getFront().getDx());
626                 }
627             }
628         }
629         Throw.when(numRegistered == 0, GTUException.class, "Gtu %s starting %s lane change, but no adjacent lane found.",
630                 getId(), laneChangeDirection);
631     }
632 
633     /**
634      * Performs the finalization of a lane change by leaving the from lanes.
635      * @param laneChangeDirection LateralDirectionality; direction of lane change
636      */
637     @SuppressWarnings("checkstyle:designforextension")
638     protected void finalizeLaneChange(final LateralDirectionality laneChangeDirection)
639     {
640         Map<Lane, GTUDirectionality> lanesCopy = new LinkedHashMap<>(this.currentLanes);
641         Set<Lane> lanesToBeRemoved = new LinkedHashSet<>();
642         Lane fromLane = null;
643         Length fromPosition = null;
644         GTUDirectionality fromDirection = null;
645         try
646         {
647             // find lanes to leave as they have an adjacent lane the GTU is also on in the lane change direction
648             for (Lane lane : lanesCopy.keySet())
649             {
650                 Iterator<Lane> iterator =
651                         lane.accessibleAdjacentLanesPhysical(laneChangeDirection, getGTUType(), getDirection(lane)).iterator();
652                 if (iterator.hasNext() && lanesCopy.keySet().contains(iterator.next()))
653                 {
654                     lanesToBeRemoved.add(lane);
655                 }
656             }
657             // some lanes registered to the GTU may be downstream of a split and have no adjacent lane, find longitudinally
658             boolean added = true;
659             while (added)
660             {
661                 added = false;
662                 Set<Lane> lanesToAlsoBeRemoved = new LinkedHashSet<>();
663                 for (Lane lane : lanesToBeRemoved)
664                 {
665                     GTUDirectionality direction = getDirection(lane);
666                     for (Lane nextLane : direction.isPlus() ? lane.nextLanes(getGTUType()).keySet()
667                             : lane.prevLanes(getGTUType()).keySet())
668                     {
669                         if (lanesCopy.containsKey(nextLane) && !lanesToBeRemoved.contains(nextLane))
670                         {
671                             added = true;
672                             lanesToAlsoBeRemoved.add(nextLane);
673                         }
674                     }
675                 }
676                 lanesToBeRemoved.addAll(lanesToAlsoBeRemoved);
677             }
678             double nearest = Double.POSITIVE_INFINITY;
679             for (Lane lane : lanesToBeRemoved)
680             {
681                 Length pos = position(lane, RelativePosition.REFERENCE_POSITION);
682                 if (0.0 <= pos.si && pos.si <= lane.getLength().si)
683                 {
684                     fromLane = lane;
685                     fromPosition = pos;
686                     fromDirection = getDirection(lane);
687                 }
688                 else if (fromLane == null && (getDirection(lane).isPlus() ? pos.si > lane.getLength().si : pos.le0()))
689                 {
690                     // if the reference point is in between two lanes, this recognizes the lane upstream of the gap
691                     double distance = getDirection(lane).isPlus() ? pos.si - lane.getLength().si : -pos.si;
692                     if (distance < nearest)
693                     {
694                         nearest = distance;
695                         fromLane = lane;
696                         fromPosition = pos;
697                         fromDirection = getDirection(lane);
698                     }
699                 }
700                 leaveLane(lane);
701             }
702             this.referencePositionTime = Double.NaN;
703             this.finalizeLaneChangeEvent = null;
704         }
705         catch (GTUException exception)
706         {
707             // should not happen, lane was obtained from GTU
708             throw new RuntimeException("position on lane not possible", exception);
709         }
710         Throw.when(fromLane == null, RuntimeException.class, "No from lane for lane change event.");
711         DirectedLanePosition from;
712         try
713         {
714             from = new DirectedLanePosition(fromLane, fromPosition, fromDirection);
715         }
716         catch (GTUException exception)
717         {
718             throw new RuntimeException(exception);
719         }
720         this.fireTimedEvent(LaneBasedGTU.LANE_CHANGE_EVENT, new Object[] { getId(), laneChangeDirection, from },
721                 getSimulator().getSimulatorTime());
722     }
723 
724     /** {@inheritDoc} */
725     @Override
726     public void setFinalizeLaneChangeEvent(final SimEventInterface<SimTimeDoubleUnit> event)
727     {
728         this.finalizeLaneChangeEvent = event;
729     }
730 
731     /** {@inheritDoc} */
732     @Override
733     public final GTUDirectionality getDirection(final Lane lane) throws GTUException
734     {
735         Throw.when(!this.currentLanes.containsKey(lane), GTUException.class, "getDirection: Lanes %s does not contain %s",
736                 this.currentLanes.keySet(), lane);
737         return this.currentLanes.get(lane);
738     }
739 
740     /** {@inheritDoc} */
741     @Override
742     @SuppressWarnings("checkstyle:designforextension")
743     protected boolean move(final DirectedPoint fromLocation)
744             throws SimRuntimeException, GTUException, OperationalPlanException, NetworkException, ParameterException
745     {
746         // DirectedPoint currentPoint = getLocation(); // used for "jump" detection that is also commented out
747         // Only carry out move() if we still have lane(s) to drive on.
748         // Note: a (Sink) trigger can have 'destroyed' us between the previous evaluation step and this one.
749         if (this.currentLanes.isEmpty())
750         {
751             destroy();
752             return false; // Done; do not re-schedule execution of this move method.
753         }
754 
755         // remove enter events
756         // WS: why?
757         // for (Lane lane : this.pendingEnterTriggers.keySet())
758         // {
759         // System.out.println("GTU " + getId() + " is canceling event on lane " + lane.getFullId());
760         // List<SimEventInterface<SimTimeDoubleUnit>> events = this.pendingEnterTriggers.get(lane);
761         // for (SimEventInterface<SimTimeDoubleUnit> event : events)
762         // {
763         // // also unregister from lane
764         // this.currentLanes.remove(lane);
765         // getSimulator().cancelEvent(event);
766         // }
767         // }
768         // this.pendingEnterTriggers.clear();
769 
770         // get distance covered in previous plan, to aid a shift in link fraction (from which a plan moves onwards)
771         Length covered;
772         if (getOperationalPlan() instanceof LaneBasedOperationalPlan
773                 && ((LaneBasedOperationalPlan) getOperationalPlan()).isDeviative())
774         {
775             // traveled distance as difference between start and current position on reference lane
776             // note that for a deviative plan the traveled distance along the path is not valuable here
777             LaneBasedOperationalPlanrg/opentrafficsim/road/gtu/lane/plan/operational/LaneBasedOperationalPlan.html#LaneBasedOperationalPlan">LaneBasedOperationalPlan plan = (LaneBasedOperationalPlan) getOperationalPlan();
778             DirectedLanePosition ref = getReferencePosition();
779             covered = ref.getGtuDirection().isPlus()
780                     ? position(ref.getLane(), getReference())
781                             .minus(position(ref.getLane(), getReference(), plan.getStartTime()))
782                     : position(ref.getLane(), getReference(), plan.getStartTime())
783                             .minus(position(ref.getLane(), getReference()));
784             // Note that distance is valid as the reference lane can not change (and location of previous plan is start location
785             // of current plan). Only instantaneous lane changes can do that, which do not result in deviative plans.
786         }
787         else
788         {
789             covered = getOperationalPlan().getTraveledDistance(getSimulator().getSimulatorTime());
790         }
791 
792         // generate the next operational plan and carry it out
793         // in case of an instantaneous lane change, fractionalLinkPositions will be accordingly adjusted to the new lane
794         super.move(fromLocation);
795 
796         // update the positions on the lanes we are registered on
797         // WS: this was previously done using fractions calculated before super.move() based on the GTU position, but an
798         // instantaneous lane change while e.g. the nose is on the next lane which is curved, results in a different fraction on
799         // the next link (the GTU doesn't stretch or shrink)
800         Map<Link, Double> newLinkFractions = new LinkedHashMap<>(this.fractionalLinkPositions);
801         Set<Link> done = new LinkedHashSet<>();
802         // WS: this used to be on all current lanes, skipping links already processed, but 'covered' regards the reference lane
803         updateLinkFraction(getReferencePosition().getLane(), newLinkFractions, done, false, covered, true);
804         updateLinkFraction(getReferencePosition().getLane(), newLinkFractions, done, true, covered, true);
805         this.fractionalLinkPositions.clear();
806         this.fractionalLinkPositions.putAll(newLinkFractions);
807 
808         DirectedLanePosition dlp = getReferencePosition();
809         fireTimedEvent(
810                 LaneBasedGTU.LANEBASED_MOVE_EVENT,
811                 new Object[] { getId(), new OTSPoint3D(fromLocation).doubleVector(PositionUnit.METER),
812                         OTSPoint3D.direction(fromLocation, DirectionUnit.EAST_RADIAN), getSpeed(), getAcceleration(),
813                         getTurnIndicatorStatus(), getOdometer(), dlp.getLane().getParentLink().getId(), dlp.getLane().getId(),
814                         dlp.getPosition(), dlp.getGtuDirection().name() },
815                 getSimulator().getSimulatorTime());
816 
817         if (getOperationalPlan().getAcceleration(Duration.ZERO).si < -10
818                 && getOperationalPlan().getSpeed(Duration.ZERO).si > 2.5)
819         {
820             System.err.println("GTU: " + getId() + " - getOperationalPlan().getAcceleration(Duration.ZERO).si < -10)");
821             System.err.println("Lanes in current plan: " + this.currentLanes.keySet());
822             if (getTacticalPlanner().getPerception().contains(DefaultSimplePerception.class))
823             {
824                 DefaultSimplePerception p =
825                         getTacticalPlanner().getPerception().getPerceptionCategory(DefaultSimplePerception.class);
826                 System.err.println("HeadwayGTU: " + p.getForwardHeadwayGTU());
827                 System.err.println("HeadwayObject: " + p.getForwardHeadwayObject());
828             }
829         }
830         // DirectedPoint currentPointAfterMove = getLocation();
831         // if (currentPoint.distance(currentPointAfterMove) > 0.1)
832         // {
833         // System.err.println(this.getId() + " jumped");
834         // }
835         // schedule triggers and determine when to enter lanes with front and leave lanes with rear
836         scheduleEnterLeaveTriggers();
837         return false;
838     }
839 
840     /**
841      * Recursive update of link fractions based on a moved distance.
842      * @param lane Lane; current lane, start with reference lane
843      * @param newLinkFractions Map&lt;Link, Double&gt;; map to put new fractions in
844      * @param done Set&lt;Link&gt;; links to skip as link are already done
845      * @param prevs boolean; whether to loop to the previous or next lanes, regardless of driving direction
846      * @param covered Length; covered distance along the reference lane
847      * @param isReferenceLane boolean; whether this lane is the reference lane (to skip in second call)
848      */
849     private void updateLinkFraction(final Lane lane, final Map<Link, Double> newLinkFractions, final Set<Link> done,
850             final boolean prevs, final Length covered, final boolean isReferenceLane)
851     {
852         if (!prevs || !isReferenceLane)
853         {
854             if (done.contains(lane.getParentLink()) || !this.currentLanes.containsKey(lane))
855             {
856                 return;
857             }
858             double sign;
859             try
860             {
861                 sign = getDirection(lane).isPlus() ? 1.0 : -1.0;
862             }
863             catch (GTUException exception)
864             {
865                 // can not happen as we check that the lane is in the currentLanes
866                 throw new RuntimeException("Unexpected exception: trying to obtain direction on lane.", exception);
867             }
868             newLinkFractions.put(lane.getParentLink(),
869                     this.fractionalLinkPositions.get(lane.getParentLink()) + sign * covered.si / lane.getLength().si);
870             done.add(lane.getParentLink());
871         }
872         for (Lane nextLane : (prevs ? lane.prevLanes(getGTUType()) : lane.nextLanes(getGTUType())).keySet())
873         {
874             updateLinkFraction(nextLane, newLinkFractions, done, prevs, covered, false);
875         }
876     }
877 
878     /** {@inheritDoc} */
879     @Override
880     public final Map<Lane, Length> positions(final RelativePosition relativePosition) throws GTUException
881     {
882         return positions(relativePosition, getSimulator().getSimulatorTime());
883     }
884 
885     /** {@inheritDoc} */
886     @Override
887     public final Map<Lane, Length> positions(final RelativePosition relativePosition, final Time when) throws GTUException
888     {
889         Map<Lane, Length> positions = new LinkedHashMap<>();
890         for (Lane lane : this.currentLanes.keySet())
891         {
892             positions.put(lane, position(lane, relativePosition, when));
893         }
894         return positions;
895     }
896 
897     /** {@inheritDoc} */
898     @Override
899     public final Length position(final Lane lane, final RelativePosition relativePosition) throws GTUException
900     {
901         return position(lane, relativePosition, getSimulator().getSimulatorTime());
902     }
903 
904     /**
905      * Return the longitudinal position that the indicated relative position of this GTU would have if it were to change to
906      * another Lane with a / the current CrossSectionLink. This point may be before the begin or after the end of the link of
907      * the projection lane of the GTU. This preserves the length of the GTU.
908      * @param projectionLane Lane; the lane onto which the position of this GTU must be projected
909      * @param relativePosition RelativePosition; the point on this GTU that must be projected
910      * @param when Time; the time for which to project the position of this GTU
911      * @return Length; the position of this GTU in the projectionLane
912      * @throws GTUException when projectionLane it not in any of the CrossSectionLink that the GTU is on
913      */
914     @SuppressWarnings("checkstyle:designforextension")
915     public Length translatedPosition(final Lane projectionLane, final RelativePosition relativePosition, final Time when)
916             throws GTUException
917     {
918         CrossSectionLink link = projectionLane.getParentLink();
919         for (CrossSectionElement cse : link.getCrossSectionElementList())
920         {
921             if (cse instanceof Lane)
922             {
923                 Lane"../../../../../org/opentrafficsim/road/network/lane/Lane.html#Lane">Lane cseLane = (Lane) cse;
924                 if (null != this.currentLanes.get(cseLane))
925                 {
926                     double fractionalPosition = fractionalPosition(cseLane, RelativePosition.REFERENCE_POSITION, when);
927                     Length pos = new Length(projectionLane.getLength().getSI() * fractionalPosition, LengthUnit.SI);
928                     if (this.currentLanes.get(cseLane).isPlus())
929                     {
930                         return pos.plus(relativePosition.getDx());
931                     }
932                     return pos.minus(relativePosition.getDx());
933                 }
934             }
935         }
936         throw new GTUException(this + " is not on any lane of Link " + link);
937     }
938 
939     /**
940      * Return the longitudinal position on the projection lane that has the same fractional position on one of the current lanes
941      * of the indicated relative position. This preserves the fractional positions of all relative positions of the GTU.
942      * @param projectionLane Lane; the lane onto which the position of this GTU must be projected
943      * @param relativePosition RelativePosition; the point on this GTU that must be projected
944      * @param when Time; the time for which to project the position of this GTU
945      * @return Length; the position of this GTU in the projectionLane
946      * @throws GTUException when projectionLane it not in any of the CrossSectionLink that the GTU is on
947      */
948     @SuppressWarnings("checkstyle:designforextension")
949     public Length projectedPosition(final Lane projectionLane, final RelativePosition relativePosition, final Time when)
950             throws GTUException
951     {
952         CrossSectionLink link = projectionLane.getParentLink();
953         for (CrossSectionElement cse : link.getCrossSectionElementList())
954         {
955             if (cse instanceof Lane)
956             {
957                 Lane"../../../../../org/opentrafficsim/road/network/lane/Lane.html#Lane">Lane cseLane = (Lane) cse;
958                 if (null != this.currentLanes.get(cseLane))
959                 {
960                     double fractionalPosition = fractionalPosition(cseLane, relativePosition, when);
961                     return new Length(projectionLane.getLength().getSI() * fractionalPosition, LengthUnit.SI);
962                 }
963             }
964         }
965         throw new GTUException(this + " is not on any lane of Link " + link);
966     }
967 
968     /** caching of time field for last stored position(s). */
969     private double cachePositionsTime = Double.NaN;
970 
971     /** caching of last stored position(s). */
972     private Map<Integer, Length> cachedPositions = new LinkedHashMap<>();
973 
974     /** {@inheritDoc} */
975     @Override
976     @SuppressWarnings("checkstyle:designforextension")
977     public Length position(final Lane lane, final RelativePosition relativePosition, final Time when) throws GTUException
978     {
979         int cacheIndex = 0;
980         if (CACHING)
981         {
982             cacheIndex = 17 * lane.hashCode() + relativePosition.hashCode();
983             Length l;
984             if (when.si == this.cachePositionsTime && (l = this.cachedPositions.get(cacheIndex)) != null)
985             {
986                 // PK verify the result; uncomment if you don't trust the cache
987                 // this.cachedPositions.clear();
988                 // Length difficultWay = position(lane, relativePosition, when);
989                 // if (Math.abs(l.si - difficultWay.si) > 0.00001)
990                 // {
991                 // System.err.println("Whoops: cache returns bad value for GTU " + getId());
992                 // }
993                 CACHED_POSITION++;
994                 return l;
995             }
996             if (when.si != this.cachePositionsTime)
997             {
998                 this.cachedPositions.clear();
999                 this.cachePositionsTime = when.si;
1000             }
1001         }
1002         NON_CACHED_POSITION++;
1003 
1004         synchronized (this.lock)
1005         {
1006             double loc = Double.NaN;
1007             try
1008             {
1009                 OperationalPlan plan = getOperationalPlan(when);
1010                 if (!(plan instanceof LaneBasedOperationalPlanorg/opentrafficsim/road/gtu/lane/plan/operational/LaneBasedOperationalPlan.html#LaneBasedOperationalPlan">LaneBasedOperationalPlan) || !((LaneBasedOperationalPlan) plan).isDeviative())
1011                 {
1012                     double longitudinalPosition;
1013                     try
1014                     {
1015                         longitudinalPosition =
1016                                 lane.positionSI(this.fractionalLinkPositions.get(when).get(lane.getParentLink()));
1017                     }
1018                     catch (NullPointerException exception)
1019                     {
1020                         throw exception;
1021                     }
1022                     if (this.currentLanes.get(when).get(lane).isPlus())
1023                     {
1024                         loc = longitudinalPosition + plan.getTraveledDistanceSI(when) + relativePosition.getDx().si;
1025                     }
1026                     else
1027                     {
1028                         loc = longitudinalPosition - plan.getTraveledDistanceSI(when) - relativePosition.getDx().si;
1029                     }
1030                 }
1031                 else
1032                 {
1033                     // deviative LaneBasedOperationalPlan, i.e. the GTU is not on a center line
1034                     DirectedPoint p = plan.getLocation(when, relativePosition);
1035                     double f = lane.getCenterLine().projectFractional(null, null, p.x, p.y, FractionalFallback.NaN);
1036                     if (!Double.isNaN(f))
1037                     {
1038                         loc = f * lane.getLength().si;
1039                     }
1040                     else
1041                     {
1042                         // the point does not project fractionally to this lane, it has to be up- or downstream of the lane
1043 
1044                         // simple heuristic to decide if we first look upstream or downstream
1045                         boolean upstream = this.fractionalLinkPositions.get(lane.getParentLink()) < 0.0 ? true : false;
1046 
1047                         // use loop up to 2 times (for loop creates 'loc not initialized' warning)
1048                         int i = 0;
1049                         while (true)
1050                         {
1051                             Set<Lane> otherLanesToConsider = new LinkedHashSet<>();
1052                             otherLanesToConsider.addAll(this.currentLanes.keySet());
1053                             double distance = getDistanceAtOtherLane(lane, when, upstream, 0.0, p, otherLanesToConsider);
1054                             // distance can be positive on an upstream lane due to a loop
1055                             if (!Double.isNaN(distance))
1056                             {
1057                                 if (i == 1 && !Double.isNaN(loc))
1058                                 {
1059                                     // loc was determined in both loops, this constitutes a lane-loop, select nearest
1060                                     double loc2 = upstream ? -distance : distance + lane.getLength().si;
1061                                     double d1 = loc < 0.0 ? -loc : loc - lane.getLength().si;
1062                                     double d2 = loc2 < 0.0 ? -loc2 : loc2 - lane.getLength().si;
1063                                     loc = d1 < d2 ? loc : loc2;
1064                                     break;
1065                                 }
1066                                 else
1067                                 {
1068                                     // loc was determined in second loop
1069                                     loc = upstream ? -distance : distance + lane.getLength().si;
1070                                 }
1071                             }
1072                             else if (!Double.isNaN(loc))
1073                             {
1074                                 // loc was determined in first loop
1075                                 break;
1076                             }
1077                             else if (i == 1)
1078                             {
1079                                 // loc was determined in neither loop
1080                                 // Lane change ended while moving to next link. The source lanes are left and for a leave-lane
1081                                 // event the position is required. This may depend on upstream or downstream lanes as the
1082                                 // reference position is projected to that lane. But if we already left that lane, we can't use
1083                                 // it. We thus use ENDPOINT fallback instead.
1084                                 loc = lane.getLength().si * lane.getCenterLine().projectFractional(null, null, p.x, p.y,
1085                                         FractionalFallback.ENDPOINT);
1086                                 break;
1087                             }
1088                             // try other direction
1089                             i++;
1090                             upstream = !upstream;
1091                         }
1092                     }
1093                 }
1094             }
1095             catch (NullPointerException e)
1096             {
1097                 throw new GTUException("lanesCurrentOperationalPlan or fractionalLinkPositions is null", e);
1098             }
1099             catch (Exception e)
1100             {
1101                 System.err.println(toString());
1102                 System.err.println(this.currentLanes.get(when));
1103                 System.err.println(this.fractionalLinkPositions.get(when));
1104                 throw new GTUException(e);
1105             }
1106             if (Double.isNaN(loc))
1107             {
1108                 System.out.println("loc is NaN");
1109             }
1110             Length length = Length.instantiateSI(loc);
1111             if (CACHING)
1112             {
1113                 this.cachedPositions.put(cacheIndex, length);
1114             }
1115             return length;
1116         }
1117     }
1118 
1119     /** Set of lane to attempt when determining the location with a deviative lane change. */
1120     // private Set<Lane> otherLanesToConsider;
1121 
1122     /**
1123      * In case of a deviative operational plan (not on the center lines), positions are projected fractionally to the center
1124      * lines. For points upstream or downstream of a lane, fractional projection is not valid. In such cases we need to project
1125      * the position to an upstream or downstream lane instead, and adjust length along the center lines.
1126      * @param lane Lane; lane to determine the position on
1127      * @param when Time; time
1128      * @param upstream boolean; whether to check upstream (or downstream)
1129      * @param distance double; cumulative distance in recursive search, starts at 0.0
1130      * @param point DirectedPoint; absolute point of GTU to be projected to center line
1131      * @param otherLanesToConsider Set&lt;Lane&gt;; lanes to consider
1132      * @return Length; position on lane being &lt;0 or &gt;{@code lane.getLength()}
1133      * @throws GTUException if GTU is not on the lane
1134      */
1135     private double getDistanceAtOtherLane(final Lane lane, final Time when, final boolean upstream, final double distance,
1136             final DirectedPoint point, final Set<Lane> otherLanesToConsider) throws GTUException
1137     {
1138         Set<Lane> nextLanes = new LinkedHashSet<>(upstream == getDirection(lane).isPlus()
1139                 ? lane.prevLanes(getGTUType()).keySet() : lane.nextLanes(getGTUType()).keySet()); // safe copy
1140         nextLanes.retainAll(otherLanesToConsider); // as we delete here
1141         if (!upstream && nextLanes.size() > 1)
1142         {
1143             LaneDirectionane/LaneDirection.html#LaneDirection">LaneDirection laneDir = new LaneDirection(lane, getDirection(lane)).getNextLaneDirection(this);
1144             if (nextLanes.contains(laneDir.getLane()))
1145             {
1146                 nextLanes.clear();
1147                 nextLanes.add(laneDir.getLane());
1148             }
1149             else
1150             {
1151                 getSimulator().getLogger().always().error("Distance on downstream lane could not be determined.");
1152             }
1153         }
1154         // TODO When requesting the position at the end of the plan, which will be on a further lane, this lane is not yet
1155         // part of the lanes in the current operational plan. This can be upstream or downstream depending on the direction of
1156         // travel. We might check whether getDirection(lane)=DIR_PLUS and upstream=false, or getDirection(lane)=DIR_MINUS and
1157         // upstream=true, to then use LaneDirection.getNextLaneDirection(this) to obtain the next lane. This is only required if
1158         // nextLanes originally had more than 1 lane, otherwise we can simply use that one lane. Problem is that the search
1159         // might go on far or even eternally (on a circular network), as projection simply keeps failing because the GTU is
1160         // actually towards the other longitudinal direction. Hence, the heuristic used before this method is called should
1161         // change and first always search against the direction of travel, and only consider lanes in currentLanes, while the
1162         // consecutive search in the direction of travel should then always find a point. We could build in a counter to prevent
1163         // a hanging software.
1164         if (nextLanes.size() == 0)
1165         {
1166             return Double.NaN; // point must be in the other direction
1167         }
1168         Throw.when(nextLanes.size() > 1, IllegalStateException.class,
1169                 "A position (%s) of GTU %s is not on any of the current registered lanes.", point, this.getId());
1170         Lane nextLane = nextLanes.iterator().next();
1171         otherLanesToConsider.remove(lane);
1172         double f = nextLane.getCenterLine().projectFractional(null, null, point.x, point.y, FractionalFallback.NaN);
1173         if (Double.isNaN(f))
1174         {
1175             return getDistanceAtOtherLane(nextLane, when, upstream, distance + nextLane.getLength().si, point,
1176                     otherLanesToConsider);
1177         }
1178         return distance + (upstream == this.currentLanes.get(nextLane).isPlus() ? 1.0 - f : f) * nextLane.getLength().si;
1179     }
1180 
1181     /** Time of reference position cache. */
1182     private double referencePositionTime = Double.NaN;
1183 
1184     /** Cached reference position. */
1185     private DirectedLanePosition cachedReferencePosition = null;
1186 
1187     /** {@inheritDoc} */
1188     @Override
1189     @SuppressWarnings("checkstyle:designforextension")
1190     public DirectedLanePosition getReferencePosition() throws GTUException
1191     {
1192         if (this.referencePositionTime == getSimulator().getSimulatorTime().si)
1193         {
1194             return this.cachedReferencePosition;
1195         }
1196         boolean anyOnLink = false;
1197         Lane refLane = null;
1198         double closest = Double.POSITIVE_INFINITY;
1199         double minEps = Double.POSITIVE_INFINITY;
1200         for (Lane lane : this.currentLanes.keySet())
1201         {
1202             double fraction = fractionalPosition(lane, getReference());
1203             if (fraction >= 0.0 && fraction <= 1.0)
1204             {
1205                 // TODO widest lane in case we are registered on more than one lane with the reference point?
1206                 // TODO lane that leads to our location or not if we are registered on parallel lanes?
1207                 if (!anyOnLink)
1208                 {
1209                     refLane = lane;
1210                 }
1211                 else
1212                 {
1213                     DirectedPoint loc = getLocation();
1214                     double f = lane.getCenterLine().projectFractional(null, null, loc.x, loc.y, FractionalFallback.ENDPOINT);
1215                     double distance = loc.distance(lane.getCenterLine().getLocationFractionExtended(f));
1216                     if (refLane != null && Double.isInfinite(closest))
1217                     {
1218                         f = refLane.getCenterLine().projectFractional(null, null, loc.x, loc.y, FractionalFallback.ENDPOINT);
1219                         closest = loc.distance(refLane.getCenterLine().getLocationFractionExtended(f));
1220                     }
1221                     if (distance < closest)
1222                     {
1223                         refLane = lane;
1224                         closest = distance;
1225                     }
1226                 }
1227                 anyOnLink = true;
1228             }
1229             else if (!anyOnLink && Double.isInfinite(closest))// && getOperationalPlan() instanceof LaneBasedOperationalPlan
1230             // && ((LaneBasedOperationalPlan) getOperationalPlan()).isDeviative())
1231             {
1232                 double eps = (fraction > 1.0 ? lane.getCenterLine().getLast() : lane.getCenterLine().getFirst())
1233                         .distanceSI(new OTSPoint3D(getLocation()));
1234                 if (eps < minEps)
1235                 {
1236                     minEps = eps;
1237                     refLane = lane;
1238                 }
1239             }
1240         }
1241         if (refLane != null)
1242         {
1243             this.cachedReferencePosition =
1244                     new DirectedLanePosition(refLane, position(refLane, getReference()), this.getDirection(refLane));
1245             this.referencePositionTime = getSimulator().getSimulatorTime().si;
1246             return this.cachedReferencePosition;
1247         }
1248         // for (Lane lane : this.currentLanes.keySet())
1249         // {
1250         // Length relativePosition = position(lane, RelativePosition.REFERENCE_POSITION);
1251         // System.err
1252         // .println(String.format("Lane %s of Link %s: absolute position %s, relative position %5.1f%%", lane.getId(),
1253         // lane.getParentLink().getId(), relativePosition, relativePosition.si * 100 / lane.getLength().si));
1254         // }
1255         throw new GTUException("The reference point of GTU " + this + " is not on any of the lanes on which it is registered");
1256     }
1257 
1258     /**
1259      * Schedule the triggers for this GTU that are going to happen until the next evaluation time. Also schedule the
1260      * registration and deregistration of lanes when the vehicle enters or leaves them, at the exact right time. <br>
1261      * Note: when the GTU makes a lane change, the vehicle will be registered for both lanes during the entire maneuver.
1262      * @throws NetworkException on network inconsistency
1263      * @throws SimRuntimeException should never happen
1264      * @throws GTUException when a branch is reached where the GTU does not know where to go next
1265      */
1266     @SuppressWarnings("checkstyle:designforextension")
1267     protected void scheduleEnterLeaveTriggers() throws NetworkException, SimRuntimeException, GTUException
1268     {
1269 
1270         LaneBasedOperationalPlan plan = null;
1271         double moveSI;
1272         if (getOperationalPlan() instanceof LaneBasedOperationalPlan)
1273         {
1274             plan = (LaneBasedOperationalPlan) getOperationalPlan();
1275             moveSI = plan.getTotalLengthAlongLane(this).si;
1276         }
1277         else
1278         {
1279             moveSI = getOperationalPlan().getTotalLength().si;
1280         }
1281 
1282         // Figure out which lanes this GTU will enter with its FRONT, and schedule the lane enter events
1283         Map<Lane, GTUDirectionality> lanesCopy = new LinkedHashMap<>(this.currentLanes);
1284         Iterator<Lane> it = lanesCopy.keySet().iterator();
1285         Lane enteredLane = null;
1286         // LateralDirectionality forceSide = LateralDirectionality.NONE;
1287         while (it.hasNext() || enteredLane != null) // use a copy because this.currentLanes can change
1288         {
1289             // next lane from 'lanesCopy', or asses the lane we just entered as it may be very short and also passed fully
1290             Lane lane;
1291             GTUDirectionality laneDir;
1292             if (enteredLane == null)
1293             {
1294                 lane = it.next();
1295                 laneDir = lanesCopy.get(lane);
1296             }
1297             else
1298             {
1299                 lane = enteredLane;
1300                 laneDir = this.currentLanes.get(lane);
1301             }
1302             double sign = laneDir.isPlus() ? 1.0 : -1.0;
1303             enteredLane = null;
1304 
1305             // skip if already on next lane
1306             if (!Collections.disjoint(this.currentLanes.keySet(),
1307                     lane.downstreamLanes(laneDir, getGTUType()).keySet().toCollection()))
1308             {
1309                 continue;
1310             }
1311 
1312             // schedule triggers on this lane
1313             double referenceStartSI = this.fractionalLinkPositions.get(lane.getParentLink()) * lane.getLength().getSI();
1314             // referenceStartSI is position of reference of GTU on current lane
1315             if (laneDir.isPlus())
1316             {
1317                 lane.scheduleSensorTriggers(this, referenceStartSI, moveSI);
1318             }
1319             else
1320             {
1321                 lane.scheduleSensorTriggers(this, referenceStartSI - moveSI, moveSI);
1322             }
1323 
1324             double nextFrontPosSI = referenceStartSI + sign * (moveSI + getFront().getDx().si);
1325             Lane nextLane = null;
1326             GTUDirectionality nextDirection = null;
1327             Length refPosAtLastTimestep = null;
1328             DirectedPoint end = null;
1329             if (laneDir.isPlus() ? nextFrontPosSI > lane.getLength().si : nextFrontPosSI < 0.0)
1330             {
1331                 LaneDirectionk/lane/LaneDirection.html#LaneDirection">LaneDirection next = new LaneDirection(lane, laneDir).getNextLaneDirection(this);
1332                 if (null == next)
1333                 {
1334                     // A sink should delete the GTU, or a lane change should end, before reaching the end of the lane
1335                     continue;
1336                 }
1337                 nextLane = next.getLane();
1338                 nextDirection = next.getDirection();
1339                 double endPos = laneDir.isPlus() ? lane.getLength().si - getFront().getDx().si : getFront().getDx().si;
1340                 Lane endLane = lane;
1341                 GTUDirectionality endLaneDir = laneDir;
1342                 while (endLaneDir.isPlus() ? endPos < 0.0 : endPos > endLane.getLength().si)
1343                 {
1344                     // GTU front is more than lane length, so end location can not be extracted from the lane, let's move then
1345                     Map<Lane, GTUDirectionality> map = endLane.upstreamLanes(endLaneDir, getGTUType()).toMap();
1346                     map.keySet().retainAll(this.currentLanes.keySet());
1347                     double remain = endLaneDir.isPlus() ? -endPos : endPos - endLane.getLength().si;
1348                     endLane = map.keySet().iterator().next();
1349                     endLaneDir = map.get(endLane);
1350                     endPos = endLaneDir.isPlus() ? endLane.getLength().si - remain : remain;
1351                 }
1352                 end = endLane.getCenterLine().getLocationExtendedSI(endPos);
1353                 if (laneDir.isPlus())
1354                 {
1355                     refPosAtLastTimestep = nextDirection.isPlus() ? Length.instantiateSI(referenceStartSI - lane.getLength().si)
1356                             : Length.instantiateSI(nextLane.getLength().si - referenceStartSI + lane.getLength().si);
1357                 }
1358                 else
1359                 {
1360                     refPosAtLastTimestep = nextDirection.isPlus() ? Length.instantiateSI(-referenceStartSI)
1361                             : Length.instantiateSI(nextLane.getLength().si + referenceStartSI);
1362                 }
1363             }
1364 
1365             if (end != null)
1366             {
1367                 Time enterTime = getOperationalPlan().timeAtPoint(end, false);
1368                 if (enterTime != null)
1369                 {
1370                     if (Double.isNaN(enterTime.si))
1371                     {
1372                         // TODO: this escape was in timeAtPoint, where it was changed to return null for leave lane events
1373                         enterTime = Time.instantiateSI(getOperationalPlan().getEndTime().si - 1e-9);
1374                         // -1e-9 prevents that next move() reschedules enter
1375                     }
1376                     addLaneToGtu(nextLane, refPosAtLastTimestep, nextDirection);
1377                     enteredLane = nextLane;
1378                     Length coveredDistance;
1379                     if (plan == null || !plan.isDeviative())
1380                     {
1381                         try
1382                         {
1383                             coveredDistance = getOperationalPlan().getTraveledDistance(enterTime);
1384                         }
1385                         catch (OperationalPlanException exception)
1386                         {
1387                             throw new RuntimeException("Enter time of lane beyond plan.", exception);
1388                         }
1389                     }
1390                     else
1391                     {
1392                         coveredDistance = plan.getDistanceAlongLane(this, end);
1393                     }
1394                     SimEventInterface<SimTimeDoubleUnit> event = getSimulator().scheduleEventAbs(enterTime, this, this,
1395                             "addGtuToLane", new Object[] { nextLane, refPosAtLastTimestep.plus(coveredDistance) });
1396                     addEnterTrigger(nextLane, event);
1397                 }
1398             }
1399         }
1400 
1401         // Figure out which lanes this GTU will exit with its BACK, and schedule the lane exit events
1402         for (Lane lane : this.currentLanes.keySet())
1403         {
1404 
1405             double referenceStartSI = this.fractionalLinkPositions.get(lane.getParentLink()) * lane.getLength().getSI();
1406             Time exitTime = null;
1407 
1408             GTUDirectionality laneDir = getDirection(lane);
1409 
1410             if (plan == null || !plan.isDeviative())
1411             {
1412                 double sign = laneDir.isPlus() ? 1.0 : -1.0;
1413                 double nextRearPosSI = referenceStartSI + sign * (getRear().getDx().si + moveSI);
1414                 if (laneDir.isPlus() ? nextRearPosSI > lane.getLength().si : nextRearPosSI < 0.0)
1415                 {
1416                     exitTime = getOperationalPlan().timeAtDistance(
1417                             Length.instantiateSI((laneDir.isPlus() ? lane.getLength().si - referenceStartSI : referenceStartSI)
1418                                     - getRear().getDx().si));
1419                 }
1420             }
1421             else
1422             {
1423                 DirectedPoint end = null;
1424                 double endPos = laneDir.isPlus() ? lane.getLength().si - getRear().getDx().si : getRear().getDx().si;
1425                 Lane endLane = lane;
1426                 GTUDirectionality endLaneDir = laneDir;
1427                 while (endLaneDir.isPlus() ? endPos > endLane.getLength().si : endPos < 0.0)
1428                 {
1429                     Map<Lane, GTUDirectionality> map = endLane.downstreamLanes(endLaneDir, getGTUType()).toMap();
1430                     map.keySet().retainAll(this.currentLanes.keySet());
1431                     if (!map.isEmpty())
1432                     {
1433                         double remain = endLaneDir.isPlus() ? endPos - endLane.getLength().si : -endPos;
1434                         endLane = map.keySet().iterator().next();
1435                         endLaneDir = map.get(endLane);
1436                         endPos = endLaneDir.isPlus() ? remain : endLane.getLength().si - remain;
1437                     }
1438                     else
1439                     {
1440                         endPos = endLaneDir.isPlus() ? endLane.getLength().si - getRear().getDx().si : getRear().getDx().si;
1441                         break;
1442                     }
1443                 }
1444                 end = endLane.getCenterLine().getLocationExtendedSI(endPos);
1445                 if (end != null)
1446                 {
1447                     exitTime = getOperationalPlan().timeAtPoint(end, false);
1448                     if (Double.isNaN(exitTime.si))
1449                     {
1450                         // code below will leave entered lanes if exitTime is null, make this so if NaN results due to the lane
1451                         // end being beyond the plan (rather than the GTU never having been there, but being registered there
1452                         // upon lane change initiation)
1453                         double sign = laneDir.isPlus() ? 1.0 : -1.0;
1454                         double nextRearPosSI = referenceStartSI + sign * (getRear().getDx().si + moveSI);
1455                         if (laneDir.isPlus() ? nextRearPosSI < lane.getLength().si : nextRearPosSI > 0.0)
1456                         {
1457                             exitTime = null;
1458                         }
1459                     }
1460                 }
1461             }
1462 
1463             if (exitTime != null && !Double.isNaN(exitTime.si))
1464             {
1465                 SimEvent<SimTimeDoubleUnit> event = new SimEvent<>(new SimTimeDoubleUnit(exitTime), this, this, "leaveLane",
1466                         new Object[] { lane, new Boolean(false) });
1467                 getSimulator().scheduleEvent(event);
1468                 addTrigger(lane, event);
1469             }
1470             else if (exitTime != null && this.enteredLanes.contains(lane))
1471             {
1472                 // This lane was entered when initiating the lane change due to a fractional calculation. Now, the deviative
1473                 // plan indicates we will never reach this location.
1474                 SimEvent<SimTimeDoubleUnit> event = new SimEvent<>(getSimulator().getSimTime(), this, this, "leaveLane",
1475                         new Object[] { lane, new Boolean(false) });
1476                 getSimulator().scheduleEvent(event);
1477                 addTrigger(lane, event);
1478             }
1479         }
1480 
1481         this.enteredLanes.clear();
1482     }
1483 
1484     /** {@inheritDoc} */
1485     @Override
1486     public final Map<Lane, Double> fractionalPositions(final RelativePosition relativePosition) throws GTUException
1487     {
1488         return fractionalPositions(relativePosition, getSimulator().getSimulatorTime());
1489     }
1490 
1491     /** {@inheritDoc} */
1492     @Override
1493     public final Map<Lane, Double> fractionalPositions(final RelativePosition relativePosition, final Time when)
1494             throws GTUException
1495     {
1496         Map<Lane, Double> positions = new LinkedHashMap<>();
1497         for (Lane lane : this.currentLanes.keySet())
1498         {
1499             positions.put(lane, fractionalPosition(lane, relativePosition, when));
1500         }
1501         return positions;
1502     }
1503 
1504     /** {@inheritDoc} */
1505     @Override
1506     public final double fractionalPosition(final Lane lane, final RelativePosition relativePosition, final Time when)
1507             throws GTUException
1508     {
1509         return position(lane, relativePosition, when).getSI() / lane.getLength().getSI();
1510     }
1511 
1512     /** {@inheritDoc} */
1513     @Override
1514     public final double fractionalPosition(final Lane lane, final RelativePosition relativePosition) throws GTUException
1515     {
1516         return position(lane, relativePosition).getSI() / lane.getLength().getSI();
1517     }
1518 
1519     /** {@inheritDoc} */
1520     @Override
1521     public final void addTrigger(final Lane lane, final SimEventInterface<SimTimeDoubleUnit> event)
1522     {
1523         List<SimEventInterface<SimTimeDoubleUnit>> list = this.pendingLeaveTriggers.get(lane);
1524         if (null == list)
1525         {
1526             list = new ArrayList<>();
1527         }
1528         list.add(event);
1529         this.pendingLeaveTriggers.put(lane, list);
1530     }
1531 
1532     /**
1533      * Add enter trigger.
1534      * @param lane Lane; lane
1535      * @param event SimEventInterface&lt;SimTimeDoubleUnit&gt;; event
1536      */
1537     private void addEnterTrigger(final Lane lane, final SimEventInterface<SimTimeDoubleUnit> event)
1538     {
1539         List<SimEventInterface<SimTimeDoubleUnit>> list = this.pendingEnterTriggers.get(lane);
1540         if (null == list)
1541         {
1542             list = new ArrayList<>();
1543         }
1544         list.add(event);
1545         this.pendingEnterTriggers.put(lane, list);
1546     }
1547 
1548     /**
1549      * Sets a vehicle model.
1550      * @param vehicleModel VehicleModel; vehicle model
1551      */
1552     public void setVehicleModel(final VehicleModel vehicleModel)
1553     {
1554         this.vehicleModel = vehicleModel;
1555     }
1556 
1557     /** {@inheritDoc} */
1558     @Override
1559     public VehicleModel getVehicleModel()
1560     {
1561         return this.vehicleModel;
1562     }
1563 
1564     /** {@inheritDoc} */
1565     @Override
1566     @SuppressWarnings("checkstyle:designforextension")
1567     public void destroy()
1568     {
1569         DirectedLanePosition dlp = null;
1570         try
1571         {
1572             dlp = getReferencePosition();
1573         }
1574         catch (GTUException e)
1575         {
1576             // ignore. not important at destroy
1577         }
1578         DirectedPoint location = this.getOperationalPlan() == null ? new DirectedPoint(0.0, 0.0, 0.0) : getLocation();
1579 
1580         synchronized (this.lock)
1581         {
1582             Set<Lane> laneSet = new LinkedHashSet<>(this.currentLanes.keySet()); // Operate on a copy of the key
1583                                                                                  // set
1584             for (Lane lane : laneSet)
1585             {
1586                 try
1587                 {
1588                     leaveLane(lane, true);
1589                 }
1590                 catch (GTUException e)
1591                 {
1592                     // ignore. not important at destroy
1593                 }
1594             }
1595         }
1596 
1597         if (dlp != null && dlp.getLane() != null)
1598         {
1599             Lane referenceLane = dlp.getLane();
1600             fireTimedEvent(LaneBasedGTU.LANEBASED_DESTROY_EVENT,
1601                     new Object[] { getId(), location, getOdometer(), referenceLane, dlp.getPosition(), dlp.getGtuDirection() },
1602                     getSimulator().getSimulatorTime());
1603         }
1604         else
1605         {
1606             fireTimedEvent(LaneBasedGTU.LANEBASED_DESTROY_EVENT,
1607                     new Object[] { getId(), location, getOdometer(), null, Length.ZERO, null },
1608                     getSimulator().getSimulatorTime());
1609         }
1610         if (this.finalizeLaneChangeEvent != null)
1611         {
1612             getSimulator().cancelEvent(this.finalizeLaneChangeEvent);
1613         }
1614 
1615         super.destroy();
1616     }
1617 
1618     /** {@inheritDoc} */
1619     @Override
1620     public final Bounds getBounds()
1621     {
1622         double dx = 0.5 * getLength().doubleValue();
1623         double dy = 0.5 * getWidth().doubleValue();
1624         return new BoundingBox(new Point3d(-dx, -dy, 0.0), new Point3d(dx, dy, 0.0));
1625     }
1626 
1627     /** {@inheritDoc} */
1628     @Override
1629     public final LaneBasedStrategicalPlanner getStrategicalPlanner()
1630     {
1631         return (LaneBasedStrategicalPlanner) super.getStrategicalPlanner();
1632     }
1633 
1634     /** {@inheritDoc} */
1635     @Override
1636     public final LaneBasedStrategicalPlanner getStrategicalPlanner(final Time time)
1637     {
1638         return (LaneBasedStrategicalPlanner) super.getStrategicalPlanner(time);
1639     }
1640 
1641     /** {@inheritDoc} */
1642     @Override
1643     public RoadNetwork getNetwork()
1644     {
1645         return (RoadNetwork) super.getPerceivableContext();
1646     }
1647 
1648     /** {@inheritDoc} */
1649     @Override
1650     public Speed getDesiredSpeed()
1651     {
1652         Time simTime = getSimulator().getSimulatorTime();
1653         if (this.desiredSpeedTime == null || this.desiredSpeedTime.si < simTime.si)
1654         {
1655             InfrastructurePerception infra =
1656                     getTacticalPlanner().getPerception().getPerceptionCategoryOrNull(InfrastructurePerception.class);
1657             SpeedLimitInfo speedInfo;
1658             if (infra == null)
1659             {
1660                 speedInfo = new SpeedLimitInfo();
1661                 speedInfo.addSpeedInfo(SpeedLimitTypes.MAX_VEHICLE_SPEED, getMaximumSpeed());
1662             }
1663             else
1664             {
1665                 // Throw.whenNull(infra, "InfrastructurePerception is required to determine the desired speed.");
1666                 speedInfo = infra.getSpeedLimitProspect(RelativeLane.CURRENT).getSpeedLimitInfo(Length.ZERO);
1667             }
1668             this.cachedDesiredSpeed =
1669                     Try.assign(() -> getTacticalPlanner().getCarFollowingModel().desiredSpeed(getParameters(), speedInfo),
1670                             "Parameter exception while obtaining the desired speed.");
1671             this.desiredSpeedTime = simTime;
1672         }
1673         return this.cachedDesiredSpeed;
1674     }
1675 
1676     /** {@inheritDoc} */
1677     @Override
1678     public Acceleration getCarFollowingAcceleration()
1679     {
1680         Time simTime = getSimulator().getSimulatorTime();
1681         if (this.carFollowingAccelerationTime == null || this.carFollowingAccelerationTime.si < simTime.si)
1682         {
1683             LanePerception perception = getTacticalPlanner().getPerception();
1684             // speed
1685             EgoPerception<?, ?> ego = perception.getPerceptionCategoryOrNull(EgoPerception.class);
1686             Throw.whenNull(ego, "EgoPerception is required to determine the speed.");
1687             Speed speed = ego.getSpeed();
1688             // speed limit info
1689             InfrastructurePerception infra = perception.getPerceptionCategoryOrNull(InfrastructurePerception.class);
1690             Throw.whenNull(infra, "InfrastructurePerception is required to determine the desired speed.");
1691             SpeedLimitInfo speedInfo = infra.getSpeedLimitProspect(RelativeLane.CURRENT).getSpeedLimitInfo(Length.ZERO);
1692             // leaders
1693             NeighborsPerception neighbors = perception.getPerceptionCategoryOrNull(NeighborsPerception.class);
1694             Throw.whenNull(neighbors, "NeighborsPerception is required to determine the car-following acceleration.");
1695             PerceptionCollectable<HeadwayGTU, LaneBasedGTU> leaders = neighbors.getLeaders(RelativeLane.CURRENT);
1696             // obtain
1697             this.cachedCarFollowingAcceleration =
1698                     Try.assign(() -> getTacticalPlanner().getCarFollowingModel().followingAcceleration(getParameters(), speed,
1699                             speedInfo, leaders), "Parameter exception while obtaining the desired speed.");
1700             this.carFollowingAccelerationTime = simTime;
1701         }
1702         return this.cachedCarFollowingAcceleration;
1703     }
1704 
1705     /** {@inheritDoc} */
1706     @Override
1707     public final TurnIndicatorStatus getTurnIndicatorStatus()
1708     {
1709         return this.turnIndicatorStatus.get();
1710     }
1711 
1712     /** {@inheritDoc} */
1713     @Override
1714     public final TurnIndicatorStatus getTurnIndicatorStatus(final Time time)
1715     {
1716         return this.turnIndicatorStatus.get(time);
1717     }
1718 
1719     /** {@inheritDoc} */
1720     @Override
1721     public final void setTurnIndicatorStatus(final TurnIndicatorStatus turnIndicatorStatus)
1722     {
1723         this.turnIndicatorStatus.set(turnIndicatorStatus);
1724     }
1725 
1726     /** {@inheritDoc} */
1727     @Override
1728     @SuppressWarnings("checkstyle:designforextension")
1729     public String toString()
1730     {
1731         return String.format("GTU " + getId());
1732     }
1733 
1734 }