View Javadoc
1   package org.opentrafficsim.road.gtu.lane.plan.operational;
2   
3   import java.util.ArrayList;
4   import java.util.Arrays;
5   import java.util.Iterator;
6   import java.util.List;
7   import java.util.Map;
8   
9   import org.djunits.unit.AccelerationUnit;
10  import org.djunits.unit.DurationUnit;
11  import org.djunits.unit.LengthUnit;
12  import org.djunits.unit.SpeedUnit;
13  import org.djunits.value.ValueRuntimeException;
14  import org.djunits.value.vdouble.scalar.Acceleration;
15  import org.djunits.value.vdouble.scalar.Duration;
16  import org.djunits.value.vdouble.scalar.Length;
17  import org.djunits.value.vdouble.scalar.Speed;
18  import org.djunits.value.vdouble.scalar.Time;
19  import org.djutils.exceptions.Throw;
20  import org.djutils.logger.CategoryLogger;
21  import org.opentrafficsim.base.parameters.ParameterException;
22  import org.opentrafficsim.core.geometry.DirectedPoint;
23  import org.opentrafficsim.core.geometry.OTSGeometryException;
24  import org.opentrafficsim.core.geometry.OTSLine3D;
25  import org.opentrafficsim.core.geometry.OTSPoint3D;
26  import org.opentrafficsim.core.gtu.GTUException;
27  import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan;
28  import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan.Segment;
29  import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan.SpeedSegment;
30  import org.opentrafficsim.core.gtu.plan.operational.OperationalPlanException;
31  import org.opentrafficsim.core.math.Solver;
32  import org.opentrafficsim.core.network.LateralDirectionality;
33  import org.opentrafficsim.core.network.NetworkException;
34  import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
35  import org.opentrafficsim.road.network.lane.DirectedLanePosition;
36  import org.opentrafficsim.road.network.lane.Lane;
37  import org.opentrafficsim.road.network.lane.LaneDirection;
38  import org.opentrafficsim.road.network.lane.object.sensor.SingleSensor;
39  import org.opentrafficsim.road.network.lane.object.sensor.SinkSensor;
40  
41  import nl.tudelft.simulation.dsol.SimRuntimeException;
42  import nl.tudelft.simulation.dsol.formalisms.eventscheduling.SimEventInterface;
43  
44  /**
45   * Builder for several often used operational plans. E.g., decelerate to come to a full stop at the end of a shape; accelerate
46   * to reach a certain speed at the end of a curve; drive constant on a curve; decelerate or accelerate to reach a given end
47   * speed at the end of a curve, etc.<br>
48   * TODO driving with negative speeds (backward driving) is not yet supported.
49   * <p>
50   * Copyright (c) 2013-2022 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
51   * BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
52   * </p>
53   * $LastChangedDate: 2015-07-24 02:58:59 +0200 (Fri, 24 Jul 2015) $, @version $Revision: 1147 $, by $Author: averbraeck $,
54   * initial version Nov 15, 2015 <br>
55   * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
56   * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
57   */
58  public final class LaneOperationalPlanBuilder // class package private for scheduling static method on an instance
59  {
60  
61      /** Maximum acceleration for unbounded accelerations: 1E12 m/s2. */
62      private static final Acceleration MAX_ACCELERATION = new Acceleration(1E12, AccelerationUnit.SI);
63  
64      /** Maximum deceleration for unbounded accelerations: -1E12 m/s2. */
65      private static final Acceleration MAX_DECELERATION = new Acceleration(-1E12, AccelerationUnit.SI);
66  
67      /**
68       * Minimum distance of an operational plan path; anything shorter will be truncated to 0. <br>
69       * If objects related to e.g. molecular movements are simulated using this code, a setter for this parameter will be needed.
70       */
71      private static final Length MINIMUM_CREDIBLE_PATH_LENGTH = new Length(0.001, LengthUnit.METER);
72  
73      /** Constructor. */
74      LaneOperationalPlanBuilder()
75      {
76          // class should not be instantiated
77      }
78  
79      /**
80       * Build a plan with a path and a given start speed to try to reach a provided end speed, exactly at the end of the curve.
81       * The acceleration (and deceleration) are capped by maxAcceleration and maxDeceleration. Therefore, there is no guarantee
82       * that the end speed is actually reached by this plan.
83       * @param gtu LaneBasedGTU; the GTU for debugging purposes
84       * @param distance Length; distance to drive for reaching the end speed
85       * @param startTime Time; the current time or a time in the future when the plan should start
86       * @param startSpeed Speed; the speed at the start of the path
87       * @param endSpeed Speed; the required end speed
88       * @param maxAcceleration Acceleration; the maximum acceleration that can be applied, provided as a POSITIVE number
89       * @param maxDeceleration Acceleration; the maximum deceleration that can be applied, provided as a NEGATIVE number
90       * @return the operational plan to accomplish the given end speed
91       * @throws OperationalPlanException when the plan cannot be generated, e.g. because of a path that is too short
92       * @throws OperationalPlanException when the length of the path and the calculated driven distance implied by the
93       *             constructed segment list differ more than a given threshold
94       * @throws OTSGeometryException in case the lanes are not connected or firstLanePosition is larger than the length of the
95       *             first lane
96       */
97      public static LaneBasedOperationalPlan buildGradualAccelerationPlan(final LaneBasedGTU gtu, final Length distance,
98              final Time startTime, final Speed startSpeed, final Speed endSpeed, final Acceleration maxAcceleration,
99              final Acceleration maxDeceleration) throws OperationalPlanException, OTSGeometryException
100     {
101         OTSLine3D path = createPathAlongCenterLine(gtu, distance);
102         Segment segment;
103         if (startSpeed.eq(endSpeed))
104         {
105             segment = new SpeedSegment(distance.divide(startSpeed));
106         }
107         else
108         {
109             try
110             {
111                 // t = 2x / (vt + v0); a = (vt - v0) / t
112                 Duration duration = distance.times(2.0).divide(endSpeed.plus(startSpeed));
113                 Acceleration acceleration = endSpeed.minus(startSpeed).divide(duration);
114                 if (acceleration.si < 0.0 && acceleration.lt(maxDeceleration))
115                 {
116                     acceleration = maxDeceleration;
117                     duration = new Duration(Solver.firstSolutionAfter(0, acceleration.si / 2, startSpeed.si, -distance.si),
118                             DurationUnit.SI);
119                 }
120                 if (acceleration.si > 0.0 && acceleration.gt(maxAcceleration))
121                 {
122                     acceleration = maxAcceleration;
123                     duration = new Duration(Solver.firstSolutionAfter(0, acceleration.si / 2, startSpeed.si, -distance.si),
124                             DurationUnit.SI);
125                 }
126                 segment = new OperationalPlan.AccelerationSegment(duration, acceleration);
127             }
128             catch (ValueRuntimeException ve)
129             {
130                 throw new OperationalPlanException(ve);
131             }
132         }
133         ArrayList<Segment> segmentList = new ArrayList<>();
134         segmentList.add(segment);
135         return new LaneBasedOperationalPlan(gtu, path, startTime, startSpeed, segmentList, false);
136     }
137 
138     /**
139      * Build a plan with a path and a given start speed to reach a provided end speed, exactly at the end of the curve.
140      * Acceleration and deceleration are virtually unbounded (1E12 m/s2) to reach the end speed (e.g., to come to a complete
141      * stop).
142      * @param gtu LaneBasedGTU; the GTU for debugging purposes
143      * @param distance Length; distance to drive for reaching the end speed
144      * @param startTime Time; the current time or a time in the future when the plan should start
145      * @param startSpeed Speed; the speed at the start of the path
146      * @param endSpeed Speed; the required end speed
147      * @return the operational plan to accomplish the given end speed
148      * @throws OperationalPlanException when the length of the path and the calculated driven distance implied by the
149      *             constructed segment list differ more than a given threshold
150      * @throws OTSGeometryException in case the lanes are not connected or firstLanePositiion is larger than the length of the
151      *             first lane
152      */
153     public static LaneBasedOperationalPlan buildGradualAccelerationPlan(final LaneBasedGTU gtu, final Length distance,
154             final Time startTime, final Speed startSpeed, final Speed endSpeed)
155             throws OperationalPlanException, OTSGeometryException
156     {
157         return buildGradualAccelerationPlan(gtu, distance, startTime, startSpeed, endSpeed, MAX_ACCELERATION, MAX_DECELERATION);
158     }
159 
160     /**
161      * Build a plan with a path and a given start speed to try to reach a provided end speed. Acceleration or deceleration is as
162      * provided, until the end speed is reached. After this, constant end speed is used to reach the end point of the path.
163      * There is no guarantee that the end speed is actually reached by this plan. If the end speed is zero, and it is reached
164      * before completing the path, a truncated path that ends where the GTU stops is used instead.
165      * @param gtu LaneBasedGTU; the GTU for debugging purposes
166      * @param distance Length; distance to drive for reaching the end speed
167      * @param startTime Time; the current time or a time in the future when the plan should start
168      * @param startSpeed Speed; the speed at the start of the path
169      * @param endSpeed Speed; the required end speed
170      * @param acceleration Acceleration; the acceleration to use if endSpeed &gt; startSpeed, provided as a POSITIVE number
171      * @param deceleration Acceleration; the deceleration to use if endSpeed &lt; startSpeed, provided as a NEGATIVE number
172      * @return the operational plan to accomplish the given end speed
173      * @throws OperationalPlanException when the construction of the operational path fails
174      * @throws OTSGeometryException in case the lanes are not connected or firstLanePositiion is larger than the length of the
175      *             first lane
176      */
177     public static LaneBasedOperationalPlan buildMaximumAccelerationPlan(final LaneBasedGTU gtu, final Length distance,
178             final Time startTime, final Speed startSpeed, final Speed endSpeed, final Acceleration acceleration,
179             final Acceleration deceleration) throws OperationalPlanException, OTSGeometryException
180     {
181         OTSLine3D path = createPathAlongCenterLine(gtu, distance);
182         ArrayList<Segment> segmentList = new ArrayList<>();
183         if (startSpeed.eq(endSpeed))
184         {
185             segmentList.add(new OperationalPlan.SpeedSegment(distance.divide(startSpeed)));
186         }
187         else
188         {
189             try
190             {
191                 if (endSpeed.gt(startSpeed))
192                 {
193                     Duration t = endSpeed.minus(startSpeed).divide(acceleration);
194                     Length x = startSpeed.times(t).plus(acceleration.times(0.5).times(t).times(t));
195                     if (x.ge(distance))
196                     {
197                         // we cannot reach the end speed in the given distance with the given acceleration
198                         Duration duration =
199                                 new Duration(Solver.firstSolutionAfter(0, acceleration.si / 2, startSpeed.si, -distance.si),
200                                         DurationUnit.SI);
201                         segmentList.add(new OperationalPlan.AccelerationSegment(duration, acceleration));
202                     }
203                     else
204                     {
205                         // we reach the (higher) end speed before the end of the segment. Make two segments.
206                         segmentList.add(new OperationalPlan.AccelerationSegment(t, acceleration));
207                         Duration duration = distance.minus(x).divide(endSpeed);
208                         segmentList.add(new OperationalPlan.SpeedSegment(duration));
209                     }
210                 }
211                 else
212                 {
213                     Duration t = endSpeed.minus(startSpeed).divide(deceleration);
214                     Length x = startSpeed.times(t).plus(deceleration.times(0.5).times(t).times(t));
215                     if (x.ge(distance))
216                     {
217                         // we cannot reach the end speed in the given distance with the given deceleration
218                         Duration duration =
219                                 new Duration(Solver.firstSolutionAfter(0, deceleration.si / 2, startSpeed.si, -distance.si),
220                                         DurationUnit.SI);
221                         segmentList.add(new OperationalPlan.AccelerationSegment(duration, deceleration));
222                     }
223                     else
224                     {
225                         if (endSpeed.si == 0.0)
226                         {
227                             // if endSpeed == 0, we cannot reach the end of the path. Therefore, build a partial path.
228                             OTSLine3D partialPath = path.truncate(x.si);
229                             segmentList.add(new OperationalPlan.AccelerationSegment(t, deceleration));
230                             return new LaneBasedOperationalPlan(gtu, partialPath, startTime, startSpeed, segmentList, false);
231                         }
232                         // we reach the (lower) end speed, larger than zero, before the end of the segment. Make two segments.
233                         segmentList.add(new OperationalPlan.AccelerationSegment(t, deceleration));
234                         Duration duration = distance.minus(x).divide(endSpeed);
235                         segmentList.add(new OperationalPlan.SpeedSegment(duration));
236                     }
237                 }
238             }
239             catch (ValueRuntimeException ve)
240             {
241                 throw new OperationalPlanException(ve);
242             }
243 
244         }
245         return new LaneBasedOperationalPlan(gtu, path, startTime, startSpeed, segmentList, false);
246     }
247 
248     /**
249      * Build a plan with a path and a given start speed to try to reach a provided end speed. Acceleration or deceleration is as
250      * provided, until the end speed is reached. After this, constant end speed is used to reach the end point of the path.
251      * There is no guarantee that the end speed is actually reached by this plan. If the end speed is zero, and it is reached
252      * before completing the path, a truncated path that ends where the GTU stops is used instead.
253      * @param gtu LaneBasedGTU; the GTU for debugging purposes
254      * @param startTime Time; the current time or a time in the future when the plan should start
255      * @param startSpeed Speed; the speed at the start of the path
256      * @param acceleration Acceleration; the acceleration to use
257      * @param timeStep Duration; time step for the plan
258      * @param deviative boolean; whether the plan is deviative
259      * @return the operational plan to accomplish the given end speed
260      * @throws OperationalPlanException when the construction of the operational path fails
261      * @throws OTSGeometryException in case the lanes are not connected or firstLanePositiion is larger than the length of the
262      *             first lane
263      */
264     public static LaneBasedOperationalPlan buildAccelerationPlan(final LaneBasedGTU gtu, final Time startTime,
265             final Speed startSpeed, final Acceleration acceleration, final Duration timeStep, final boolean deviative)
266             throws OperationalPlanException, OTSGeometryException
267     {
268         if (startSpeed.si <= OperationalPlan.DRIFTING_SPEED_SI && acceleration.le(Acceleration.ZERO))
269         {
270             return new LaneBasedOperationalPlan(gtu, gtu.getLocation(), startTime, timeStep, deviative);
271         }
272 
273         Duration brakingTime = brakingTime(acceleration, startSpeed, timeStep);
274         Length distance =
275                 Length.instantiateSI(startSpeed.si * brakingTime.si + .5 * acceleration.si * brakingTime.si * brakingTime.si);
276         List<Segment> segmentList = createAccelerationSegments(startSpeed, acceleration, brakingTime, timeStep);
277         if (distance.le(MINIMUM_CREDIBLE_PATH_LENGTH))
278         {
279             return new LaneBasedOperationalPlan(gtu, gtu.getLocation(), startTime, timeStep, deviative);
280         }
281         OTSLine3D path = createPathAlongCenterLine(gtu, distance);
282         return new LaneBasedOperationalPlan(gtu, path, startTime, startSpeed, segmentList, deviative);
283     }
284 
285     /**
286      * Creates a path along lane center lines.
287      * @param gtu LaneBasedGTU; gtu
288      * @param distance Length; minimum distance
289      * @return OTSLine3D; path along lane center lines
290      * @throws OTSGeometryException when any of the OTSLine3D operations fails
291      */
292     public static OTSLine3D createPathAlongCenterLine(final LaneBasedGTU gtu, final Length distance) throws OTSGeometryException
293     {
294         // if (gtu.getId().equals("1669") && gtu.getSimulator().getSimulatorTime().si >= 2508.9)
295         // {
296         // System.out.println("processing gtu " + gtu);
297         // try
298         // {
299         // for (Lane l : gtu.fractionalPositions(RelativePosition.REFERENCE_POSITION).keySet())
300         // {
301         // System.out.println("fractional position on lane " + l + ": "
302         // + gtu.fractionalPositions(RelativePosition.REFERENCE_POSITION).get(l));
303         // }
304         // System.out.println("reference position is " + gtu.getReferencePosition());
305         // System.out.println("operational plan path is " + gtu.getOperationalPlan().getPath());
306         // }
307         // catch (GTUException e)
308         // {
309         // e.printStackTrace();
310         // }
311         // }
312         OTSLine3D path = null;
313         try
314         {
315             DirectedLanePosition ref = gtu.getReferencePosition();
316             double f = ref.getLane().fraction(ref.getPosition());
317             if (ref.getGtuDirection().isPlus() && f < 1.0)
318             {
319                 if (f >= 0.0)
320                 {
321                     path = ref.getLane().getCenterLine().extractFractional(f, 1.0);
322                 }
323                 else
324                 {
325                     path = ref.getLane().getCenterLine().extractFractional(0.0, 1.0);
326                 }
327             }
328             else if (ref.getGtuDirection().isMinus() && f > 0.0)
329             {
330                 if (f <= 1.0)
331                 {
332                     path = ref.getLane().getCenterLine().extractFractional(0.0, f).reverse();
333                 }
334                 else
335                 {
336                     path = ref.getLane().getCenterLine().extractFractional(0.0, 1.0).reverse();
337                 }
338             }
339             // if (gtu.getId().equals("1669") && gtu.getSimulator().getSimulatorTime().si >= 2508.9)
340             // {
341             // System.out.println("First part of path is " + path);
342             // }
343             LaneDirection prevFrom = null;
344             LaneDirection from = ref.getLaneDirection();
345             int n = 1;
346             boolean alternativeTried = false;
347             while (path == null || path.getLength().si < distance.si + n * Lane.MARGIN.si)
348             {
349                 n++;
350                 prevFrom = from;
351                 if (null == from)
352                 {
353                     CategoryLogger.always().warn("About to die: GTU {} has null from value", gtu.getId());
354                 }
355                 from = from.getNextLaneDirection(gtu);
356                 if (from == null)
357                 {
358                     // check sink sensor
359                     Length pos = prevFrom.getDirection().isPlus() ? prevFrom.getLength() : Length.ZERO;
360                     for (SingleSensor sensor : prevFrom.getLane().getSensors(pos, pos, gtu.getGTUType(),
361                             prevFrom.getDirection()))
362                     {
363                         // XXX for now, the same is not done for the DestinationSensor (e.g., decrease speed for parking)
364                         if (sensor instanceof SinkSensor)
365                         {
366                             // just add some length so the GTU is happy to go to the sink
367                             DirectedPoint end = path.getLocationExtendedSI(distance.si + n * Lane.MARGIN.si);
368                             List<OTSPoint3D> points = new ArrayList<>(Arrays.asList(path.getPoints()));
369                             points.add(new OTSPoint3D(end));
370                             return new OTSLine3D(points);
371                         }
372                     }
373                     // START CLEVER
374                     /*-
375                     if (!alternativeTried)
376                     {
377                         for (Lane l : gtu.fractionalPositions(RelativePosition.REFERENCE_POSITION).keySet())
378                         {
379                             if (ref.getLane().equals(l))
380                             {
381                                 continue;
382                             }
383                             CategoryLogger.always().warn("GTU {} dead end on {}; but reference position is on {}; trying that",
384                                     gtu.getId(), ref, l);
385                             // Figure out the driving direction and position on Lane l
386                             // For now assume that lane l and ref are lanes on the same parent link. If not, chaos may occur
387                             if (!l.getParentLink().equals(ref.getLane().getParentLink()))
388                             {
389                                 CategoryLogger.always()
390                                         .error("Assumption that l and ref.getLane are on same Link does not hold");
391                             }
392                             from = new LaneDirection(l, ref.getGtuDirection());
393                             if (ref.getGtuDirection().isPlus() && f < 1.0)
394                             {
395                                 if (f >= 0.0)
396                                 {
397                                     path = l.getCenterLine().extractFractional(f, 1.0);
398                                 }
399                                 else
400                                 {
401                                     path = l.getCenterLine().extractFractional(0.0, 1.0);
402                                 }
403                             }
404                             else if (ref.getGtuDirection().isMinus() && f > 0.0)
405                             {
406                                 if (f <= 1.0)
407                                 {
408                                     path = l.getCenterLine().extractFractional(0.0, f).reverse();
409                                 }
410                                 else
411                                 {
412                                     path = l.getCenterLine().extractFractional(0.0, 1.0).reverse();
413                                 }
414                             }
415                             alternativeTried = true;
416                         }
417                         if (null != from)
418                         {
419                             continue;
420                         }
421                     }
422                     */
423                     // END CLEVER
424                     CategoryLogger.always().error("GTU {} has nowhere to go and no sink sensor either", gtu);
425                     // gtu.getReferencePosition(); // CLEVER
426                     gtu.destroy();
427                     return path;
428                 }
429                 if (path == null)
430                 {
431                     path = from.getDirection().isPlus() ? from.getLane().getCenterLine()
432                             : from.getLane().getCenterLine().reverse();
433                 }
434                 else
435                 {
436                     path = OTSLine3D.concatenate(Lane.MARGIN.si, path, from.getDirection().isPlus()
437                             ? from.getLane().getCenterLine() : from.getLane().getCenterLine().reverse());
438                 }
439             }
440         }
441         catch (GTUException exception)
442         {
443             throw new RuntimeException("Error during creation of path.", exception);
444         }
445         return path;
446     }
447 
448     /**
449      * Build a plan with a path and a given start speed to try to reach a provided end speed. Acceleration or deceleration is as
450      * provided, until the end speed is reached. After this, constant end speed is used to reach the end point of the path.
451      * There is no guarantee that the end speed is actually reached by this plan. If the end speed is zero, and it is reached
452      * before completing the path, a truncated path that ends where the GTU stops is used instead.
453      * @param gtu LaneBasedGTU; the GTU for debugging purposes
454      * @param laneChangeDirectionality LateralDirectionality; direction of lane change (on initiation only, after that not
455      *            important)
456      * @param startPosition DirectedPoint; current position
457      * @param startTime Time; the current time or a time in the future when the plan should start
458      * @param startSpeed Speed; the speed at the start of the path
459      * @param acceleration Acceleration; the acceleration to use
460      * @param timeStep Duration; time step for the plan
461      * @param laneChange LaneChange; lane change status
462      * @return the operational plan to accomplish the given end speed
463      * @throws OperationalPlanException when the construction of the operational path fails
464      * @throws OTSGeometryException in case the lanes are not connected or firstLanePositiion is larger than the length of the
465      *             first lane
466      */
467     @SuppressWarnings("checkstyle:parameternumber")
468     public static LaneBasedOperationalPlan buildAccelerationLaneChangePlan(final LaneBasedGTU gtu,
469             final LateralDirectionality laneChangeDirectionality, final DirectedPoint startPosition, final Time startTime,
470             final Speed startSpeed, final Acceleration acceleration, final Duration timeStep, final LaneChange laneChange)
471             throws OperationalPlanException, OTSGeometryException
472     {
473 
474         // on first call during lane change, use laneChangeDirectionality as laneChange.getDirection() is NONE
475         // on successive calls, use laneChange.getDirection() as laneChangeDirectionality is NONE (i.e. no LC initiated)
476         LateralDirectionality direction = laneChange.isChangingLane() ? laneChange.getDirection() : laneChangeDirectionality;
477 
478         Duration brakingTime = brakingTime(acceleration, startSpeed, timeStep);
479         Length planDistance =
480                 Length.instantiateSI(startSpeed.si * brakingTime.si + .5 * acceleration.si * brakingTime.si * brakingTime.si);
481         List<Segment> segmentList = createAccelerationSegments(startSpeed, acceleration, brakingTime, timeStep);
482 
483         try
484         {
485             // get position on from lane
486             Map<Lane, Length> positions = gtu.positions(gtu.getReference());
487             DirectedLanePosition ref = gtu.getReferencePosition();
488             Iterator<Lane> iterator = ref.getLane()
489                     .accessibleAdjacentLanesPhysical(direction, gtu.getGTUType(), ref.getGtuDirection()).iterator();
490             Lane adjLane = iterator.hasNext() ? iterator.next() : null;
491             DirectedLanePosition from = null;
492             if (laneChange.getDirection() == null || (adjLane != null && positions.containsKey(adjLane)))
493             {
494                 // reference lane is from lane, this is ok
495                 from = ref;
496             }
497             else
498             {
499                 // reference lane is to lane, this should be accounted for
500                 for (Lane lane : positions.keySet())
501                 {
502                     if (lane.accessibleAdjacentLanesPhysical(direction, gtu.getGTUType(), ref.getGtuDirection())
503                             .contains(ref.getLane()))
504                     {
505                         from = new DirectedLanePosition(lane, positions.get(lane), ref.getGtuDirection());
506                         break;
507                     }
508                 }
509             }
510             Throw.when(from == null, RuntimeException.class, "From lane could not be determined during lane change.");
511 
512             // get path and make plan
513             OTSLine3D path = laneChange.getPath(timeStep, gtu, from, startPosition, planDistance, direction);
514             LaneBasedOperationalPlan plan = new LaneBasedOperationalPlan(gtu, path, startTime, startSpeed, segmentList, true);
515             return plan;
516         }
517         catch (GTUException exception)
518         {
519             throw new RuntimeException("Error during creation of lane change plan.", exception);
520         }
521     }
522 
523     /**
524      * Returns the effective braking time, which stops if stand-still is reached.
525      * @param acceleration Acceleration; acceleration
526      * @param startSpeed Speed; start speed
527      * @param time Duration; intended time step
528      * @return Duration; effective braking time
529      */
530     public static Duration brakingTime(final Acceleration acceleration, final Speed startSpeed, final Duration time)
531     {
532         if (acceleration.ge0())
533         {
534             return time;
535         }
536         double t = startSpeed.si / -acceleration.si;
537         if (t >= time.si)
538         {
539             return time;
540         }
541         return Duration.instantiateSI(t);
542     }
543 
544     /**
545      * Creates 1 or 2 segments in an operational plan. Two segments are returned of stand-still is reached within the time step.
546      * @param startSpeed Speed; start speed
547      * @param acceleration Acceleration; acceleration
548      * @param brakingTime Duration; braking time until stand-still
549      * @param timeStep Duration; time step
550      * @return 1 or 2 segments in an operational plan
551      */
552     private static List<Segment> createAccelerationSegments(final Speed startSpeed, final Acceleration acceleration,
553             final Duration brakingTime, final Duration timeStep)
554     {
555         List<Segment> segmentList = new ArrayList<>();
556         if (brakingTime.si < timeStep.si)
557         {
558             if (brakingTime.si > 0.0)
559             {
560                 segmentList.add(new OperationalPlan.AccelerationSegment(brakingTime, acceleration));
561             }
562             segmentList.add(new OperationalPlan.SpeedSegment(timeStep.minus(brakingTime)));
563         }
564         else
565         {
566             segmentList.add(new OperationalPlan.AccelerationSegment(timeStep, acceleration));
567         }
568         return segmentList;
569     }
570 
571     /**
572      * Build an operational plan based on a simple operational plan and status info.
573      * @param gtu LaneBasedGTU; gtu
574      * @param startTime Time; start time for plan
575      * @param simplePlan SimpleOperationalPlan; simple operational plan
576      * @param laneChange LaneChange; lane change status
577      * @return operational plan
578      * @throws ParameterException if parameter is not defined
579      * @throws GTUException gtu exception
580      * @throws NetworkException network exception
581      * @throws OperationalPlanException operational plan exeption
582      */
583     public static LaneBasedOperationalPlan buildPlanFromSimplePlan(final LaneBasedGTU gtu, final Time startTime,
584             final SimpleOperationalPlan simplePlan, final LaneChange laneChange)
585             throws ParameterException, GTUException, NetworkException, OperationalPlanException
586     {
587         Acceleration acc = gtu.getVehicleModel().boundAcceleration(simplePlan.getAcceleration(), gtu);
588 
589         if (gtu.isInstantaneousLaneChange())
590         {
591             if (simplePlan.isLaneChange())
592             {
593                 gtu.changeLaneInstantaneously(simplePlan.getLaneChangeDirection());
594             }
595             try
596             {
597                 return LaneOperationalPlanBuilder.buildAccelerationPlan(gtu, startTime, gtu.getSpeed(), acc,
598                         simplePlan.getDuration(), false);
599             }
600             catch (OTSGeometryException exception)
601             {
602                 throw new OperationalPlanException(exception);
603             }
604         }
605 
606         // gradual lane change
607         try
608         {
609             if (!simplePlan.isLaneChange() && !laneChange.isChangingLane())
610             {
611                 return LaneOperationalPlanBuilder.buildAccelerationPlan(gtu, startTime, gtu.getSpeed(), acc,
612                         simplePlan.getDuration(), true);
613             }
614             if (gtu.getSpeed().si == 0.0 && acc.si <= 0.0)
615             {
616                 return LaneOperationalPlanBuilder.buildAccelerationPlan(gtu, startTime, gtu.getSpeed(), acc,
617                         simplePlan.getDuration(), false);
618             }
619             return LaneOperationalPlanBuilder.buildAccelerationLaneChangePlan(gtu, simplePlan.getLaneChangeDirection(),
620                     gtu.getLocation(), startTime, gtu.getSpeed(), acc, simplePlan.getDuration(), laneChange);
621         }
622         catch (OTSGeometryException exception)
623         {
624             throw new OperationalPlanException(exception);
625         }
626     }
627 
628     /**
629      * Schedules a lane change finalization after the given distance is covered. This distance is known as the plan is created,
630      * but at that point no time can be derived as the plan is required for that. Hence, this method can be scheduled at the
631      * same time (sequentially after creation of the plan) to then schedule the actual finalization by deriving time from
632      * distance with the plan.
633      * @param gtu LaneBasedGTU; gtu
634      * @param distance Length; distance
635      * @param laneChangeDirection LateralDirectionality; lane change direction
636      * @throws SimRuntimeException on bad time
637      */
638     public static void scheduleLaneChangeFinalization(final LaneBasedGTU gtu, final Length distance,
639             final LateralDirectionality laneChangeDirection) throws SimRuntimeException
640     {
641         Time time = gtu.getOperationalPlan().timeAtDistance(distance);
642         if (Double.isNaN(time.si))
643         {
644             // rounding...
645             time = gtu.getOperationalPlan().getEndTime();
646         }
647         SimEventInterface<Duration> event = gtu.getSimulator().scheduleEventAbsTime(time, (short) 6, gtu, gtu,
648                 "finalizeLaneChange", new Object[] { laneChangeDirection });
649         gtu.setFinalizeLaneChangeEvent(event);
650     }
651 
652     /**
653      * Build a plan with a path and a given start speed to try to come to a stop with a given deceleration. If the GTU can stop
654      * before completing the given path, a truncated path that ends where the GTU stops is used instead. There is no guarantee
655      * that the OperationalPlan will lead to a complete stop.
656      * @param gtu LaneBasedGTU; the GTU for debugging purposes
657      * @param distance Length; distance to drive for reaching the end speed
658      * @param startTime Time; the current time or a time in the future when the plan should start
659      * @param startSpeed Speed; the speed at the start of the path
660      * @param deceleration Acceleration; the deceleration to use if endSpeed &lt; startSpeed, provided as a NEGATIVE number
661      * @return the operational plan to accomplish the given end speed
662      * @throws OperationalPlanException when construction of the operational path fails
663      * @throws OTSGeometryException in case the lanes are not connected or firstLanePositiion is larger than the length of the
664      *             first lane
665      */
666     public static LaneBasedOperationalPlan buildStopPlan(final LaneBasedGTU gtu, final Length distance, final Time startTime,
667             final Speed startSpeed, final Acceleration deceleration) throws OperationalPlanException, OTSGeometryException
668     {
669         return buildMaximumAccelerationPlan(gtu, distance, startTime, startSpeed, new Speed(0.0, SpeedUnit.SI),
670                 new Acceleration(1.0, AccelerationUnit.SI), deceleration);
671     }
672 
673 }