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