OperationalPlan.java

  1. package org.opentrafficsim.core.gtu.plan.operational;

  3. import java.io.Serializable;
  4. import java.util.ArrayList;
  5. import java.util.Arrays;
  6. import java.util.List;

  8. import org.djunits.unit.AccelerationUnit;
  9. import org.djunits.unit.LengthUnit;
  10. import org.djunits.unit.SpeedUnit;
  11. import org.djunits.unit.TimeUnit;
  12. import org.djunits.value.vdouble.scalar.Acceleration;
  13. import org.djunits.value.vdouble.scalar.Duration;
  14. import org.djunits.value.vdouble.scalar.Length;
  15. import org.djunits.value.vdouble.scalar.Speed;
  16. import org.djunits.value.vdouble.scalar.Time;
  17. import org.opentrafficsim.core.geometry.OTSGeometryException;
  18. import org.opentrafficsim.core.geometry.OTSLine3D;
  19. import org.opentrafficsim.core.geometry.OTSPoint3D;
  20. import org.opentrafficsim.core.gtu.GTU;
  21. import org.opentrafficsim.core.math.Solver;

  23. import nl.tudelft.simulation.language.d3.DirectedPoint;

  25. /**
  26.  * An Operational plan describes a path through the world with a speed profile that a GTU intends to follow. The OperationalPlan
  27.  * can be updated or replaced at any time (including before it has been totally executed), for which a tactical planner is
  28.  * responsible. The operational plan is implemented using segments of the movement (time, location, speed, acceleration) that
  29.  * the GTU will use to plan its location and movement. Within an OperationalPlan the GTU cannot reverse direction along the path
  30.  * of movement. This ensures that the timeAtDistance method will never have to select among several valid solutions.
  31.  * <p>
  32.  * Copyright (c) 2013-2016 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
  33.  * BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
  34.  * </p>
  35.  * $LastChangedDate: 2015-07-24 02:58:59 +0200 (Fri, 24 Jul 2015) $, @version $Revision: 1147 $, by $Author: averbraeck $,
  36.  * initial version Nov 14, 2015 <br>
  37.  * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
  38.  * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
  39.  */
  40. public class OperationalPlan implements Serializable
  41. {
  42.     /** */
  43.     private static final long serialVersionUID = 20151114L;

  45.     /** The path to follow from a certain time till a certain time. */
  46.     private final OTSLine3D path;

  48.     /** The absolute start time when we start executing the path. */
  49.     private final Time startTime;

  51.     /** The GTU speed when we start executing the path. */
  52.     private final Speed startSpeed;

  54.     /** The segments that make up the path with an acceleration, constant speed or deceleration profile. */
  55.     private final List<OperationalPlan.Segment> operationalPlanSegmentList;

  57.     /** The duration of executing the entire operational plan. */
  58.     private final Duration totalDuration;

  60.     /** The length of the entire operational plan. */
  61.     private final Length totalLength;

  63.     /** The speed at the end of the operational plan. */
  64.     private final Speed endSpeed;

  66.     /** Is this operational plan a wait plan? */
  67.     private final boolean waitPlan;

  69.     /** GTU for debugging purposes. */
  70.     private final GTU gtu;

  72.     /**
  73.      * An array of relative start times of each segment, expressed in the SI unit, where the last time is the overall ending
  74.      * time of the operational plan.
  75.      */
  76.     private final double[] segmentStartTimesRelSI;

  78.     /**
  79.      * The maximum difference in the length of the path and the calculated driven distance implied by the segment list. The same
  80.      * constant is also used as a maximum between speeds of segments that should align in terms of speed.
  81.      */
  82.     static final double MAX_DELTA_SI = 1E-6;

  84.     /** The drifting speed. Speeds under this value will be cropped to zero. */
  85.     public static final double DRIFTING_SPEED_SI = 1E-3;

  87.     /**
  88.      * Construct an operational plan.
  89.      * @param gtu the GTU for debugging purposes
  90.      * @param path the path to follow from a certain time till a certain time. The path should have <i>at least</i> the length
  91.      * @param startTime the absolute start time when we start executing the path
  92.      * @param startSpeed the GTU speed when we start executing the path
  93.      * @param operationalPlanSegmentList the segments that make up the path with an acceleration, constant speed or deceleration
  94.      *            profile
  95.      * @throws OperationalPlanException when the path is too short for the operation
  96.      */
  97.     public OperationalPlan(final GTU gtu, final OTSLine3D path, final Time startTime, final Speed startSpeed,
  98.             final List<Segment> operationalPlanSegmentList) throws OperationalPlanException
  99.     {
  100.         this.waitPlan = false;
  101.         this.gtu = gtu;
  102.         this.startTime = startTime;
  103.         this.startSpeed = startSpeed;
  104.         this.operationalPlanSegmentList = operationalPlanSegmentList;
  105.         this.segmentStartTimesRelSI = new double[this.operationalPlanSegmentList.size() + 1];

  107.         // check the driven distance of the segments
  108.         Speed v0 = this.startSpeed;
  109.         double distanceSI = 0.0;
  110.         double durationSI = 0.0;
  111.         for (int i = 0; i < this.operationalPlanSegmentList.size(); i++)
  112.         {
  113.             Segment segment = this.operationalPlanSegmentList.get(i);
  114.             if (Math.abs(v0.si) < DRIFTING_SPEED_SI && segment.accelerationSI(0.0) == 0.0)
  115.             {
  116.                 v0 = Speed.ZERO;
  117.             }
  118.             segment.setV0(v0);
  119.             this.segmentStartTimesRelSI[i] = durationSI;
  120.             distanceSI += segment.distanceSI();
  121.             v0 = segment.endSpeed();
  122.             durationSI += segment.getDuration().si;
  123.         }
  124.         this.segmentStartTimesRelSI[this.segmentStartTimesRelSI.length - 1] = durationSI;
  125.         try
  126.         {
  127.             this.path = path.extract(0.0, Math.min(distanceSI, path.getLengthSI()));
  128.         }
  129.         catch (OTSGeometryException exception)
  130.         {
  131.             throw new OperationalPlanException(exception);
  132.         }
  133.         this.totalDuration = new Duration(durationSI, TimeUnit.SI);
  134.         this.totalLength = new Length(distanceSI, LengthUnit.SI);
  135.         this.endSpeed = v0;

  137.         double pathDistanceDeviation = Math.abs(this.totalLength.si - this.path.getLengthSI()) / this.totalLength.si;
  138.         if (pathDistanceDeviation < -0.01 || pathDistanceDeviation > 0.01)
  139.         {
  140.             System.err.println("path length and driven distance deviate more than 1% for operationalPlan: " + this);
  141.         }
  142.     }

  144.     /**
  145.      * Build a plan where the GTU will wait for a certain time.
  146.      * @param gtu the GTU for debugging purposes
  147.      * @param waitPoint the point at which the GTU will wait
  148.      * @param startTime the current time or a time in the future when the plan should start
  149.      * @param duration the waiting time
  150.      * @throws OperationalPlanException when construction of a waiting path fails
  151.      */
  152.     public OperationalPlan(final GTU gtu, final DirectedPoint waitPoint, final Time startTime, final Duration duration)
  153.             throws OperationalPlanException
  154.     {
  155.         this.waitPlan = true;
  156.         this.gtu = gtu;
  157.         this.startTime = startTime;
  158.         this.startSpeed = Speed.ZERO;
  159.         this.endSpeed = Speed.ZERO;
  160.         this.totalDuration = duration;
  161.         this.totalLength = Length.ZERO;

  163.         // make a path
  164.         OTSPoint3D p2 = new OTSPoint3D(waitPoint.x + Math.cos(waitPoint.getRotZ()), waitPoint.y + Math.sin(waitPoint.getRotZ()),
  165.                 waitPoint.z);
  166.         try
  167.         {
  168.             this.path = new OTSLine3D(new OTSPoint3D(waitPoint), p2);
  169.         }
  170.         catch (OTSGeometryException exception)
  171.         {
  172.             throw new OperationalPlanException(exception);
  173.         }

  175.         this.operationalPlanSegmentList = new ArrayList<>();
  176.         Segment segment = new SpeedSegment(duration);
  177.         segment.setV0(Speed.ZERO);
  178.         this.operationalPlanSegmentList.add(segment);
  179.         this.segmentStartTimesRelSI = new double[2];
  180.         this.segmentStartTimesRelSI[0] = 0.0;
  181.         this.segmentStartTimesRelSI[1] = duration.si;
  182.     }

  184.     /**
  185.      * 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
  186.      * the path is bogus (should only be used to determine the orientation of the GTU).
  187.      * @return the path
  188.      */
  189.     public final OTSLine3D getPath()
  190.     {
  191.         return this.path;
  192.     }

  194.     /**
  195.      * Return the (absolute) start time of the operational plan.
  196.      * @return startTime
  197.      */
  198.     public final Time getStartTime()
  199.     {
  200.         return this.startTime;
  201.     }

  203.     /**
  204.      * Return the start speed of the entire plan.
  205.      * @return startSpeed
  206.      */
  207.     public final Speed getStartSpeed()
  208.     {
  209.         return this.startSpeed;
  210.     }

  212.     /**
  213.      * @return the end speed when completing the entire plan.
  214.      */
  215.     public final Speed getEndSpeed()
  216.     {
  217.         return this.endSpeed;
  218.     }

  220.     /**
  221.      * Return the segments (parts with constant speed, acceleration or deceleration) of the operational plan. <br>
  222.      * The caller MUST NOT MODIFY the returned object.
  223.      * @return operationalPlanSegmentList
  224.      */
  225.     public final List<OperationalPlan.Segment> getOperationalPlanSegmentList()
  226.     {
  227.         return this.operationalPlanSegmentList;
  228.     }

  230.     /**
  231.      * Return the time it will take to complete the entire operational plan.
  232.      * @return the time it will take to complete the entire operational plan
  233.      */
  234.     public final Duration getTotalDuration()
  235.     {
  236.         return this.totalDuration;
  237.     }

  239.     /**
  240.      * Return the distance the entire operational plan will cover.
  241.      * @return the distance of the entire operational plan
  242.      */
  243.     public final Length getTotalLength()
  244.     {
  245.         return this.totalLength;
  246.     }

  248.     /**
  249.      * Return the time it will take to complete the entire operational plan.
  250.      * @return the time it will take to complete the entire operational plan
  251.      */
  252.     public final Time getEndTime()
  253.     {
  254.         return this.startTime.plus(this.totalDuration);
  255.     }

  257.     /**
  258.      * Provide the end location of this operational plan as a DirectedPoint.
  259.      * @return the end location
  260.      */
  261.     public final DirectedPoint getEndLocation()
  262.     {
  263.         try
  264.         {
  265.             if (this.waitPlan)
  266.             {
  267.                 return this.path.getLocationFraction(0.0); // no move...
  268.             }
  269.             else
  270.             {
  271.                 return this.path.getLocationFraction(1.0);
  272.             }
  273.         }
  274.         catch (OTSGeometryException exception)
  275.         {
  276.             // should not happen -- only for fractions less than 0.0 or larger than 1.0.
  277.             throw new RuntimeException(exception);
  278.         }
  279.     }

  281.     /**
  282.      * Store a Segment and the progress within that segment in one Object.
  283.      * <p>
  284.      * Copyright (c) 2013-2016 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. <br>
  285.      * All rights reserved. <br>
  286.      * BSD-style license. See <a href="http://opentrafficsim.org/docs/current/license.html">OpenTrafficSim License</a>.
  287.      * <p>
  288.      * @version $Revision$, $LastChangedDate$, by $Author$, initial version Dec 16, 2015 <br>
  289.      * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
  290.      * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
  291.      */
  292.     private class SegmentProgress implements Serializable
  293.     {
  294.         /** */
  295.         private static final long serialVersionUID = 20160000L;

  297.         /** Active Segment. */
  298.         private final Segment segment;

  300.         /** Start time of the Segment. */
  301.         private final Time segmentStartTime;

  303.         /** Position on the path of the plan. */
  304.         private final Length segmentStartPosition;

  306.         /**
  307.          * Construct a new SegmentProgress object.
  308.          * @param segment Segment; the Segment
  309.          * @param segmentStartTime Time; the start time of the Segment
  310.          * @param segmentStartPosition Length; the position of the start of the segment on the path of the OperationalPlan
  311.          */
  312.         SegmentProgress(final Segment segment, final Time segmentStartTime, final Length segmentStartPosition)
  313.         {
  314.             this.segment = segment;
  315.             this.segmentStartTime = segmentStartTime;
  316.             this.segmentStartPosition = segmentStartPosition;
  317.         }

  319.         /**
  320.          * Retrieve the Segment.
  321.          * @return Segment
  322.          */
  323.         public final Segment getSegment()
  324.         {
  325.             return this.segment;
  326.         }

  328.         /**
  329.          * Retrieve the start time of the Segment.
  330.          * @return Time; the start time of the Segment
  331.          */
  332.         public final Time getSegmentStartTime()
  333.         {
  334.             return this.segmentStartTime;
  335.         }

  337.         /**
  338.          * Retrieve the fractionalPosition at the start of the Segment.
  339.          * @return double; the fractional position at the start of the Segment
  340.          */
  341.         public final Length getSegmentStartPosition()
  342.         {
  343.             return this.segmentStartPosition;
  344.         }

  346.         /** {@inheritDoc} */
  347.         @Override
  348.         public final String toString()
  349.         {
  350.             return String.format("SegmentProgress segment=%s startpos.rel=%s starttime.abs=%s", this.segment,
  351.                     this.segmentStartPosition, this.segmentStartTime);
  352.         }
  353.     }

  355.     /**
  356.      * Find the Segment and the progress within that Segment at a specified time.
  357.      * @param time Time; the time
  358.      * @return SegmentProgress; the Segment and progress within that segment, or null when no Segment applies to the specified
  359.      *         time
  360.      * @throws OperationalPlanException when SegmentProgress cannot be determined
  361.      */
  362.     private SegmentProgress getSegmentProgress(final Time time) throws OperationalPlanException
  363.     {
  364.         if (time.lt(this.startTime))
  365.         {
  366.             throw new OperationalPlanException(
  367.                     this.gtu + ", t = " + time + "SegmentProgress cannot be determined for time before startTime "
  368.                             + getStartTime() + " of this OperationalPlan");
  369.         }
  370.         double cumulativeDistance = 0;
  371.         for (int i = 0; i < this.segmentStartTimesRelSI.length - 1; i++)
  372.         {
  373.             if (this.startTime.si + this.segmentStartTimesRelSI[i + 1] >= time.si)
  374.             {
  375.                 return new SegmentProgress(this.operationalPlanSegmentList.get(i),
  376.                         new Time(this.startTime.si + this.segmentStartTimesRelSI[i], TimeUnit.SI),
  377.                         new Length(cumulativeDistance, LengthUnit.SI));
  378.             }
  379.         }
  380.         throw new OperationalPlanException(this.gtu + ", t = " + time
  381.                 + " SegmentProgress cannot be determined for time after endTime " + getEndTime() + " of this OperationalPlan");
  382.     }

  384.     /**
  385.      * Return the time when the GTU will reach the given distance.
  386.      * @param distance the distance to calculate the time for
  387.      * @return the time it will take to have traveled the given distance
  388.      */
  389.     public final Time timeAtDistance(final Length distance)
  390.     {
  391.         double remainingDistanceSI = distance.si;
  392.         double timeAtStartOfSegment = this.startTime.si;
  393.         for (Segment segment : this.operationalPlanSegmentList)
  394.         {
  395.             double distanceOfSegment = segment.distanceSI();
  396.             if (distanceOfSegment > remainingDistanceSI)
  397.             {
  398.                 return new Time(
  399.                         timeAtStartOfSegment + segment.timeAtDistance(new Length(remainingDistanceSI, LengthUnit.SI)).si,
  400.                         TimeUnit.SI);
  401.             }
  402.             remainingDistanceSI -= distanceOfSegment;
  403.             timeAtStartOfSegment += segment.getDurationSI();
  404.         }
  405.         return new Time(Double.NaN, TimeUnit.SI);
  406.     }

  408.     /**
  409.      * Calculate the location at the given time.
  410.      * @param time the absolute time to look for a location
  411.      * @return the location at the given time.
  412.      * @throws OperationalPlanException when the time is after the validity of the operational plan
  413.      */
  414.     public final DirectedPoint getLocation(final Time time) throws OperationalPlanException
  415.     {
  416.         SegmentProgress sp = getSegmentProgress(time);
  417.         Segment segment = sp.getSegment();
  418.         Duration deltaT = time.minus(sp.getSegmentStartTime());
  419.         double distanceTraveledInSegment = segment.distanceSI(deltaT.si);
  420.         double startDistance = sp.getSegmentStartPosition().si;
  421.         double fraction = (startDistance + distanceTraveledInSegment) / this.path.getLengthSI();
  422.         DirectedPoint p = new DirectedPoint();
  423.         try
  424.         {
  425.             p = this.path.getLocationFraction(fraction, 0.01);
  426.         }
  427.         catch (OTSGeometryException exception)
  428.         {
  429.             System.err.println("OperationalPlan.getLocation(): " + exception.getMessage());
  430.             p = this.path.getLocationFractionExtended(fraction);
  431.         }
  432.         p.setZ(p.getZ() + 0.001);
  433.         return p;
  434.     }

  436.     /**
  437.      * Calculate the speed of the GTU after the given duration since the start of the plan.
  438.      * @param time the relative time to look for a location
  439.      * @return the location after the given duration since the start of the plan.
  440.      * @throws OperationalPlanException when the time is after the validity of the operational plan
  441.      */
  442.     public final Speed getSpeed(final Duration time) throws OperationalPlanException
  443.     {
  444.         return getSpeed(time.plus(this.startTime));
  445.     }

  447.     /**
  448.      * Calculate the speed of the GTU at the given time.
  449.      * @param time the absolute time to look for a location
  450.      * @return the location at the given time.
  451.      * @throws OperationalPlanException when the time is after the validity of the operational plan
  452.      */
  453.     public final Speed getSpeed(final Time time) throws OperationalPlanException
  454.     {
  455.         SegmentProgress sp = getSegmentProgress(time);
  456.         return new Speed(sp.getSegment().speedSI(time.minus(sp.getSegmentStartTime()).si), SpeedUnit.SI);
  457.     }

  459.     /**
  460.      * Calculate the acceleration of the GTU after the given duration since the start of the plan.
  461.      * @param time the relative time to look for a location
  462.      * @return the location after the given duration since the start of the plan.
  463.      * @throws OperationalPlanException when the time is after the validity of the operational plan
  464.      */
  465.     public final Acceleration getAcceleration(final Duration time) throws OperationalPlanException
  466.     {
  467.         return getAcceleration(time.plus(this.startTime));
  468.     }

  470.     /**
  471.      * Calculate the acceleration of the GTU at the given time.
  472.      * @param time the absolute time to look for a location
  473.      * @return the location at the given time.
  474.      * @throws OperationalPlanException when the time is after the validity of the operational plan
  475.      */
  476.     public final Acceleration getAcceleration(final Time time) throws OperationalPlanException
  477.     {
  478.         SegmentProgress sp = getSegmentProgress(time);
  479.         return new Acceleration(sp.getSegment().accelerationSI(time.minus(sp.getSegmentStartTime()).si), AccelerationUnit.SI);
  480.     }

  482.     /**
  483.      * Calculate the location after the given duration since the start of the plan.
  484.      * @param time the relative time to look for a location
  485.      * @return the location after the given duration since the start of the plan.
  486.      * @throws OperationalPlanException when the time is after the validity of the operational plan
  487.      */
  488.     public final DirectedPoint getLocation(final Duration time) throws OperationalPlanException
  489.     {
  490.         double distanceSI = getTraveledDistanceSI(time);
  491.         return this.path.getLocationExtendedSI(distanceSI);
  492.     }

  494.     /**
  495.      * Calculate the distance traveled as part of this plan after the given duration since the start of the plan. This method
  496.      * returns the traveled distance as a double in SI units.
  497.      * @param duration the relative time to calculate the traveled distance
  498.      * @return the distance traveled as part of this plan after the given duration since the start of the plan.
  499.      * @throws OperationalPlanException when the time is after the validity of the operational plan
  500.      */
  501.     public final double getTraveledDistanceSI(final Duration duration) throws OperationalPlanException
  502.     {
  503.         Time absTime = duration.plus(this.startTime);
  504.         SegmentProgress sp = getSegmentProgress(absTime);
  505.         return sp.getSegment().distanceSI(absTime.minus(sp.getSegmentStartTime()).si);
  506.     }

  508.     /**
  509.      * Calculate the distance traveled as part of this plan after the given duration since the start of the plan.
  510.      * @param duration the relative time to calculate the traveled distance
  511.      * @return the distance traveled as part of this plan after the given duration since the start of the plan.
  512.      * @throws OperationalPlanException when the time is after the validity of the operational plan
  513.      */
  514.     public final Length getTraveledDistance(final Duration duration) throws OperationalPlanException
  515.     {
  516.         return new Length(getTraveledDistanceSI(duration), LengthUnit.SI);
  517.     }

  519.     /**
  520.      * Calculate the distance traveled as part of this plan at the given absolute time. This method returns the traveled
  521.      * distance as a double in SI units.
  522.      * @param time the absolute time to calculate the traveled distance for as part of this plan
  523.      * @return the distance traveled as part of this plan at the given time
  524.      * @throws OperationalPlanException when the time is after the validity of the operational plan
  525.      */
  526.     public final double getTraveledDistanceSI(final Time time) throws OperationalPlanException
  527.     {
  528.         return getTraveledDistanceSI(time.minus(this.startTime));
  529.     }

  531.     /**
  532.      * Calculate the distance traveled as part of this plan at the given absolute time.
  533.      * @param time the absolute time to calculate the traveled distance for as part of this plan
  534.      * @return the distance traveled as part of this plan at the given time
  535.      * @throws OperationalPlanException when the time is after the validity of the operational plan
  536.      */
  537.     public final Length getTraveledDistance(final Time time) throws OperationalPlanException
  538.     {
  539.         return new Length(getTraveledDistanceSI(time.minus(this.startTime)), LengthUnit.SI);
  540.     }

  542.     /** {@inheritDoc} */
  543.     @SuppressWarnings("checkstyle:designforextension")
  544.     @Override
  545.     public int hashCode()
  546.     {
  547.         final int prime = 31;
  548.         int result = 1;
  549.         result = prime * result + ((this.operationalPlanSegmentList == null) ? 0 : this.operationalPlanSegmentList.hashCode());
  550.         result = prime * result + ((this.path == null) ? 0 : this.path.hashCode());
  551.         result = prime * result + ((this.startSpeed == null) ? 0 : this.startSpeed.hashCode());
  552.         result = prime * result + ((this.startTime == null) ? 0 : this.startTime.hashCode());
  553.         return result;
  554.     }

  556.     /** {@inheritDoc} */
  557.     @SuppressWarnings({ "checkstyle:needbraces", "checkstyle:designforextension" })
  558.     @Override
  559.     public boolean equals(final Object obj)
  560.     {
  561.         if (this == obj)
  562.             return true;
  563.         if (obj == null)
  564.             return false;
  565.         if (getClass() != obj.getClass())
  566.             return false;
  567.         OperationalPlan other = (OperationalPlan) obj;
  568.         if (this.operationalPlanSegmentList == null)
  569.         {
  570.             if (other.operationalPlanSegmentList != null)
  571.                 return false;
  572.         }
  573.         else if (!this.operationalPlanSegmentList.equals(other.operationalPlanSegmentList))
  574.             return false;
  575.         if (this.path == null)
  576.         {
  577.             if (other.path != null)
  578.                 return false;
  579.         }
  580.         else if (!this.path.equals(other.path))
  581.             return false;
  582.         if (this.startSpeed == null)
  583.         {
  584.             if (other.startSpeed != null)
  585.                 return false;
  586.         }
  587.         else if (!this.startSpeed.equals(other.startSpeed))
  588.             return false;
  589.         if (this.startTime == null)
  590.         {
  591.             if (other.startTime != null)
  592.                 return false;
  593.         }
  594.         else if (!this.startTime.equals(other.startTime))
  595.             return false;
  596.         return true;
  597.     }

  599.     /** {@inheritDoc} */
  600.     @SuppressWarnings("checkstyle:designforextension")
  601.     @Override
  602.     public String toString()
  603.     {
  604.         return "OperationalPlan [path=" + this.path + ", startTime=" + this.startTime + ", startSpeed=" + this.startSpeed
  605.                 + ", operationalPlanSegmentList=" + this.operationalPlanSegmentList + ", totalDuration=" + this.totalDuration
  606.                 + ", segmentStartTimesSI=" + Arrays.toString(this.segmentStartTimesRelSI) + ", endSpeed = " + this.endSpeed
  607.                 + "]";
  608.     }

  610.     /****************************************************************************************************************/
  611.     /******************************************** SEGMENT DEFINITIONS ***********************************************/
  612.     /****************************************************************************************************************/

  614.     /**
  615.      * The segment of an operational plan contains a part of the speed profile of a movement in which some of the variables
  616.      * determining movement (speed, acceleration) are constant.
  617.      * <p>
  618.      * Copyright (c) 2013-2016 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. <br>
  619.      * All rights reserved. <br>
  620.      * BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
  621.      * </p>
  622.      * $LastChangedDate: 2015-07-24 02:58:59 +0200 (Fri, 24 Jul 2015) $, @version $Revision: 1147 $, by $Author: averbraeck $,
  623.      * initial version Nov 14, 2015 <br>
  624.      * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
  625.      * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
  626.      */
  627.     public abstract static class Segment implements Serializable
  628.     {
  629.         /** */
  630.         private static final long serialVersionUID = 20151114L;

  632.         /** The duration of the acceleration or speed for this segment. */
  633.         @SuppressWarnings("checkstyle:visibilitymodifier")
  634.         protected final Duration duration;

  636.         /** The initial speed for this segment. */
  637.         @SuppressWarnings("checkstyle:visibilitymodifier")
  638.         protected Speed v0;

  640.         /**
  641.          * @param duration the duration of the acceleration or speed for this segment
  642.          */
  643.         public Segment(final Duration duration)
  644.         {
  645.             super();
  646.             this.duration = duration;
  647.         }

  649.         /**
  650.          * @param v0 the initial speed of this segment; called from the Operational Plan constructor.
  651.          */
  652.         final void setV0(final Speed v0)
  653.         {
  654.             this.v0 = v0;
  655.         }

  657.         /**
  658.          * @return duration the duration of the acceleration or speed for this segment
  659.          */
  660.         public final Duration getDuration()
  661.         {
  662.             return this.duration;
  663.         }

  665.         /**
  666.          * @return duration the duration of the acceleration or speed for this segment
  667.          */
  668.         public final double getDurationSI()
  669.         {
  670.             return this.duration.si;
  671.         }

  673.         /**
  674.          * Calculate the distance covered by a GTU in this segment.
  675.          * @return distance covered
  676.          */
  677.         final double distanceSI()
  678.         {
  679.             return distanceSI(getDuration().si);
  680.         }

  682.         /**
  683.          * Calculate the distance covered by a GTU in this segment after relative time t.
  684.          * @param t the relative time since starting this segment for which to calculate the distance covered
  685.          * @return distance covered
  686.          */
  687.         abstract double distanceSI(double t);

  689.         /**
  690.          * Calculate the speed of a GTU in this segment after relative time t.
  691.          * @param t the relative time since starting this segment for which to calculate the speed
  692.          * @return speed at relative time t
  693.          */
  694.         abstract double speedSI(double t);

  696.         /**
  697.          * Calculate the acceleration of a GTU in this segment after relative time t.
  698.          * @param t the relative time since starting this segment for which to calculate the acceleration
  699.          * @return acceleration at relative time t
  700.          */
  701.         abstract double accelerationSI(double t);

  703.         /**
  704.          * Calculate the end speed for this segment.
  705.          * @return speed at end of the segment
  706.          */
  707.         abstract Speed endSpeed();

  709.         /**
  710.          * Calculate the time it takes for the GTU to travel from the start of this Segment to the specified distance within
  711.          * this Segment.
  712.          * @param distance the distance for which the travel time has to be calculated
  713.          * @return the time at distance
  714.          */
  715.         abstract Duration timeAtDistance(final Length distance);

  717.         /** {@inheritDoc} */
  718.         @SuppressWarnings("checkstyle:designforextension")
  719.         @Override
  720.         public int hashCode()
  721.         {
  722.             final int prime = 31;
  723.             int result = 1;
  724.             result = prime * result + ((this.duration == null) ? 0 : this.duration.hashCode());
  725.             result = prime * result + ((this.v0 == null) ? 0 : this.v0.hashCode());
  726.             return result;
  727.         }

  729.         /** {@inheritDoc} */
  730.         @SuppressWarnings({ "checkstyle:needbraces", "checkstyle:designforextension" })
  731.         @Override
  732.         public boolean equals(final Object obj)
  733.         {
  734.             if (this == obj)
  735.                 return true;
  736.             if (obj == null)
  737.                 return false;
  738.             if (getClass() != obj.getClass())
  739.                 return false;
  740.             Segment other = (Segment) obj;
  741.             if (this.duration == null)
  742.             {
  743.                 if (other.duration != null)
  744.                     return false;
  745.             }
  746.             else if (!this.duration.equals(other.duration))
  747.                 return false;
  748.             if (this.v0 == null)
  749.             {
  750.                 if (other.v0 != null)
  751.                     return false;
  752.             }
  753.             else if (!this.v0.equals(other.v0))
  754.                 return false;
  755.             return true;
  756.         }
  757.     }

  759.     /**
  760.      * The segment of an operational plan in which the acceleration is constant.
  761.      * <p>
  762.      * Copyright (c) 2013-2016 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. <br>
  763.      * All rights reserved. <br>
  764.      * BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
  765.      * </p>
  766.      * $LastChangedDate: 2015-07-24 02:58:59 +0200 (Fri, 24 Jul 2015) $, @version $Revision: 1147 $, by $Author: averbraeck $,
  767.      * initial version Nov 14, 2015 <br>
  768.      * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
  769.      * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
  770.      */
  771.     public static class AccelerationSegment extends Segment
  772.     {
  773.         /** */
  774.         private static final long serialVersionUID = 20151114L;

  776.         /** The acceleration for the given duration. */
  777.         private final Acceleration acceleration;

  779.         /**
  780.          * @param duration the duration of the constant acceleration for this segment
  781.          * @param acceleration the acceleration for the given duration
  782.          */
  783.         public AccelerationSegment(final Duration duration, final Acceleration acceleration)
  784.         {
  785.             super(duration);
  786.             this.acceleration = acceleration;
  787.         }

  789.         /** {@inheritDoc} */
  790.         @Override
  791.         final double distanceSI(final double t)
  792.         {
  793.             return this.v0.si * t + 0.5 * this.acceleration.si * t * t;
  794.         }

  796.         /** {@inheritDoc} */
  797.         @Override
  798.         final double accelerationSI(final double t)
  799.         {
  800.             return this.acceleration.si;
  801.         }

  803.         /** {@inheritDoc} */
  804.         @Override
  805.         final double speedSI(final double t)
  806.         {
  807.             return this.v0.si + this.acceleration.si * t;
  808.         }

  810.         /** {@inheritDoc} */
  811.         @Override
  812.         final Speed endSpeed()
  813.         {
  814.             return this.v0.plus(this.acceleration.multiplyBy(getDuration()));
  815.         }

  817.         /** {@inheritDoc} */
  818.         @Override
  819.         public final int hashCode()
  820.         {
  821.             final int prime = 31;
  822.             int result = super.hashCode();
  823.             result = prime * result + ((this.acceleration == null) ? 0 : this.acceleration.hashCode());
  824.             return result;
  825.         }

  827.         /** {@inheritDoc} */
  828.         @Override
  829.         @SuppressWarnings("checkstyle:needbraces")
  830.         public final boolean equals(final Object obj)
  831.         {
  832.             if (this == obj)
  833.                 return true;
  834.             if (!super.equals(obj))
  835.                 return false;
  836.             if (getClass() != obj.getClass())
  837.                 return false;
  838.             AccelerationSegment other = (AccelerationSegment) obj;
  839.             if (this.acceleration == null)
  840.             {
  841.                 if (other.acceleration != null)
  842.                     return false;
  843.             }
  844.             else if (!this.acceleration.equals(other.acceleration))
  845.                 return false;
  846.             return true;
  847.         }

  849.         /** {@inheritDoc} */
  850.         @Override
  851.         public final Duration timeAtDistance(final Length distance)
  852.         {
  853.             double[] solutions = Solver.solve(this.acceleration.si / 2, this.v0.si, -distance.si);
  854.             // Find the solution that occurs within our duration (there should be only one).
  855.             for (double solution : solutions)
  856.             {
  857.                 if (solution >= 0 && solution <= this.duration.si)
  858.                 {
  859.                     return new Duration(solution, TimeUnit.SI);
  860.                 }
  861.             }
  862.             System.err.println("AccelerationSegment " + this + " timeAtDistance( " + distance + ") failed");
  863.             return null; // No valid solution
  864.         }

  866.         /** {@inheritDoc} */
  867.         @Override
  868.         public final String toString()
  869.         {
  870.             return "AccelerationSegment [t=" + this.duration + ", v0=" + this.v0 + ", a=" + this.acceleration + "]";
  871.         }

  873.     }

  875.     /**
  876.      * The segment of an operational plan in which the speed is constant.
  877.      * <p>
  878.      * Copyright (c) 2013-2016 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. <br>
  879.      * All rights reserved. <br>
  880.      * BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
  881.      * </p>
  882.      * $LastChangedDate: 2015-07-24 02:58:59 +0200 (Fri, 24 Jul 2015) $, @version $Revision: 1147 $, by $Author: averbraeck $,
  883.      * initial version Nov 14, 2015 <br>
  884.      * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
  885.      * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
  886.      */
  887.     public static class SpeedSegment extends Segment
  888.     {
  889.         /** */
  890.         private static final long serialVersionUID = 20151114L;

  892.         /**
  893.          * @param duration the duration of the constant speed for this segment
  894.          */
  895.         public SpeedSegment(final Duration duration)
  896.         {
  897.             super(duration);
  898.         }

  900.         /** {@inheritDoc} */
  901.         @Override
  902.         final double distanceSI(final double t)
  903.         {
  904.             return this.v0.si * t;
  905.         }

  907.         /** {@inheritDoc} */
  908.         @Override
  909.         final double accelerationSI(final double t)
  910.         {
  911.             return 0.0;
  912.         }

  914.         /** {@inheritDoc} */
  915.         @Override
  916.         final double speedSI(final double t)
  917.         {
  918.             return this.v0.si;
  919.         }

  921.         /** {@inheritDoc} */
  922.         @Override
  923.         final Speed endSpeed()
  924.         {
  925.             return this.v0;
  926.         }

  928.         /**
  929.          * @return speed
  930.          */
  931.         public final Speed getSpeed()
  932.         {
  933.             return this.v0;
  934.         }

  936.         /** {@inheritDoc} */
  937.         @Override
  938.         public final Duration timeAtDistance(final Length distance)
  939.         {
  940.             double[] solution = Solver.solve(this.v0.si, -distance.si);
  941.             if (solution.length > 0 && solution[0] >= 0 && solution[0] <= getDurationSI())
  942.             {
  943.                 return new Duration(solution[0], TimeUnit.SI);
  944.             }
  945.             System.err.println("SpeedSegment " + this + " timeAtDistance( " + distance + ") failed");
  946.             return null; // No valid solution
  947.         }

  949.         /** {@inheritDoc} */
  950.         @Override
  951.         public final String toString()
  952.         {
  953.             return "SpeedSegment [t=" + this.duration + ", v0=" + this.v0 + "]";
  954.         }

  956.     }

  958. }
Created with JaCoCo 0.7.5.201505241946