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