1 package org.opentrafficsim.core.gtu.plan.operational;
2
3 import java.io.Serializable;
4 import java.util.ArrayList;
5 import java.util.Arrays;
6 import java.util.Iterator;
7 import java.util.List;
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.unit.TimeUnit;
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.opentrafficsim.core.geometry.OTSGeometryException;
20 import org.opentrafficsim.core.geometry.OTSLine3D;
21 import org.opentrafficsim.core.geometry.OTSPoint3D;
22 import org.opentrafficsim.core.gtu.GTU;
23 import org.opentrafficsim.core.gtu.RelativePosition;
24 import org.opentrafficsim.core.logger.SimLogger;
25 import org.opentrafficsim.core.math.Solver;
26
27 import nl.tudelft.simulation.language.Throw;
28 import nl.tudelft.simulation.language.d3.DirectedPoint;
29
30 /**
31 * An Operational plan describes a path through the world with a speed profile that a GTU intends to follow. The OperationalPlan
32 * can be updated or replaced at any time (including before it has been totally executed), for which a tactical planner is
33 * responsible. The operational plan is implemented using segments of the movement (time, location, speed, acceleration) that
34 * the GTU will use to plan its location and movement. Within an OperationalPlan the GTU cannot reverse direction along the path
35 * of movement. This ensures that the timeAtDistance method will never have to select among several valid solutions.
36 * <p>
37 * Copyright (c) 2013-2018 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
38 * BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
39 * </p>
40 * $LastChangedDate: 2015-07-24 02:58:59 +0200 (Fri, 24 Jul 2015) $, @version $Revision: 1147 $, by $Author: averbraeck $,
41 * initial version Nov 14, 2015 <br>
42 * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
43 * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
44 */
45 public class OperationalPlan implements Serializable
46 {
47 /** */
48 private static final long serialVersionUID = 20151114L;
49
50 /** The path to follow from a certain time till a certain time. */
51 private final OTSLine3D path;
52
53 /** The absolute start time when we start executing the path. */
54 private final Time startTime;
55
56 /** The GTU speed when we start executing the path. */
57 private final Speed startSpeed;
58
59 /** The segments that make up the path with an acceleration, constant speed or deceleration profile. */
60 private final List<OperationalPlan.Segment> operationalPlanSegmentList;
61
62 /** The duration of executing the entire operational plan. */
63 private final Duration totalDuration;
64
65 /** The length of the entire operational plan. */
66 private final Length totalLength;
67
68 /** The speed at the end of the operational plan. */
69 private final Speed endSpeed;
70
71 /** Is this operational plan a wait plan? */
72 private final boolean waitPlan;
73
74 /** GTU for debugging purposes. */
75 private final GTU gtu;
76
77 /**
78 * An array of relative start times of each segment, expressed in the SI unit, where the last time is the overall ending
79 * time of the operational plan.
80 */
81 private final double[] segmentStartTimesRelSI;
82
83 /**
84 * The maximum difference in the length of the path and the calculated driven distance implied by the segment list. The same
85 * constant is also used as a maximum between speeds of segments that should align in terms of speed.
86 */
87 static final double MAX_DELTA_SI = 1E-6;
88
89 /** The drifting speed. Speeds under this value will be cropped to zero. */
90 public static final double DRIFTING_SPEED_SI = 1E-3;
91
92 /**
93 * Construct an operational plan.
94 * @param gtu the GTU for debugging purposes
95 * @param path the path to follow from a certain time till a certain time. The path should have <i>at least</i> the length
96 * @param startTime the absolute start time when we start executing the path
97 * @param startSpeed the GTU speed when we start executing the path
98 * @param operationalPlanSegmentList the segments that make up the path with an acceleration, constant speed or deceleration
99 * profile
100 * @throws OperationalPlanException when the path is too short for the operation
101 */
102 public OperationalPlan(final GTU gtu, final OTSLine3D path, final Time startTime, final Speed startSpeed,
103 final List<Segment> operationalPlanSegmentList) throws OperationalPlanException
104 {
105
106 this.waitPlan = false;
107 this.gtu = gtu;
108 this.startTime = startTime;
109 this.startSpeed = startSpeed;
110 this.operationalPlanSegmentList = operationalPlanSegmentList;
111 this.segmentStartTimesRelSI = new double[this.operationalPlanSegmentList.size() + 1];
112
113 // check the driven distance of the segments
114 Speed v0 = this.startSpeed;
115 double distanceSI = 0.0;
116 double durationSI = 0.0;
117 for (int i = 0; i < this.operationalPlanSegmentList.size(); i++)
118 {
119 Segment segment = this.operationalPlanSegmentList.get(i);
120 if (Math.abs(v0.si) < DRIFTING_SPEED_SI && segment.accelerationSI(0.0) == 0.0)
121 {
122 v0 = Speed.ZERO;
123 }
124 segment.setV0(v0);
125 this.segmentStartTimesRelSI[i] = durationSI;
126 distanceSI += segment.distanceSI();
127 v0 = segment.endSpeed();
128 durationSI += segment.getDuration().si;
129 }
130 this.segmentStartTimesRelSI[this.segmentStartTimesRelSI.length - 1] = durationSI;
131 try
132 {
133 this.path = path.extract(0.0, Math.min(distanceSI, path.getLengthSI()));
134 }
135 catch (OTSGeometryException exception)
136 {
137 throw new OperationalPlanException(exception);
138 }
139 this.totalDuration = new Duration(durationSI, DurationUnit.SI);
140 this.totalLength = new Length(distanceSI, LengthUnit.SI);
141 this.endSpeed = v0;
142
143 // double pathDistanceDeviation = Math.abs(this.totalLength.si - this.path.getLengthSI()) / this.totalLength.si;
144 // if (pathDistanceDeviation < -0.01 || pathDistanceDeviation > 0.01)
145 // {
146 // System.err.println("path length and driven distance deviate more than 1% for operationalPlan: " + this);
147 // }
148 }
149
150 /**
151 * Build a plan where the GTU will wait for a certain time.
152 * @param gtu the GTU for debugging purposes
153 * @param waitPoint the point at which the GTU will wait
154 * @param startTime the current time or a time in the future when the plan should start
155 * @param duration the waiting time
156 * @throws OperationalPlanException when construction of a waiting path fails
157 */
158 public OperationalPlan(final GTU gtu, final DirectedPoint waitPoint, final Time startTime, final Duration duration)
159 throws OperationalPlanException
160 {
161 this.waitPlan = true;
162 this.gtu = gtu;
163 this.startTime = startTime;
164 this.startSpeed = Speed.ZERO;
165 this.endSpeed = Speed.ZERO;
166 this.totalDuration = duration;
167 this.totalLength = Length.ZERO;
168
169 // make a path
170 OTSPoint3D p2 = new OTSPoint3D(waitPoint.x + Math.cos(waitPoint.getRotZ()), waitPoint.y + Math.sin(waitPoint.getRotZ()),
171 waitPoint.z);
172 try
173 {
174 this.path = new OTSLine3D(new OTSPoint3D(waitPoint), p2);
175 }
176 catch (OTSGeometryException exception)
177 {
178 throw new OperationalPlanException(exception);
179 }
180
181 this.operationalPlanSegmentList = new ArrayList<>();
182 Segment segment = new SpeedSegment(duration);
183 segment.setV0(Speed.ZERO);
184 this.operationalPlanSegmentList.add(segment);
185 this.segmentStartTimesRelSI = new double[2];
186 this.segmentStartTimesRelSI[0] = 0.0;
187 this.segmentStartTimesRelSI[1] = duration.si;
188 }
189
190 /**
191 * Return the path that will be traveled. If the plan is a wait plan, the start point of the path is good; the end point of
192 * the path is bogus (should only be used to determine the orientation of the GTU).
193 * @return the path
194 */
195 public final OTSLine3D getPath()
196 {
197 return this.path;
198 }
199
200 /**
201 * Return the (absolute) start time of the operational plan.
202 * @return startTime
203 */
204 public final Time getStartTime()
205 {
206 return this.startTime;
207 }
208
209 /**
210 * Return the start speed of the entire plan.
211 * @return startSpeed
212 */
213 public final Speed getStartSpeed()
214 {
215 return this.startSpeed;
216 }
217
218 /**
219 * @return the end speed when completing the entire plan.
220 */
221 public final Speed getEndSpeed()
222 {
223 return this.endSpeed;
224 }
225
226 /**
227 * Return the segments (parts with constant speed, acceleration or deceleration) of the operational plan. <br>
228 * The caller MUST NOT MODIFY the returned object.
229 * @return operationalPlanSegmentList
230 */
231 public final List<OperationalPlan.Segment> getOperationalPlanSegmentList()
232 {
233 return this.operationalPlanSegmentList;
234 }
235
236 /**
237 * Return the time it will take to complete the entire operational plan.
238 * @return the time it will take to complete the entire operational plan
239 */
240 public final Duration getTotalDuration()
241 {
242 return this.totalDuration;
243 }
244
245 /**
246 * Return the distance the entire operational plan will cover.
247 * @return the distance of the entire operational plan
248 */
249 public final Length getTotalLength()
250 {
251 return this.totalLength;
252 }
253
254 /**
255 * Return the time it will take to complete the entire operational plan.
256 * @return the time it will take to complete the entire operational plan
257 */
258 public final Time getEndTime()
259 {
260 return this.startTime.plus(this.totalDuration);
261 }
262
263 /**
264 * Provide the end location of this operational plan as a DirectedPoint.
265 * @return the end location
266 */
267 public final DirectedPoint getEndLocation()
268 {
269 try
270 {
271 if (this.waitPlan)
272 {
273 return this.path.getLocationFraction(0.0); // no move...
274 }
275 else
276 {
277 return this.path.getLocationFraction(1.0);
278 }
279 }
280 catch (OTSGeometryException exception)
281 {
282 // should not happen -- only for fractions less than 0.0 or larger than 1.0.
283 throw new RuntimeException(exception);
284 }
285 }
286
287 /**
288 * Store a Segment and the progress within that segment in one Object.
289 * <p>
290 * Copyright (c) 2013-2018 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. <br>
291 * All rights reserved. <br>
292 * BSD-style license. See <a href="http://opentrafficsim.org/docs/current/license.html">OpenTrafficSim License</a>.
293 * <p>
294 * @version $Revision$, $LastChangedDate$, by $Author$, initial version Dec 16, 2015 <br>
295 * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
296 * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
297 */
298 private class SegmentProgress implements Serializable
299 {
300 /** */
301 private static final long serialVersionUID = 20160000L;
302
303 /** Active Segment. */
304 private final Segment segment;
305
306 /** Start time of the Segment. */
307 private final Time segmentStartTime;
308
309 /** Position on the path of the plan. */
310 private final Length segmentStartPosition;
311
312 /**
313 * Construct a new SegmentProgress object.
314 * @param segment Segment; the Segment
315 * @param segmentStartTime Time; the start time of the Segment
316 * @param segmentStartPosition Length; the position of the start of the segment on the path of the OperationalPlan
317 */
318 SegmentProgress(final Segment segment, final Time segmentStartTime, final Length segmentStartPosition)
319 {
320 this.segment = segment;
321 this.segmentStartTime = segmentStartTime;
322 this.segmentStartPosition = segmentStartPosition;
323 }
324
325 /**
326 * Retrieve the Segment.
327 * @return Segment
328 */
329 public final Segment getSegment()
330 {
331 return this.segment;
332 }
333
334 /**
335 * Retrieve the start time of the Segment.
336 * @return Time; the start time of the Segment
337 */
338 public final Time getSegmentStartTime()
339 {
340 return this.segmentStartTime;
341 }
342
343 /**
344 * Retrieve the fractionalPosition at the start of the Segment.
345 * @return double; the fractional position at the start of the Segment
346 */
347 public final Length getSegmentStartPosition()
348 {
349 return this.segmentStartPosition;
350 }
351
352 /** {@inheritDoc} */
353 @Override
354 public final String toString()
355 {
356 return String.format("SegmentProgress segment=%s startpos.rel=%s starttime.abs=%s", this.segment,
357 this.segmentStartPosition, this.segmentStartTime);
358 }
359 }
360
361 /**
362 * Find the Segment and the progress within that Segment at a specified time.
363 * @param time Time; the time
364 * @return SegmentProgress; the Segment and progress within that segment, or null when no Segment applies to the specified
365 * time
366 * @throws OperationalPlanException when SegmentProgress cannot be determined
367 */
368 private SegmentProgress getSegmentProgress(final Time time) throws OperationalPlanException
369 {
370 if (time.lt(this.startTime))
371 {
372 throw new OperationalPlanException(
373 this.gtu + ", t = " + time + "SegmentProgress cannot be determined for time before startTime "
374 + getStartTime() + " of this OperationalPlan");
375 }
376 double cumulativeDistance = 0;
377 for (int i = 0; i < this.segmentStartTimesRelSI.length - 1; i++)
378 {
379 if (this.startTime.si + this.segmentStartTimesRelSI[i + 1] >= time.si)
380 {
381 return new SegmentProgress(this.operationalPlanSegmentList.get(i),
382 new Time(this.startTime.si + this.segmentStartTimesRelSI[i], TimeUnit.BASE),
383 new Length(cumulativeDistance, LengthUnit.SI));
384 }
385 cumulativeDistance += this.operationalPlanSegmentList.get(i).distanceSI();
386 }
387 throw new OperationalPlanException(this.gtu + ", t = " + time
388 + " SegmentProgress cannot be determined for time after endTime " + getEndTime() + " of this OperationalPlan");
389 }
390
391 /**
392 * Return the time when the GTU will reach the given distance.
393 * @param distance the distance to calculate the time for
394 * @return the time it will take to have traveled the given distance
395 */
396 public final Time timeAtDistance(final Length distance)
397 {
398 Throw.when(getTotalLength().lt(distance), RuntimeException.class, "Requesting %s from a plan with length %s", distance,
399 getTotalLength());
400 double remainingDistanceSI = distance.si;
401 double timeAtStartOfSegment = this.startTime.si;
402 Iterator<Segment> it = this.operationalPlanSegmentList.iterator();
403 while (it.hasNext() && remainingDistanceSI >= 0.0)
404 {
405 Segment segment = it.next();
406 double distanceOfSegment = segment.distanceSI();
407 if (distanceOfSegment > remainingDistanceSI)
408 {
409 return new Time(timeAtStartOfSegment + segment.timeAtDistance(Length.createSI(remainingDistanceSI)).si,
410 TimeUnit.BASE);
411 }
412 remainingDistanceSI -= distanceOfSegment;
413 timeAtStartOfSegment += segment.getDurationSI();
414 }
415 return new Time(Double.NaN, TimeUnit.BASE);
416 }
417
418 /**
419 * Calculate the location at the given time.
420 * @param time the absolute time to look for a location
421 * @return the location at the given time.
422 * @throws OperationalPlanException when the time is after the validity of the operational plan
423 */
424 public final DirectedPoint getLocation(final Time time) throws OperationalPlanException
425 {
426 SegmentProgress sp = getSegmentProgress(time);
427 Segment segment = sp.getSegment();
428 Duration deltaT = time.minus(sp.getSegmentStartTime());
429 double distanceTraveledInSegment = segment.distanceSI(deltaT.si);
430 double startDistance = sp.getSegmentStartPosition().si;
431 double fraction = (startDistance + distanceTraveledInSegment) / this.path.getLengthSI();
432 DirectedPoint p = new DirectedPoint();
433 try
434 {
435 p = this.path.getLocationFraction(fraction, 0.01);
436 }
437 catch (OTSGeometryException exception)
438 {
439 SimLogger.always().error("OperationalPlan.getLocation(): " + exception.getMessage());
440 p = this.path.getLocationFractionExtended(fraction);
441 }
442 p.setZ(p.getZ() + 0.001);
443 return p;
444 }
445
446 /**
447 * Calculate the speed of the GTU after the given duration since the start of the plan.
448 * @param time the relative time to look for a location
449 * @return the location after the given duration since the start of the plan.
450 * @throws OperationalPlanException when the time is after the validity of the operational plan
451 */
452 public final Speed getSpeed(final Duration time) throws OperationalPlanException
453 {
454 return getSpeed(time.plus(this.startTime));
455 }
456
457 /**
458 * Calculate the speed of the GTU at the given time.
459 * @param time the absolute time to look for a location
460 * @return the location at the given time.
461 * @throws OperationalPlanException when the time is after the validity of the operational plan
462 */
463 public final Speed getSpeed(final Time time) throws OperationalPlanException
464 {
465 SegmentProgress sp = getSegmentProgress(time);
466 return new Speed(sp.getSegment().speedSI(time.minus(sp.getSegmentStartTime()).si), SpeedUnit.SI);
467 }
468
469 /**
470 * Calculate the acceleration of the GTU after the given duration since the start of the plan.
471 * @param time the relative time to look for a location
472 * @return the location after the given duration since the start of the plan.
473 * @throws OperationalPlanException when the time is after the validity of the operational plan
474 */
475 public final Acceleration getAcceleration(final Duration time) throws OperationalPlanException
476 {
477 return getAcceleration(time.plus(this.startTime));
478 }
479
480 /**
481 * Calculate the acceleration of the GTU at the given time.
482 * @param time the absolute time to look for a location
483 * @return the location at the given time.
484 * @throws OperationalPlanException when the time is after the validity of the operational plan
485 */
486 public final Acceleration getAcceleration(final Time time) throws OperationalPlanException
487 {
488 SegmentProgress sp = getSegmentProgress(time);
489 return new Acceleration(sp.getSegment().accelerationSI(time.minus(sp.getSegmentStartTime()).si), AccelerationUnit.SI);
490 }
491
492 /**
493 * Calculate the location after the given duration since the start of the plan.
494 * @param time the relative time to look for a location
495 * @return the location after the given duration since the start of the plan.
496 * @throws OperationalPlanException when the time is after the validity of the operational plan
497 */
498 public final DirectedPoint getLocation(final Duration time) throws OperationalPlanException
499 {
500 double distanceSI = getTraveledDistanceSI(time);
501 return this.path.getLocationExtendedSI(distanceSI);
502 }
503
504 /**
505 * Calculate the location after the given duration since the start of the plan.
506 * @param time the relative time to look for a location
507 * @param pos relative position
508 * @return the location after the given duration since the start of the plan.
509 * @throws OperationalPlanException when the time is after the validity of the operational plan
510 */
511 public final DirectedPoint getLocation(final Time time, final RelativePosition pos) throws OperationalPlanException
512 {
513 double distanceSI = getTraveledDistanceSI(time) + pos.getDx().si;
514 return this.path.getLocationExtendedSI(distanceSI);
515 }
516
517 /**
518 * Calculate the distance traveled as part of this plan after the given duration since the start of the plan. This method
519 * returns the traveled distance as a double in SI units.
520 * @param duration the relative time to calculate the traveled distance
521 * @return the distance traveled as part of this plan after the given duration since the start of the plan.
522 * @throws OperationalPlanException when the time is after the validity of the operational plan
523 */
524 public final double getTraveledDistanceSI(final Duration duration) throws OperationalPlanException
525 {
526 return getTraveledDistanceSI(this.startTime.plus(duration));
527 }
528
529 /**
530 * Calculate the distance traveled as part of this plan after the given duration since the start of the plan.
531 * @param duration the relative time to calculate the traveled distance
532 * @return the distance traveled as part of this plan after the given duration since the start of the plan.
533 * @throws OperationalPlanException when the time is after the validity of the operational plan
534 */
535 public final Length getTraveledDistance(final Duration duration) throws OperationalPlanException
536 {
537 return new Length(getTraveledDistanceSI(duration), LengthUnit.SI);
538 }
539
540 /**
541 * Calculate the distance traveled as part of this plan at the given absolute time. This method returns the traveled
542 * distance as a double in SI units.
543 * @param time the absolute time to calculate the traveled distance for as part of this plan
544 * @return the distance traveled as part of this plan at the given time
545 * @throws OperationalPlanException when the time is after the validity of the operational plan
546 */
547 public final double getTraveledDistanceSI(final Time time) throws OperationalPlanException
548 {
549 Throw.when(time.lt(this.getStartTime()), OperationalPlanException.class,
550 "getTravelDistance exception: requested traveled distance before start of plan");
551 Throw.when(time.gt(this.getEndTime()), OperationalPlanException.class,
552 "getTravelDistance exception: requested traveled distance beyond end of plan");
553 if (this.operationalPlanSegmentList.size() == 1)
554 {
555 return this.operationalPlanSegmentList.get(0).distanceSI(time.si - this.startTime.si);
556 }
557 SegmentProgress sp = getSegmentProgress(time);
558 return sp.getSegmentStartPosition().si + sp.getSegment().distanceSI(time.minus(sp.getSegmentStartTime()).si);
559 }
560
561 /**
562 * Calculates when the GTU will be at the given point. The point does not need to be at the traveled path, as the point is
563 * projected to the path at 90 degrees. The point may for instance be the end of a lane, which is crossed by a GTU possibly
564 * during a lane change.
565 * @param point DirectedPoint; point with angle, which will be projected to the path at 90 degrees
566 * @param upstream boolean; true if the point is upstream of the path
567 * @return Time; time at point
568 */
569 public final Time timeAtPoint(final DirectedPoint point, final boolean upstream)
570 {
571 OTSPoint3D p1 = new OTSPoint3D(point);
572 // point at 90 degrees
573 OTSPoint3D p2 = new OTSPoint3D(point.x - Math.sin(point.getRotZ()), point.y + Math.cos(point.getRotZ()), point.z);
574 double traveledDistanceAlongPath = 0.0;
575 try
576 {
577 if (upstream)
578 {
579 OTSPoint3D p = OTSPoint3D.intersectionOfLines(this.path.get(0), this.path.get(1), p1, p2);
580 double dist = traveledDistanceAlongPath - this.path.get(0).distance(p).si;
581 dist = dist >= 0.0 ? dist : 0.0; // negative in case of a gap
582 return timeAtDistance(Length.createSI(dist));
583 }
584 for (int i = 0; i < this.path.size() - 1; i++)
585 {
586 OTSPoint3D prevPoint = this.path.get(i);
587 OTSPoint3D nextPoint = this.path.get(i + 1);
588 OTSPoint3D p = OTSPoint3D.intersectionOfLines(prevPoint, nextPoint, p1, p2);
589 if (p == null)
590 {
591 // point too close, check next section
592 continue;
593 }
594 boolean onSegment =
595 prevPoint.distanceSI(nextPoint) + 2e-5 > Math.max(prevPoint.distanceSI(p), nextPoint.distanceSI(p));
596 // (prevPoint.x - p.x) * (nextPoint.x - p.x) <= 2e-5/* PK 1e-6 is too small */
597 // && (prevPoint.y - p.y) * (nextPoint.y - p.y) <= 2e-5/* PK 1e-6 is too small */;
598 // if (i > 64)
599 // {
600 // System.err.println(String.format("i=%d, prevPoint=%s, nextPoint=%s, intersection=%s, onSegment=%s", i,
601 // prevPoint, nextPoint, p, onSegment));
602 // }
603 if (p != null // on segment, or last segment
604 && (i == this.path.size() - 2 || onSegment))
605 {
606 // point is on the line
607 traveledDistanceAlongPath += this.path.get(i).distance(p).si;
608 if (traveledDistanceAlongPath > this.path.getLengthSI())
609 {
610 return Time.createSI(getEndTime().si - 1e-9); // -13-9 prevents that next move() reschedules enter
611 }
612 return timeAtDistance(Length.createSI(traveledDistanceAlongPath));
613 }
614 else
615 {
616 traveledDistanceAlongPath += this.path.get(i).distance(this.path.get(i + 1)).si;
617 }
618 }
619 }
620 catch (OTSGeometryException exception)
621 {
622 throw new RuntimeException("Index out of bounds on projection of point to path of operational plan", exception);
623 }
624 SimLogger.always().error("timeAtPoint failed");
625 return null;
626 }
627
628 /**
629 * Calculate the distance traveled as part of this plan at the given absolute time.
630 * @param time the absolute time to calculate the traveled distance for as part of this plan
631 * @return the distance traveled as part of this plan at the given time
632 * @throws OperationalPlanException when the time is after the validity of the operational plan
633 */
634 public final Length getTraveledDistance(final Time time) throws OperationalPlanException
635 {
636 return new Length(getTraveledDistanceSI(time.minus(this.startTime)), LengthUnit.SI);
637 }
638
639 /** {@inheritDoc} */
640 @SuppressWarnings("checkstyle:designforextension")
641 @Override
642 public int hashCode()
643 {
644 final int prime = 31;
645 int result = 1;
646 result = prime * result + ((this.operationalPlanSegmentList == null) ? 0 : this.operationalPlanSegmentList.hashCode());
647 result = prime * result + ((this.path == null) ? 0 : this.path.hashCode());
648 result = prime * result + ((this.startSpeed == null) ? 0 : this.startSpeed.hashCode());
649 result = prime * result + ((this.startTime == null) ? 0 : this.startTime.hashCode());
650 return result;
651 }
652
653 /** {@inheritDoc} */
654 @SuppressWarnings({ "checkstyle:needbraces", "checkstyle:designforextension" })
655 @Override
656 public boolean equals(final Object obj)
657 {
658 if (this == obj)
659 return true;
660 if (obj == null)
661 return false;
662 if (getClass() != obj.getClass())
663 return false;
664 OperationalPlan other = (OperationalPlan) obj;
665 if (this.operationalPlanSegmentList == null)
666 {
667 if (other.operationalPlanSegmentList != null)
668 return false;
669 }
670 else if (!this.operationalPlanSegmentList.equals(other.operationalPlanSegmentList))
671 return false;
672 if (this.path == null)
673 {
674 if (other.path != null)
675 return false;
676 }
677 else if (!this.path.equals(other.path))
678 return false;
679 if (this.startSpeed == null)
680 {
681 if (other.startSpeed != null)
682 return false;
683 }
684 else if (!this.startSpeed.equals(other.startSpeed))
685 return false;
686 if (this.startTime == null)
687 {
688 if (other.startTime != null)
689 return false;
690 }
691 else if (!this.startTime.equals(other.startTime))
692 return false;
693 return true;
694 }
695
696 /** {@inheritDoc} */
697 @SuppressWarnings("checkstyle:designforextension")
698 @Override
699 public String toString()
700 {
701 return "OperationalPlan [path=" + this.path + ", startTime=" + this.startTime + ", startSpeed=" + this.startSpeed
702 + ", operationalPlanSegmentList=" + this.operationalPlanSegmentList + ", totalDuration=" + this.totalDuration
703 + ", segmentStartTimesSI=" + Arrays.toString(this.segmentStartTimesRelSI) + ", endSpeed = " + this.endSpeed
704 + "]";
705 }
706
707 /****************************************************************************************************************/
708 /******************************************** SEGMENT DEFINITIONS ***********************************************/
709 /****************************************************************************************************************/
710
711 /**
712 * The segment of an operational plan contains a part of the speed profile of a movement in which some of the variables
713 * determining movement (speed, acceleration) are constant.
714 * <p>
715 * Copyright (c) 2013-2018 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. <br>
716 * All rights reserved. <br>
717 * BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
718 * </p>
719 * $LastChangedDate: 2015-07-24 02:58:59 +0200 (Fri, 24 Jul 2015) $, @version $Revision: 1147 $, by $Author: averbraeck $,
720 * initial version Nov 14, 2015 <br>
721 * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
722 * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
723 */
724 public abstract static class Segment implements Serializable
725 {
726 /** */
727 private static final long serialVersionUID = 20151114L;
728
729 /** The duration of the acceleration or speed for this segment. */
730 @SuppressWarnings("checkstyle:visibilitymodifier")
731 protected final Duration duration;
732
733 /** The initial speed for this segment. */
734 @SuppressWarnings("checkstyle:visibilitymodifier")
735 protected Speed v0;
736
737 /**
738 * @param duration the duration of the acceleration or speed for this segment
739 */
740 public Segment(final Duration duration)
741 {
742 super();
743 this.duration = duration;
744 }
745
746 /**
747 * @param v0 the initial speed of this segment; called from the Operational Plan constructor.
748 */
749 final void setV0(final Speed v0)
750 {
751 this.v0 = v0;
752 }
753
754 /**
755 * @return duration the duration of the acceleration or speed for this segment
756 */
757 public final Duration getDuration()
758 {
759 return this.duration;
760 }
761
762 /**
763 * @return duration the duration of the acceleration or speed for this segment
764 */
765 public final double getDurationSI()
766 {
767 return this.duration.si;
768 }
769
770 /**
771 * Calculate the distance covered by a GTU in this segment.
772 * @return distance covered
773 */
774 final double distanceSI()
775 {
776 return distanceSI(getDuration().si);
777 }
778
779 /**
780 * Calculate the distance covered by a GTU in this segment after relative time t.
781 * @param t the relative time since starting this segment for which to calculate the distance covered
782 * @return distance covered
783 */
784 abstract double distanceSI(double t);
785
786 /**
787 * Calculate the speed of a GTU in this segment after relative time t.
788 * @param t the relative time since starting this segment for which to calculate the speed
789 * @return speed at relative time t
790 */
791 abstract double speedSI(double t);
792
793 /**
794 * Calculate the acceleration of a GTU in this segment after relative time t.
795 * @param t the relative time since starting this segment for which to calculate the acceleration
796 * @return acceleration at relative time t
797 */
798 abstract double accelerationSI(double t);
799
800 /**
801 * Calculate the end speed for this segment.
802 * @return speed at end of the segment
803 */
804 abstract Speed endSpeed();
805
806 /**
807 * Calculate the time it takes for the GTU to travel from the start of this Segment to the specified distance within
808 * this Segment.
809 * @param distance the distance for which the travel time has to be calculated
810 * @return the time at distance
811 */
812 abstract Duration timeAtDistance(final Length distance);
813
814 /** {@inheritDoc} */
815 @SuppressWarnings("checkstyle:designforextension")
816 @Override
817 public int hashCode()
818 {
819 final int prime = 31;
820 int result = 1;
821 result = prime * result + ((this.duration == null) ? 0 : this.duration.hashCode());
822 result = prime * result + ((this.v0 == null) ? 0 : this.v0.hashCode());
823 return result;
824 }
825
826 /** {@inheritDoc} */
827 @SuppressWarnings({ "checkstyle:needbraces", "checkstyle:designforextension" })
828 @Override
829 public boolean equals(final Object obj)
830 {
831 if (this == obj)
832 return true;
833 if (obj == null)
834 return false;
835 if (getClass() != obj.getClass())
836 return false;
837 Segment other = (Segment) obj;
838 if (this.duration == null)
839 {
840 if (other.duration != null)
841 return false;
842 }
843 else if (!this.duration.equals(other.duration))
844 return false;
845 if (this.v0 == null)
846 {
847 if (other.v0 != null)
848 return false;
849 }
850 else if (!this.v0.equals(other.v0))
851 return false;
852 return true;
853 }
854 }
855
856 /**
857 * The segment of an operational plan in which the acceleration is constant.
858 * <p>
859 * Copyright (c) 2013-2018 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. <br>
860 * All rights reserved. <br>
861 * BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
862 * </p>
863 * $LastChangedDate: 2015-07-24 02:58:59 +0200 (Fri, 24 Jul 2015) $, @version $Revision: 1147 $, by $Author: averbraeck $,
864 * initial version Nov 14, 2015 <br>
865 * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
866 * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
867 */
868 public static class AccelerationSegment extends Segment
869 {
870 /** */
871 private static final long serialVersionUID = 20151114L;
872
873 /** The acceleration for the given duration. */
874 private final Acceleration acceleration;
875
876 /**
877 * @param duration the duration of the constant acceleration for this segment
878 * @param acceleration the acceleration for the given duration
879 */
880 public AccelerationSegment(final Duration duration, final Acceleration acceleration)
881 {
882 super(duration);
883 this.acceleration = acceleration;
884 }
885
886 /** {@inheritDoc} */
887 @Override
888 final double distanceSI(final double t)
889 {
890 return this.v0.si * t + 0.5 * this.acceleration.si * t * t;
891 }
892
893 /** {@inheritDoc} */
894 @Override
895 final double accelerationSI(final double t)
896 {
897 return this.acceleration.si;
898 }
899
900 /** {@inheritDoc} */
901 @Override
902 final double speedSI(final double t)
903 {
904 return this.v0.si + this.acceleration.si * t;
905 }
906
907 /** {@inheritDoc} */
908 @Override
909 final Speed endSpeed()
910 {
911 return this.v0.plus(this.acceleration.multiplyBy(getDuration()));
912 }
913
914 /** {@inheritDoc} */
915 @Override
916 public final int hashCode()
917 {
918 final int prime = 31;
919 int result = super.hashCode();
920 result = prime * result + ((this.acceleration == null) ? 0 : this.acceleration.hashCode());
921 return result;
922 }
923
924 /** {@inheritDoc} */
925 @Override
926 @SuppressWarnings("checkstyle:needbraces")
927 public final boolean equals(final Object obj)
928 {
929 if (this == obj)
930 return true;
931 if (!super.equals(obj))
932 return false;
933 if (getClass() != obj.getClass())
934 return false;
935 AccelerationSegment other = (AccelerationSegment) obj;
936 if (this.acceleration == null)
937 {
938 if (other.acceleration != null)
939 return false;
940 }
941 else if (!this.acceleration.equals(other.acceleration))
942 return false;
943 return true;
944 }
945
946 /** {@inheritDoc} */
947 @Override
948 public final Duration timeAtDistance(final Length distance)
949 {
950 double[] solutions = Solver.solve(this.acceleration.si / 2, this.v0.si, -distance.si);
951 // Find the solution that occurs within our duration (there should be only one).
952 for (double solution : solutions)
953 {
954 if (solution >= 0 && solution <= this.duration.si)
955 {
956 return new Duration(solution, DurationUnit.SI);
957 }
958 }
959 SimLogger.always().error("AccelerationSegment " + this + " timeAtDistance( " + distance + ") failed");
960 return null; // No valid solution
961 }
962
963 /** {@inheritDoc} */
964 @Override
965 public final String toString()
966 {
967 return "AccelerationSegment [t=" + this.duration + ", v0=" + this.v0 + ", a=" + this.acceleration + "]";
968 }
969
970 }
971
972 /**
973 * The segment of an operational plan in which the speed is constant.
974 * <p>
975 * Copyright (c) 2013-2018 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. <br>
976 * All rights reserved. <br>
977 * BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
978 * </p>
979 * $LastChangedDate: 2015-07-24 02:58:59 +0200 (Fri, 24 Jul 2015) $, @version $Revision: 1147 $, by $Author: averbraeck $,
980 * initial version Nov 14, 2015 <br>
981 * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
982 * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
983 */
984 public static class SpeedSegment extends Segment
985 {
986 /** */
987 private static final long serialVersionUID = 20151114L;
988
989 /**
990 * @param duration the duration of the constant speed for this segment
991 */
992 public SpeedSegment(final Duration duration)
993 {
994 super(duration);
995 }
996
997 /** {@inheritDoc} */
998 @Override
999 final double distanceSI(final double t)
1000 {
1001 return this.v0.si * t;
1002 }
1003
1004 /** {@inheritDoc} */
1005 @Override
1006 final double accelerationSI(final double t)
1007 {
1008 return 0.0;
1009 }
1010
1011 /** {@inheritDoc} */
1012 @Override
1013 final double speedSI(final double t)
1014 {
1015 return this.v0.si;
1016 }
1017
1018 /** {@inheritDoc} */
1019 @Override
1020 final Speed endSpeed()
1021 {
1022 return this.v0;
1023 }
1024
1025 /**
1026 * @return speed
1027 */
1028 public final Speed getSpeed()
1029 {
1030 return this.v0;
1031 }
1032
1033 /** {@inheritDoc} */
1034 @Override
1035 public final Duration timeAtDistance(final Length distance)
1036 {
1037 double[] solution = Solver.solve(this.v0.si, -distance.si);
1038 if (solution.length > 0 && solution[0] >= 0 && solution[0] <= getDurationSI())
1039 {
1040 return new Duration(solution[0], DurationUnit.SI);
1041 }
1042 SimLogger.always().error("SpeedSegment " + this + " timeAtDistance( " + distance + ") failed");
1043 return null; // No valid solution
1044 }
1045
1046 /** {@inheritDoc} */
1047 @Override
1048 public final String toString()
1049 {
1050 return "SpeedSegment [t=" + this.duration + ", v0=" + this.v0 + "]";
1051 }
1052
1053 }
1054
1055 }