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