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