feat: build EM path speed limits

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPlanningInput.cs

@@ -0,0 +1,111 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Immutable longitudinal inputs derived only from an independently validated lateral PathS path.</summary>
+public sealed class LongitudinalPlanningInput
+{
+    private const double PathSTolerance = 1e-12d;
+
+    public LongitudinalPlanningInput(LateralPath path, TravelDirection direction, double initialProgressSpeedMetersPerSecond,
+        double initialAccelerationMetersPerSecondSquared, EmTerminalType terminalType, EmPlannerConfiguration configuration,
+        IReadOnlyList<double> previousPathS, IReadOnlyList<double> previousProgressSpeedMetersPerSecond)
+    {
+        if (path == null || !path.IsIndependentlyValidated || path.Points.Count < 2)
+            throw new ArgumentException("Longitudinal planning requires an independently validated lateral path with at least two points.",
+                nameof(path));
+        if (!Enum.IsDefined(typeof(TravelDirection), direction))
+            throw new ArgumentOutOfRangeException(nameof(direction));
+        if (!IsFinite(initialProgressSpeedMetersPerSecond) || initialProgressSpeedMetersPerSecond < 0d)
+            throw new ArgumentOutOfRangeException(nameof(initialProgressSpeedMetersPerSecond));
+        if (!IsFinite(initialAccelerationMetersPerSecondSquared))
+            throw new ArgumentOutOfRangeException(nameof(initialAccelerationMetersPerSecondSquared));
+        if (!Enum.IsDefined(typeof(EmTerminalType), terminalType))
+            throw new ArgumentOutOfRangeException(nameof(terminalType));
+        if (configuration == null)
+            throw new ArgumentNullException(nameof(configuration));
+
+        Path = CopyAndValidatePath(path);
+        Direction = direction;
+        InitialProgressSpeedMetersPerSecond = initialProgressSpeedMetersPerSecond;
+        InitialAccelerationMetersPerSecondSquared = initialAccelerationMetersPerSecondSquared;
+        TerminalType = terminalType;
+        Configuration = configuration.Copy();
+        PreviousPathS = CopyFiniteNonnegative(previousPathS, nameof(previousPathS));
+        PreviousProgressSpeedMetersPerSecond = CopyFiniteNonnegative(previousProgressSpeedMetersPerSecond,
+            nameof(previousProgressSpeedMetersPerSecond));
+        if (PreviousPathS.Count != PreviousProgressSpeedMetersPerSecond.Count)
+            throw new ArgumentException("Previous path-S and progress-speed samples must have matching counts.",
+                nameof(previousProgressSpeedMetersPerSecond));
+    }
+
+    public LateralPath Path { get; }
+
+    public TravelDirection Direction { get; }
+
+    public double InitialProgressSpeedMetersPerSecond { get; }
+
+    public double InitialAccelerationMetersPerSecondSquared { get; }
+
+    public EmTerminalType TerminalType { get; }
+
+    public EmPlannerConfiguration Configuration { get; }
+
+    public IReadOnlyList<double> PreviousPathS { get; }
+
+    public IReadOnlyList<double> PreviousProgressSpeedMetersPerSecond { get; }
+
+    public double TerminalPathS { get { return Path.Points[Path.Points.Count - 1].PathS; } }
+
+    public double DirectionMaximumSpeedMetersPerSecond
+    {
+        get
+        {
+            return Direction == TravelDirection.Forward
+                ? Configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond
+                : Configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond;
+        }
+    }
+
+    private static LateralPath CopyAndValidatePath(LateralPath source)
+    {
+        var copy = new List<LateralPathPoint>(source.Points.Count);
+        double previousPathS = double.NegativeInfinity;
+        for (int index = 0; index < source.Points.Count; index++)
+        {
+            LateralPathPoint point = source.Points[index];
+            if (point == null || !IsFinite(point.PathS) || point.PathS <= previousPathS)
+                throw new ArgumentException("Lateral PathS must be finite and strictly increasing for ST planning.", nameof(source));
+            if (index == 0 && Math.Abs(point.PathS) > PathSTolerance)
+                throw new ArgumentException("The lateral PathS supplied to ST must begin at zero.", nameof(source));
+            copy.Add(new LateralPathPoint(point.ReferenceS, point.PathS, point.L, point.DL, point.DDL, point.DDDL,
+                point.X, point.Y, point.VehicleYaw, point.GeometricCurvature, point.VehicleCurvature,
+                point.VehicleCurvatureDerivative));
+            previousPathS = point.PathS;
+        }
+        return new LateralPath(copy, true);
+    }
+
+    private static IReadOnlyList<double> CopyFiniteNonnegative(IReadOnlyList<double> source, string parameterName)
+    {
+        var copy = new List<double>(source == null ? 0 : source.Count);
+        if (source != null)
+        {
+            for (int index = 0; index < source.Count; index++)
+            {
+                if (!IsFinite(source[index]) || source[index] < 0d)
+                    throw new ArgumentOutOfRangeException(parameterName);
+                copy.Add(source[index]);
+            }
+        }
+        return new ReadOnlyCollection<double>(copy);
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimit.cs

@@ -0,0 +1,123 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Finite piecewise-linear speed limits indexed exclusively by actual lateral PathS.</summary>
+public sealed class PathSpeedLimit
+{
+    private const double StationTolerance = 1e-12d;
+
+    internal PathSpeedLimit(IReadOnlyList<double> pathS, IReadOnlyList<double> maximumSpeed,
+        IReadOnlyList<double> lateralAccelerationLimit, IReadOnlyList<double> curvatureRateLimit,
+        IReadOnlyList<double> stoppingLimit, double directionMaximumSpeedMetersPerSecond)
+    {
+        PathS = CopyStrictStations(pathS, nameof(pathS));
+        MaximumSpeedMetersPerSecond = CopyFiniteNonnegative(maximumSpeed, PathS.Count, nameof(maximumSpeed));
+        LateralAccelerationSpeedLimitsMetersPerSecond = CopyFiniteNonnegative(lateralAccelerationLimit, PathS.Count,
+            nameof(lateralAccelerationLimit));
+        CurvatureRateSpeedLimitsMetersPerSecond = CopyFiniteNonnegative(curvatureRateLimit, PathS.Count,
+            nameof(curvatureRateLimit));
+        StoppingSpeedLimitsMetersPerSecond = CopyFiniteNonnegative(stoppingLimit, PathS.Count, nameof(stoppingLimit));
+        if (!IsFinite(directionMaximumSpeedMetersPerSecond) || directionMaximumSpeedMetersPerSecond <= 0d)
+            throw new ArgumentOutOfRangeException(nameof(directionMaximumSpeedMetersPerSecond));
+        if (MaximumSpeedMetersPerSecond[MaximumSpeedMetersPerSecond.Count - 1] != 0d ||
+            StoppingSpeedLimitsMetersPerSecond[StoppingSpeedLimitsMetersPerSecond.Count - 1] != 0d)
+        {
+            throw new ArgumentException("Terminal PathS speed limits must be exactly zero.");
+        }
+
+        DirectionMaximumSpeedMetersPerSecond = directionMaximumSpeedMetersPerSecond;
+    }
+
+    public IReadOnlyList<double> PathS { get; }
+
+    public IReadOnlyList<double> MaximumSpeedMetersPerSecond { get; }
+
+    public IReadOnlyList<double> LateralAccelerationSpeedLimitsMetersPerSecond { get; }
+
+    public IReadOnlyList<double> CurvatureRateSpeedLimitsMetersPerSecond { get; }
+
+    public IReadOnlyList<double> StoppingSpeedLimitsMetersPerSecond { get; }
+
+    public double DirectionMaximumSpeedMetersPerSecond { get; }
+
+    public double TerminalPathS { get { return PathS[PathS.Count - 1]; } }
+
+    public double MaximumSpeedAt(double pathS)
+    {
+        return Interpolate(MaximumSpeedMetersPerSecond, pathS);
+    }
+
+    public double LateralAccelerationLimitAt(double pathS)
+    {
+        return Interpolate(LateralAccelerationSpeedLimitsMetersPerSecond, pathS);
+    }
+
+    public double CurvatureRateLimitAt(double pathS)
+    {
+        return Interpolate(CurvatureRateSpeedLimitsMetersPerSecond, pathS);
+    }
+
+    public double StoppingLimitAt(double pathS)
+    {
+        return Interpolate(StoppingSpeedLimitsMetersPerSecond, pathS);
+    }
+
+    private double Interpolate(IReadOnlyList<double> values, double pathS)
+    {
+        if (!IsFinite(pathS) || pathS < PathS[0] - StationTolerance || pathS > TerminalPathS + StationTolerance)
+            throw new ArgumentOutOfRangeException(nameof(pathS));
+        if (pathS <= PathS[0])
+            return values[0];
+        if (pathS >= TerminalPathS)
+            return values[values.Count - 1];
+
+        for (int index = 1; index < PathS.Count; index++)
+        {
+            if (pathS <= PathS[index])
+            {
+                double fraction = (pathS - PathS[index - 1]) / (PathS[index] - PathS[index - 1]);
+                return values[index - 1] + (values[index] - values[index - 1]) * fraction;
+            }
+        }
+        return values[values.Count - 1];
+    }
+
+    private static IReadOnlyList<double> CopyStrictStations(IReadOnlyList<double> source, string parameterName)
+    {
+        if (source == null || source.Count < 2)
+            throw new ArgumentException("At least two PathS stations are required.", parameterName);
+        var copy = new List<double>(source.Count);
+        double previous = double.NegativeInfinity;
+        for (int index = 0; index < source.Count; index++)
+        {
+            if (!IsFinite(source[index]) || source[index] <= previous)
+                throw new ArgumentException("PathS stations must be finite and strictly increasing.", parameterName);
+            copy.Add(source[index]);
+            previous = source[index];
+        }
+        return new ReadOnlyCollection<double>(copy);
+    }
+
+    private static IReadOnlyList<double> CopyFiniteNonnegative(IReadOnlyList<double> source, int expectedCount,
+        string parameterName)
+    {
+        if (source == null || source.Count != expectedCount)
+            throw new ArgumentException("Speed limit count must match PathS stations.", parameterName);
+        var copy = new List<double>(source.Count);
+        for (int index = 0; index < source.Count; index++)
+        {
+            if (!IsFinite(source[index]) || source[index] < 0d)
+                throw new ArgumentOutOfRangeException(parameterName);
+            copy.Add(source[index]);
+        }
+        return new ReadOnlyCollection<double>(copy);
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs

@@ -0,0 +1,188 @@
+using System;
+using System.Collections.Generic;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Builds curvature-aware, stopping-aware speed limits over actual optimized PathS.</summary>
+public sealed class PathSpeedLimitBuilder
+{
+    internal const double CurvatureEpsilon = 1e-10d;
+    private const double StopDistanceToleranceMeters = 1e-8d;
+
+    public EmPlanningStatus Build(LongitudinalPlanningInput input, out PathSpeedLimit speedLimit, out string failureReason)
+    {
+        speedLimit = null;
+        failureReason = string.Empty;
+        if (input == null)
+        {
+            failureReason = "Longitudinal planning input is required.";
+            return EmPlanningStatus.InvalidInput;
+        }
+
+        if (!TryGetLimits(input, out double directionMaximum, out double maximumAcceleration, out double maximumDeceleration,
+                out double maximumJerk, out double maximumLateralAcceleration, out double maximumCurvatureRate,
+                out failureReason))
+        {
+            return EmPlanningStatus.InvalidInput;
+        }
+        if (input.InitialProgressSpeedMetersPerSecond > directionMaximum + StopDistanceToleranceMeters ||
+            input.InitialAccelerationMetersPerSecondSquared < -maximumDeceleration - StopDistanceToleranceMeters ||
+            input.InitialAccelerationMetersPerSecondSquared > maximumAcceleration + StopDistanceToleranceMeters)
+        {
+            failureReason = "The initial longitudinal state violates the configured hard bounds.";
+            return EmPlanningStatus.InvalidInput;
+        }
+
+        LongitudinalStoppingProfile stopProfile = LongitudinalStoppingMath.Calculate(input.InitialProgressSpeedMetersPerSecond,
+            input.InitialAccelerationMetersPerSecondSquared, maximumDeceleration, maximumJerk);
+        if (stopProfile.DistanceMeters + StopDistanceToleranceMeters > input.TerminalPathS)
+        {
+            failureReason = "The available actual PathS distance is insufficient for the jerk-limited stop.";
+            return EmPlanningStatus.StoppingDistanceInsufficient;
+        }
+
+        int count = input.Path.Points.Count;
+        var pathS = new double[count];
+        var maximum = new double[count];
+        var lateral = new double[count];
+        var curvatureRate = new double[count];
+        var stopping = new double[count];
+        for (int index = 0; index < count; index++)
+        {
+            LateralPathPoint point = input.Path.Points[index];
+            pathS[index] = point.PathS;
+            double lateralLimit = Math.Sqrt(maximumLateralAcceleration /
+                Math.Max(Math.Abs(point.VehicleCurvature), CurvatureEpsilon));
+            double curvatureRateLimit = maximumCurvatureRate /
+                Math.Max(Math.Abs(point.VehicleCurvatureDerivative), CurvatureEpsilon);
+            double remainingDistance = Math.Max(0d, input.TerminalPathS - point.PathS);
+            double stoppingLimit = Math.Sqrt(2d * maximumDeceleration * remainingDistance);
+            lateral[index] = ClampFinite(lateralLimit, directionMaximum);
+            curvatureRate[index] = ClampFinite(curvatureRateLimit, directionMaximum);
+            stopping[index] = index == count - 1 ? 0d : ClampFinite(stoppingLimit, directionMaximum);
+            maximum[index] = index == count - 1 ? 0d : Math.Min(directionMaximum,
+                Math.Min(lateral[index], Math.Min(curvatureRate[index], stopping[index])));
+        }
+
+        try
+        {
+            speedLimit = new PathSpeedLimit(pathS, maximum, lateral, curvatureRate, stopping, directionMaximum);
+            return EmPlanningStatus.Success;
+        }
+        catch (ArgumentException exception)
+        {
+            failureReason = exception.Message;
+            return EmPlanningStatus.InvalidInput;
+        }
+    }
+
+    internal static bool TryGetLimits(LongitudinalPlanningInput input, out double directionMaximum,
+        out double maximumAcceleration, out double maximumDeceleration, out double maximumJerk,
+        out double maximumLateralAcceleration, out double maximumCurvatureRate, out string failureReason)
+    {
+        directionMaximum = 0d;
+        maximumAcceleration = 0d;
+        maximumDeceleration = 0d;
+        maximumJerk = 0d;
+        maximumLateralAcceleration = 0d;
+        maximumCurvatureRate = 0d;
+        failureReason = string.Empty;
+        if (input.Configuration == null || input.Configuration.Longitudinal == null)
+        {
+            failureReason = "Longitudinal configuration is required.";
+            return false;
+        }
+
+        LongitudinalConfiguration configuration = input.Configuration.Longitudinal;
+        directionMaximum = input.DirectionMaximumSpeedMetersPerSecond;
+        maximumAcceleration = configuration.MaximumAccelerationMetersPerSecondSquared;
+        maximumDeceleration = configuration.MaximumDecelerationMetersPerSecondSquared;
+        maximumJerk = configuration.MaximumJerkMetersPerSecondCubed;
+        maximumLateralAcceleration = configuration.MaximumLateralAccelerationMetersPerSecondSquared;
+        maximumCurvatureRate = configuration.MaximumCurvatureRatePerMeterPerSecond;
+        if (!IsPositiveFinite(directionMaximum) || !IsPositiveFinite(maximumAcceleration) ||
+            !IsPositiveFinite(maximumDeceleration) || !IsPositiveFinite(maximumJerk) ||
+            !IsPositiveFinite(maximumLateralAcceleration) || !IsPositiveFinite(maximumCurvatureRate))
+        {
+            failureReason = "Longitudinal limits must be positive and finite.";
+            return false;
+        }
+        return true;
+    }
+
+    private static double ClampFinite(double value, double maximum)
+    {
+        if (!IsFinite(value) || value < 0d)
+            throw new ArgumentOutOfRangeException(nameof(value));
+        return Math.Min(maximum, value);
+    }
+
+    private static bool IsPositiveFinite(double value)
+    {
+        return IsFinite(value) && value > 0d;
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}
+
+internal sealed class LongitudinalStoppingProfile
+{
+    public LongitudinalStoppingProfile(double distanceMeters, double durationSeconds)
+    {
+        DistanceMeters = distanceMeters;
+        DurationSeconds = durationSeconds;
+    }
+
+    public double DistanceMeters { get; }
+
+    public double DurationSeconds { get; }
+}
+
+internal static class LongitudinalStoppingMath
+{
+    public static LongitudinalStoppingProfile Calculate(double speedMetersPerSecond, double accelerationMetersPerSecondSquared,
+        double maximumDecelerationMetersPerSecondSquared, double maximumJerkMetersPerSecondCubed)
+    {
+        if (!IsFinite(speedMetersPerSecond) || !IsFinite(accelerationMetersPerSecondSquared) ||
+            !IsPositiveFinite(maximumDecelerationMetersPerSecondSquared) || !IsPositiveFinite(maximumJerkMetersPerSecondCubed))
+        {
+            throw new ArgumentOutOfRangeException(nameof(speedMetersPerSecond));
+        }
+        if (speedMetersPerSecond <= 0d)
+            return new LongitudinalStoppingProfile(0d, 0d);
+
+        double acceleration = Math.Max(-maximumDecelerationMetersPerSecondSquared, accelerationMetersPerSecondSquared);
+        double rampDuration = (acceleration + maximumDecelerationMetersPerSecondSquared) / maximumJerkMetersPerSecondCubed;
+        double speedAfterRamp = speedMetersPerSecond + acceleration * rampDuration -
+            0.5d * maximumJerkMetersPerSecondCubed * rampDuration * rampDuration;
+        if (speedAfterRamp <= 0d)
+        {
+            double root = (acceleration + Math.Sqrt(acceleration * acceleration + 2d * maximumJerkMetersPerSecondCubed *
+                speedMetersPerSecond)) / maximumJerkMetersPerSecondCubed;
+            double distance = speedMetersPerSecond * root + 0.5d * acceleration * root * root -
+                maximumJerkMetersPerSecondCubed * root * root * root / 6d;
+            return new LongitudinalStoppingProfile(Math.Max(0d, distance), root);
+        }
+
+        double rampDistance = speedMetersPerSecond * rampDuration + 0.5d * acceleration * rampDuration * rampDuration -
+            maximumJerkMetersPerSecondCubed * rampDuration * rampDuration * rampDuration / 6d;
+        double constantDecelerationDuration = speedAfterRamp / maximumDecelerationMetersPerSecondSquared;
+        double constantDecelerationDistance = speedAfterRamp * speedAfterRamp /
+            (2d * maximumDecelerationMetersPerSecondSquared);
+        return new LongitudinalStoppingProfile(rampDistance + constantDecelerationDistance,
+            rampDuration + constantDecelerationDuration);
+    }
+
+    private static bool IsPositiveFinite(double value)
+    {
+        return IsFinite(value) && value > 0d;
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/PlanningHorizonSelector.cs

@@ -0,0 +1,156 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Reference-distance terminal chosen before LS without crossing the current direction segment.</summary>
+public sealed class PlanningHorizonSelection
+{
+    internal PlanningHorizonSelection(double terminalReferenceS, EmTerminalType terminalType)
+    {
+        TerminalReferenceS = terminalReferenceS;
+        TerminalType = terminalType;
+    }
+
+    public double TerminalReferenceS { get; }
+
+    public EmTerminalType TerminalType { get; }
+}
+
+public sealed class PlanningHorizonSelector
+{
+    private const double BoundaryTolerance = 1e-8d;
+
+    public EmPlanningStatus Select(DirectionSegmentView segment, double currentSegmentReferenceS,
+        double initialProgressSpeedMetersPerSecond, double initialAccelerationMetersPerSecondSquared,
+        EmPlannerConfiguration configuration, out PlanningHorizonSelection selection, out string failureReason)
+    {
+        selection = null;
+        failureReason = string.Empty;
+        if (segment == null || configuration == null || configuration.Scheduling == null || configuration.Longitudinal == null ||
+            !IsFinite(currentSegmentReferenceS) || currentSegmentReferenceS < 0d ||
+            currentSegmentReferenceS > segment.LengthMeters + BoundaryTolerance ||
+            !IsFinite(initialProgressSpeedMetersPerSecond) || initialProgressSpeedMetersPerSecond < 0d ||
+            !IsFinite(initialAccelerationMetersPerSecondSquared))
+        {
+            failureReason = "Planning horizon inputs are invalid.";
+            return EmPlanningStatus.InvalidInput;
+        }
+
+        LongitudinalConfiguration longitudinal = configuration.Longitudinal;
+        SchedulingConfiguration scheduling = configuration.Scheduling;
+        double directionMaximum = segment.Direction == TravelDirection.Forward
+            ? longitudinal.MaximumForwardSpeedMetersPerSecond
+            : longitudinal.MaximumReverseSpeedMetersPerSecond;
+        if (!IsPositiveFinite(directionMaximum) || !IsPositiveFinite(longitudinal.MaximumAccelerationMetersPerSecondSquared) ||
+            !IsPositiveFinite(longitudinal.MaximumDecelerationMetersPerSecondSquared) ||
+            !IsPositiveFinite(longitudinal.MaximumJerkMetersPerSecondCubed) ||
+            !IsPositiveFinite(longitudinal.ZeroSpeedHoldSeconds) || !IsPositiveFinite(scheduling.TimeHorizonSeconds) ||
+            !IsPositiveFinite(scheduling.DistanceHorizonMeters))
+        {
+            failureReason = "Planning horizon configuration is invalid.";
+            return EmPlanningStatus.InvalidInput;
+        }
+        if (initialProgressSpeedMetersPerSecond > directionMaximum + BoundaryTolerance ||
+            initialAccelerationMetersPerSecondSquared < -longitudinal.MaximumDecelerationMetersPerSecondSquared - BoundaryTolerance ||
+            initialAccelerationMetersPerSecondSquared > longitudinal.MaximumAccelerationMetersPerSecondSquared + BoundaryTolerance)
+        {
+            failureReason = "The initial state violates longitudinal bounds.";
+            return EmPlanningStatus.InvalidInput;
+        }
+
+        double remainingSegment = Math.Max(0d, segment.LengthMeters - currentSegmentReferenceS);
+        LongitudinalStoppingProfile initialStop = LongitudinalStoppingMath.Calculate(initialProgressSpeedMetersPerSecond,
+            initialAccelerationMetersPerSecondSquared, longitudinal.MaximumDecelerationMetersPerSecondSquared,
+            longitudinal.MaximumJerkMetersPerSecondCubed);
+        if (initialStop.DistanceMeters + BoundaryTolerance > remainingSegment)
+        {
+            failureReason = "The current segment lacks the jerk-limited stopping distance.";
+            return EmPlanningStatus.StoppingDistanceInsufficient;
+        }
+
+        double timeReachable = CalculateReachableDistance(initialProgressSpeedMetersPerSecond,
+            initialAccelerationMetersPerSecondSquared, directionMaximum, longitudinal, scheduling.TimeHorizonSeconds);
+        double terminalReferenceS = currentSegmentReferenceS + Math.Min(remainingSegment,
+            Math.Min(scheduling.DistanceHorizonMeters, timeReachable));
+        if (terminalReferenceS >= segment.LengthMeters - BoundaryTolerance)
+        {
+            terminalReferenceS = segment.LengthMeters;
+            selection = new PlanningHorizonSelection(terminalReferenceS, ToTerminalType(segment.EndBoundary.BoundaryType));
+        }
+        else
+        {
+            selection = new PlanningHorizonSelection(terminalReferenceS, EmTerminalType.RollingSafetyStop);
+        }
+        return EmPlanningStatus.Success;
+    }
+
+    private static double CalculateReachableDistance(double initialSpeed, double initialAcceleration, double maximumSpeed,
+        LongitudinalConfiguration configuration, double timeHorizonSeconds)
+    {
+        LongitudinalStoppingProfile stopAtMaximumSpeed = LongitudinalStoppingMath.Calculate(maximumSpeed, 0d,
+            configuration.MaximumDecelerationMetersPerSecondSquared, configuration.MaximumJerkMetersPerSecondCubed);
+        double drivingDuration = timeHorizonSeconds - configuration.ZeroSpeedHoldSeconds - stopAtMaximumSpeed.DurationSeconds;
+        if (drivingDuration <= 0d)
+            return Math.Min(initialSpeed, maximumSpeed) * Math.Max(0d, timeHorizonSeconds - configuration.ZeroSpeedHoldSeconds);
+
+        double speed = initialSpeed;
+        double acceleration = initialAcceleration;
+        double distance = 0d;
+        const double simulationStepSeconds = 0.001d;
+        while (drivingDuration > 0d)
+        {
+            double step = Math.Min(simulationStepSeconds, drivingDuration);
+            double jerk = ChooseAccelerationJerk(speed, acceleration, maximumSpeed,
+                configuration.MaximumAccelerationMetersPerSecondSquared, configuration.MaximumJerkMetersPerSecondCubed);
+            double nextSpeed = speed + acceleration * step + 0.5d * jerk * step * step;
+            if (nextSpeed > maximumSpeed)
+            {
+                nextSpeed = maximumSpeed;
+                acceleration = 0d;
+                jerk = 0d;
+            }
+            distance += speed * step + 0.5d * acceleration * step * step + jerk * step * step * step / 6d;
+            acceleration += jerk * step;
+            speed = Math.Max(0d, nextSpeed);
+            drivingDuration -= step;
+        }
+        return Math.Max(0d, distance + stopAtMaximumSpeed.DistanceMeters);
+    }
+
+    private static double ChooseAccelerationJerk(double speed, double acceleration, double maximumSpeed,
+        double maximumAcceleration, double maximumJerk)
+    {
+        if (speed >= maximumSpeed - BoundaryTolerance)
+        {
+            if (acceleration > 0d)
+                return -maximumJerk;
+            return acceleration < 0d ? maximumJerk : 0d;
+        }
+        double speedToReduceAccelerationToZero = acceleration > 0d
+            ? acceleration * acceleration / (2d * maximumJerk)
+            : 0d;
+        if (speed + speedToReduceAccelerationToZero >= maximumSpeed - BoundaryTolerance)
+            return acceleration > 0d ? -maximumJerk : 0d;
+        return acceleration < maximumAcceleration - BoundaryTolerance ? maximumJerk : 0d;
+    }
+
+    private static EmTerminalType ToTerminalType(EmBoundaryType boundaryType)
+    {
+        return boundaryType == EmBoundaryType.Goal
+            ? EmTerminalType.Goal
+            : boundaryType == EmBoundaryType.GearSwitchApproach || boundaryType == EmBoundaryType.GearSwitchDeparture
+                ? EmTerminalType.GearSwitch
+                : EmTerminalType.RollingSafetyStop;
+    }
+
+    private static bool IsPositiveFinite(double value)
+    {
+        return IsFinite(value) && value > 0d;
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}

ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs

@@ -0,0 +1,149 @@
+using System;
+using System.Collections.Generic;
+using EMPlannerVerificationHost;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+internal static class LongitudinalModelChecks
+{
+    public static void Run()
+    {
+        VerifiesFinitePathSIndexedSpeedEnvelope();
+        VerifiesStoppingPrecheckBeforeQpAssembly();
+        VerifiesReferenceHorizonSelectionKeepsTheCurrentSegmentBoundary();
+    }
+
+    private static void VerifiesFinitePathSIndexedSpeedEnvelope()
+    {
+        LateralPath directionPath = CreatePath(new[]
+        {
+            new PathFixture(0d, 0d, 0d, 0d),
+            new PathFixture(1d, 1d, 0d, 0d),
+        });
+        var directionInput = new LongitudinalPlanningInput(directionPath, TravelDirection.Forward, 0d, 0d,
+            EmTerminalType.Goal, EmPlannerConfiguration.CreateDefault(), Array.Empty<double>(), Array.Empty<double>());
+        EmPlanningStatus directionStatus = new PathSpeedLimitBuilder().Build(directionInput,
+            out PathSpeedLimit directionEnvelope, out string directionFailureReason);
+        Verification.Equal(EmPlanningStatus.Success, directionStatus, "default direction speed limit status: " +
+            directionFailureReason);
+        Verification.NearlyEqual(0.20d, directionEnvelope.DirectionMaximumSpeedMetersPerSecond,
+            "default direction speed limit");
+
+        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
+        configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 1d;
+        configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond = 1d;
+        LateralPath path = CreatePath(new[]
+        {
+            new PathFixture(10d, 0d, 0d, 0d),
+            new PathFixture(20d, 2d, 2d, 4d),
+            new PathFixture(20.5d, 4d, 20d, 0d),
+            new PathFixture(21d, 5d, 0d, 0d),
+        });
+        var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0d, 0d,
+            EmTerminalType.Goal, configuration, Array.Empty<double>(), Array.Empty<double>());
+
+        EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
+            out string failureReason);
+        Verification.Equal(EmPlanningStatus.Success, status, "speed envelope status: " + failureReason);
+        Verification.NearlyEqual(1d, envelope.DirectionMaximumSpeedMetersPerSecond, "overridden direction speed limit");
+        Verification.NearlyEqual(Math.Sqrt(0.20d / 2d), envelope.LateralAccelerationLimitAt(2d),
+            "curvature lateral-acceleration limit");
+        Verification.NearlyEqual(0.50d / 4d, envelope.CurvatureRateLimitAt(2d), "curvature-rate limit");
+        Verification.True(double.IsFinite(envelope.LateralAccelerationLimitAt(0d)) &&
+            double.IsFinite(envelope.CurvatureRateLimitAt(0d)), "zero curvature limits stay finite");
+        Verification.NearlyEqual(Math.Sqrt(2d * 0.30d * (5d - 4d)), envelope.StoppingLimitAt(4d),
+            "stopping speed limit");
+        Verification.NearlyEqual(Math.Sqrt(0.20d / 20d), envelope.MaximumSpeedAt(4d),
+            "combined limit chooses the finite minimum");
+        Verification.NearlyEqual((envelope.MaximumSpeedAt(0d) + envelope.MaximumSpeedAt(2d)) / 2d,
+            envelope.MaximumSpeedAt(1d), "speed envelope interpolates by PathS rather than ReferenceS");
+        Verification.NearlyEqual(0d, envelope.MaximumSpeedAt(5d), "terminal speed is exactly zero");
+    }
+
+    private static void VerifiesStoppingPrecheckBeforeQpAssembly()
+    {
+        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
+        LateralPath shortPath = CreatePath(new[]
+        {
+            new PathFixture(0d, 0d, 0d, 0d),
+            new PathFixture(100d, 0.01d, 0d, 0d),
+        });
+        var input = new LongitudinalPlanningInput(shortPath, TravelDirection.Forward, 0.20d, 0.20d,
+            EmTerminalType.RollingSafetyStop, configuration, Array.Empty<double>(), Array.Empty<double>());
+
+        EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
+            out string failureReason);
+        Verification.Equal(EmPlanningStatus.StoppingDistanceInsufficient, status,
+            "jerk/deceleration stopping precheck status");
+        Verification.True(envelope == null, "stopping-distance failure does not create a speed envelope");
+        Verification.True(failureReason.Length != 0, "stopping-distance failure explains the rejection");
+    }
+
+    private static void VerifiesReferenceHorizonSelectionKeepsTheCurrentSegmentBoundary()
+    {
+        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
+        DirectionSegmentView shortGoal = CreateSegment(0.25d, EmBoundaryType.Goal);
+        EmPlanningStatus status = new PlanningHorizonSelector().Select(shortGoal, 0d, 0d, 0d, configuration,
+            out PlanningHorizonSelection goalSelection, out string goalFailure);
+
+        Verification.Equal(EmPlanningStatus.Success, status, "goal horizon status: " + goalFailure);
+        Verification.Equal(EmTerminalType.Goal, goalSelection.TerminalType, "goal terminal type");
+        Verification.NearlyEqual(0.25d, goalSelection.TerminalReferenceS, "goal terminal reference S");
+
+        DirectionSegmentView longSegment = CreateSegment(4d, EmBoundaryType.GearSwitchApproach);
+        status = new PlanningHorizonSelector().Select(longSegment, 0d, 0d, 0d, configuration,
+            out PlanningHorizonSelection rollingSelection, out string rollingFailure);
+        Verification.Equal(EmPlanningStatus.Success, status, "rolling horizon status: " + rollingFailure);
+        Verification.Equal(EmTerminalType.RollingSafetyStop, rollingSelection.TerminalType, "rolling terminal type");
+        Verification.True(rollingSelection.TerminalReferenceS >= 0d && rollingSelection.TerminalReferenceS < 4d,
+            "rolling horizon remains within the current segment");
+    }
+
+    private static LateralPath CreatePath(IReadOnlyList<PathFixture> fixtures)
+    {
+        var points = new List<LateralPathPoint>(fixtures.Count);
+        for (int index = 0; index < fixtures.Count; index++)
+        {
+            PathFixture fixture = fixtures[index];
+            points.Add(new LateralPathPoint(fixture.ReferenceS, fixture.PathS, 0d, 0d, 0d, 0d, fixture.PathS, 0d,
+                0d, fixture.Curvature, fixture.Curvature, fixture.CurvatureDerivative));
+        }
+        return new LateralPath(points, true);
+    }
+
+    private static DirectionSegmentView CreateSegment(double length, EmBoundaryType endBoundaryType)
+    {
+        var points = new List<SmoothedPathPoint>
+        {
+            Point(0d, 0d),
+            Point(length, length),
+        };
+        return new DirectionSegmentView(0, TravelDirection.Forward, points,
+            new ReferenceBoundary(0, 0d, EmBoundaryType.None, 0d),
+            new ReferenceBoundary(0, length, endBoundaryType, length), 0d);
+    }
+
+    private static SmoothedPathPoint Point(double x, double s)
+    {
+        return new SmoothedPathPoint(x, 0d, 0d, 0d, s, TravelDirection.Forward, 0d, 0d, 0d, 1d, false,
+            SmoothedPathPointSource.Anchor);
+    }
+
+    private sealed class PathFixture
+    {
+        public PathFixture(double referenceS, double pathS, double curvature, double curvatureDerivative)
+        {
+            ReferenceS = referenceS;
+            PathS = pathS;
+            Curvature = curvature;
+            CurvatureDerivative = curvatureDerivative;
+        }
+
+        public double ReferenceS { get; }
+        public double PathS { get; }
+        public double Curvature { get; }
+        public double CurvatureDerivative { get; }
+    }
+}

ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs

@@ -9,7 +9,8 @@ internal static class Program
         if (args.Length != 1 || (args[0] != "foundation" && args[0] != "segmentation" && args[0] != "frenet" &&
             args[0] != "corridor" && args[0] != "optimization" && args[0] != "osqp" && args[0] != "osqp-loader" && args[0] != "osqp-probe" &&
             args[0] != "all-foundation" && args[0] != "lateral-model" && args[0] != "lateral-integration" &&
-            args[0] != "lateral-real-osqp" && args[0] != "lateral-real-osqp-probe" && args[0] != "lateral-all"))
+            args[0] != "lateral-real-osqp" && args[0] != "lateral-real-osqp-probe" && args[0] != "lateral-all" &&
+            args[0] != "longitudinal-model"))
         {
             Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation|frenet|corridor|optimization|osqp|osqp-loader|all-foundation|lateral-model|lateral-integration|lateral-real-osqp|lateral-all");
             return 2;
@@ -71,6 +72,11 @@ internal static class Program
                 MultiWheelC.TrajectoryPlanning.EMPlanner.LateralIntegrationChecks.RunRealOsqpInCleanPluginBundle();
                 Console.WriteLine("PASS lateral-real-osqp");
             }
+            if (args[0] == "longitudinal-model")
+            {
+                MultiWheelC.TrajectoryPlanning.EMPlanner.LongitudinalModelChecks.Run();
+                Console.WriteLine("PASS longitudinal-model");
+            }
             return 0;
         }
         catch (Exception exception)