feat: separate rolling horizons from stop boundaries

2026-08-05 17:13:09 +08:00
parent e56220fc29
commit 21b20d09d4
9 changed files with 246 additions and 96 deletions
@@ -0,0 +1,8 @@
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public enum EmLongitudinalMode
+{
+    RollingContinuation,
+    ApproachStopBoundary,
+    ExactStopAtBoundary,
+}
@@ -58,11 +58,11 @@ public sealed class EmPlanningService : IEmPlanningService
                initialAcceleration, configuration, out PlanningHorizonSelection horizon, out string horizonReason);
            if (horizonStatus != EmPlanningStatus.Success)
                return Failure(horizonStatus, request, horizonReason);
-            ReferenceHorizonSlice slice = ReferenceHorizonSlicer.Slice(segment, horizon.TerminalReferenceS);
+            ReferenceHorizonSlice slice = ReferenceHorizonSlicer.Slice(segment, horizon.WindowEndReferenceS);
            EmitDebug(request, "exact horizon and terminal selection succeeded");

            IReadOnlyList<FrenetProjection> previousSeed = ProjectPreviousTrajectorySeed(request.PreviousTrajectory, segment,
-                startProjection.ReferenceS, horizon.TerminalReferenceS, configuration.Frenet.MaximumProjectionDistanceMeters);
+                startProjection.ReferenceS, horizon.WindowEndReferenceS, configuration.Frenet.MaximumProjectionDistanceMeters);
            var corridorSeed = new List<FrenetProjection>(previousSeed.Count + 1) { startProjection };
            for (int index = 0; index < previousSeed.Count; index++) corridorSeed.Add(previousSeed[index]);
            EmitDebug(request, "previous-trajectory seed projection completed");
@@ -83,7 +83,8 @@ public sealed class EmPlanningService : IEmPlanningService
            EmitDebug(request, "LS optimization and validation succeeded");

            var longitudinalInput = new LongitudinalPlanningInput(lateral.Path, segment.Direction, initialProgressSpeed,
-                initialAcceleration, horizon.TerminalType, configuration, Array.Empty<double>(), Array.Empty<double>());
+                initialAcceleration, horizon.TerminalType, horizon.LongitudinalMode, configuration,
+                Array.Empty<double>(), Array.Empty<double>());
            EmPlanningStatus envelopeStatus = new PathSpeedLimitBuilder().Build(longitudinalInput, out _, out string envelopeReason);
            if (envelopeStatus != EmPlanningStatus.Success)
                return Failure(envelopeStatus, request, envelopeReason);
@@ -11,7 +11,8 @@ public sealed class LongitudinalPlanningInput
    private const double PathSTolerance = 1e-12d;

    public LongitudinalPlanningInput(LateralPath path, TravelDirection direction, double initialProgressSpeedMetersPerSecond,
-        double initialAccelerationMetersPerSecondSquared, EmTerminalType terminalType, EmPlannerConfiguration configuration,
+        double initialAccelerationMetersPerSecondSquared, EmTerminalType terminalType, EmLongitudinalMode mode,
+        EmPlannerConfiguration configuration,
        IReadOnlyList<double> previousPathS, IReadOnlyList<double> previousProgressSpeedMetersPerSecond)
    {
        if (path == null || !path.IsIndependentlyValidated || path.Points.Count < 2)
@@ -25,6 +26,12 @@ public sealed class LongitudinalPlanningInput
            throw new ArgumentOutOfRangeException(nameof(initialAccelerationMetersPerSecondSquared));
        if (!Enum.IsDefined(typeof(EmTerminalType), terminalType))
            throw new ArgumentOutOfRangeException(nameof(terminalType));
+        if (!Enum.IsDefined(typeof(EmLongitudinalMode), mode))
+            throw new ArgumentOutOfRangeException(nameof(mode));
+        if (mode == EmLongitudinalMode.RollingContinuation && terminalType != EmTerminalType.RollingSafetyStop)
+            throw new ArgumentException("Rolling continuation requires a rolling window boundary.");
+        if (mode != EmLongitudinalMode.RollingContinuation && terminalType == EmTerminalType.RollingSafetyStop)
+            throw new ArgumentException("Stop-boundary modes require Goal or GearSwitch.");
        if (configuration == null)
            throw new ArgumentNullException(nameof(configuration));

@@ -33,6 +40,7 @@ public sealed class LongitudinalPlanningInput
        InitialProgressSpeedMetersPerSecond = initialProgressSpeedMetersPerSecond;
        InitialAccelerationMetersPerSecondSquared = initialAccelerationMetersPerSecondSquared;
        TerminalType = terminalType;
+        Mode = mode;
        Configuration = configuration.Copy();
        PreviousPathS = CopyFiniteNonnegative(previousPathS, nameof(previousPathS));
        PreviousProgressSpeedMetersPerSecond = CopyFiniteNonnegative(previousProgressSpeedMetersPerSecond,
@@ -42,6 +50,17 @@ public sealed class LongitudinalPlanningInput
                nameof(previousProgressSpeedMetersPerSecond));
    }

+    public LongitudinalPlanningInput(LateralPath path, TravelDirection direction, double initialProgressSpeedMetersPerSecond,
+        double initialAccelerationMetersPerSecondSquared, EmTerminalType terminalType, EmPlannerConfiguration configuration,
+        IReadOnlyList<double> previousPathS, IReadOnlyList<double> previousProgressSpeedMetersPerSecond)
+        : this(path, direction, initialProgressSpeedMetersPerSecond, initialAccelerationMetersPerSecondSquared, terminalType,
+            terminalType == EmTerminalType.RollingSafetyStop
+                ? EmLongitudinalMode.RollingContinuation
+                : EmLongitudinalMode.ExactStopAtBoundary,
+            configuration, previousPathS, previousProgressSpeedMetersPerSecond)
+    {
+    }
+
    public LateralPath Path { get; }

    public TravelDirection Direction { get; }
@@ -52,13 +71,33 @@ public sealed class LongitudinalPlanningInput

    public EmTerminalType TerminalType { get; }

+    public EmLongitudinalMode Mode { 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 PathUpperBoundS { get { return Path.Points[Path.Points.Count - 1].PathS; } }
+
+    public bool HasStopBoundary { get { return TerminalType != EmTerminalType.RollingSafetyStop; } }
+
+    public double StopBoundaryPathS { get { return HasStopBoundary ? PathUpperBoundS : double.NaN; } }
+
+    public EmBoundaryType StopBoundaryType
+    {
+        get
+        {
+            return TerminalType == EmTerminalType.Goal
+                ? EmBoundaryType.Goal
+                : TerminalType == EmTerminalType.GearSwitch
+                    ? EmBoundaryType.GearSwitchApproach
+                    : EmBoundaryType.None;
+        }
+    }
+
+    public double TerminalPathS { get { return PathUpperBoundS; } }

    public double DirectionMaximumSpeedMetersPerSecond
    {
@@ -0,0 +1,39 @@
+using System;
+using System.Collections.Generic;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+public static class LongitudinalTerminalSchedule
+{
+    public static int GetStabilizationStartIndex(IReadOnlyList<double> knotTimes,
+        double minimumStabilizationDurationSeconds)
+    {
+        if (knotTimes == null || knotTimes.Count < 3 ||
+            double.IsNaN(minimumStabilizationDurationSeconds) ||
+            double.IsInfinity(minimumStabilizationDurationSeconds) ||
+            minimumStabilizationDurationSeconds <= 0d)
+        {
+            throw new ArgumentException("A positive stabilization tail and at least three knots are required.");
+        }
+
+        double previous = double.NegativeInfinity;
+        for (int index = 0; index < knotTimes.Count; index++)
+        {
+            if (double.IsNaN(knotTimes[index]) || double.IsInfinity(knotTimes[index]) ||
+                knotTimes[index] <= previous)
+            {
+                throw new ArgumentException("Terminal-schedule knots must be finite and strictly increasing.");
+            }
+            previous = knotTimes[index];
+        }
+
+        double finalTime = knotTimes[knotTimes.Count - 1];
+        for (int index = knotTimes.Count - 2; index >= 1; index--)
+        {
+            if (finalTime - knotTimes[index] >= minimumStabilizationDurationSeconds - 1e-12d)
+                return index;
+        }
+
+        throw new ArgumentException("The time horizon cannot contain a full terminal stabilization interval.");
+    }
+}
@@ -1,4 +1,5 @@
 using System;
+using System.Collections.Generic;
 using MultiWheelC.TrajectoryPlanning.CoarsePath;

 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
@@ -6,15 +7,31 @@ 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)
+    internal PlanningHorizonSelection(double windowEndReferenceS, EmBoundaryType windowEndBoundaryType,
+        EmTerminalType terminalType, EmLongitudinalMode longitudinalMode,
+        double stopBoundaryReferenceS, bool hasStopBoundary)
    {
-        TerminalReferenceS = terminalReferenceS;
+        WindowEndReferenceS = windowEndReferenceS;
+        WindowEndBoundaryType = windowEndBoundaryType;
        TerminalType = terminalType;
+        LongitudinalMode = longitudinalMode;
+        StopBoundaryReferenceS = stopBoundaryReferenceS;
+        HasStopBoundary = hasStopBoundary;
    }

-    public double TerminalReferenceS { get; }
+    public double WindowEndReferenceS { get; }
+
+    public double TerminalReferenceS => WindowEndReferenceS;
+
+    public EmBoundaryType WindowEndBoundaryType { get; }

    public EmTerminalType TerminalType { get; }
+
+    public EmLongitudinalMode LongitudinalMode { get; }
+
+    public double StopBoundaryReferenceS { get; }
+
+    public bool HasStopBoundary { get; }
 }

 public sealed class PlanningHorizonSelector
@@ -73,75 +90,41 @@ public sealed class PlanningHorizonSelector
            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)
+        double windowEnd = Math.Min(currentSegmentReferenceS + scheduling.DistanceHorizonMeters, segment.LengthMeters);
+        bool windowReachesSegmentEnd = windowEnd >= segment.LengthMeters - BoundaryTolerance;
+        bool hasStopBoundary = windowReachesSegmentEnd && IsStopBoundary(segment.EndBoundary.BoundaryType);
+        EmBoundaryType windowEndBoundaryType = windowReachesSegmentEnd
+            ? segment.EndBoundary.BoundaryType
+            : EmBoundaryType.RollingSafetyStop;
+        if (!hasStopBoundary)
        {
-            terminalReferenceS = segment.LengthMeters;
-            selection = new PlanningHorizonSelection(terminalReferenceS, ToTerminalType(segment.EndBoundary.BoundaryType));
-        }
-        else
-        {
-            selection = new PlanningHorizonSelection(terminalReferenceS, EmTerminalType.RollingSafetyStop);
-        }
+            selection = new PlanningHorizonSelection(windowEnd, windowEndBoundaryType,
+                EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation,
+                segment.LengthMeters, false);
            return EmPlanningStatus.Success;
        }

-    private static double CalculateReachableDistance(double initialSpeed, double initialAcceleration, double maximumSpeed,
-        LongitudinalConfiguration configuration, double timeHorizonSeconds)
-    {
-        if (!JerkLimitedStoppingMath.TryCalculate(maximumSpeed, 0d,
-                configuration.MaximumDecelerationMetersPerSecondSquared,
-                configuration.MaximumJerkMetersPerSecondCubed,
-                out JerkLimitedStoppingProfile stopAtMaximumSpeed, out _))
-        {
-            throw new ArgumentOutOfRangeException(nameof(configuration));
-        }
-        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);
+        IReadOnlyList<double> knotTimes = LongitudinalCandidate.CreateKnotTimes(
+            scheduling.TimeHorizonSeconds, scheduling.OutputTimeStepSeconds);
+        int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
+            knotTimes, scheduling.OutputTimeStepSeconds);
+        double availableMotionTime = knotTimes[stabilizationStart];
+        double maximumStoppedDistance = JerkLimitedStoppingMath.CalculateMaximumStoppedDistance(
+            initialProgressSpeedMetersPerSecond, initialAccelerationMetersPerSecondSquared, directionMaximum,
+            longitudinal.MaximumAccelerationMetersPerSecondSquared,
+            longitudinal.MaximumDecelerationMetersPerSecondSquared,
+            longitudinal.MaximumJerkMetersPerSecondCubed, availableMotionTime);
+        EmLongitudinalMode mode = remainingSegment <= maximumStoppedDistance + BoundaryTolerance
+            ? EmLongitudinalMode.ExactStopAtBoundary
+            : EmLongitudinalMode.ApproachStopBoundary;
+        selection = new PlanningHorizonSelection(segment.LengthMeters, windowEndBoundaryType,
+            ToTerminalType(segment.EndBoundary.BoundaryType), mode, segment.LengthMeters, true);
+        return EmPlanningStatus.Success;
    }

-    private static double ChooseAccelerationJerk(double speed, double acceleration, double maximumSpeed,
-        double maximumAcceleration, double maximumJerk)
+    private static bool IsStopBoundary(EmBoundaryType boundaryType)
    {
-        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;
+        return boundaryType == EmBoundaryType.Goal || boundaryType == EmBoundaryType.GearSwitchApproach;
    }

    private static EmTerminalType ToTerminalType(EmBoundaryType boundaryType)
@@ -1,4 +1,5 @@
 using System;
+using System.Collections.Generic;
 using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
 using MultiWheelC.TrajectoryPlanning.PathSmoothing;
 using MultiWheelC.TrajectoryPlanning.Utils;
@@ -142,6 +143,45 @@ public static class EmPlanningRequestValidator
            !WeightsAreValid(configuration.Lateral.Weights) || !WeightsAreValid(configuration.Longitudinal.Weights))
            return false;

+        if (!JerkLimitedStoppingMath.TryCalculate(longitudinal.MaximumForwardSpeedMetersPerSecond,
+                longitudinal.MaximumAccelerationMetersPerSecondSquared,
+                longitudinal.MaximumDecelerationMetersPerSecondSquared,
+                longitudinal.MaximumJerkMetersPerSecondCubed, out JerkLimitedStoppingProfile forwardStop,
+                out _) ||
+            !JerkLimitedStoppingMath.TryCalculate(longitudinal.MaximumReverseSpeedMetersPerSecond,
+                longitudinal.MaximumAccelerationMetersPerSecondSquared,
+                longitudinal.MaximumDecelerationMetersPerSecondSquared,
+                longitudinal.MaximumJerkMetersPerSecondCubed, out JerkLimitedStoppingProfile reverseStop,
+                out _))
+        {
+            failureReason = "Configuration cannot construct the jerk-limited stopping model.";
+            return false;
+        }
+
+        double maximumSpeed = Math.Max(longitudinal.MaximumForwardSpeedMetersPerSecond,
+            longitudinal.MaximumReverseSpeedMetersPerSecond);
+        double requiredDistanceHorizon = Math.Max(forwardStop.DistanceMeters, reverseStop.DistanceMeters) +
+            maximumSpeed * scheduling.ReplanPeriodSeconds;
+        if (scheduling.DistanceHorizonMeters + 1e-12d < requiredDistanceHorizon)
+        {
+            failureReason = "DistanceHorizonMeters is insufficient: required=" + requiredDistanceHorizon +
+                ";configured=" + scheduling.DistanceHorizonMeters;
+            return false;
+        }
+
+        try
+        {
+            IReadOnlyList<double> knotTimes = LongitudinalCandidate.CreateKnotTimes(
+                scheduling.TimeHorizonSeconds, scheduling.OutputTimeStepSeconds);
+            LongitudinalTerminalSchedule.GetStabilizationStartIndex(knotTimes,
+                scheduling.OutputTimeStepSeconds);
+        }
+        catch (ArgumentException exception)
+        {
+            failureReason = exception.Message;
+            return false;
+        }
+
        return true;
    }

@@ -106,6 +106,17 @@ internal static class FoundationChecks
        VerifyLateralWeights(configuration.Lateral.Weights);
        VerifyLongitudinalWeights(configuration.Longitudinal.Weights);

+        EmPlannerConfiguration insufficientPreview = EmPlannerConfiguration.CreateDefault();
+        insufficientPreview.Scheduling.DistanceHorizonMeters = 0.01d;
+        EmPlanningRequestValidationResult insufficientPreviewValidation = EmPlanningRequestValidator.Validate(
+            CreateValidRequest(insufficientPreview));
+        AssertStatus(EmPlanningStatus.InvalidInput, insufficientPreviewValidation, "insufficient stopping preview");
+        EMPlannerVerificationHost.Verification.True(
+            insufficientPreviewValidation.FailureReason.Contains("DistanceHorizonMeters") &&
+            insufficientPreviewValidation.FailureReason.Contains("required") &&
+            insufficientPreviewValidation.FailureReason.Contains("configured"),
+            "insufficient stopping preview describes configured and required distance");
+
        EmPlanningRequest valid = CreateValidRequest(configuration);
        AssertStatus(EmPlanningStatus.InvalidInput, EmPlanningRequestValidator.Validate(
            CreateRequest(null!, valid.Map, valid.Vehicle, valid.VehicleState, configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "null reference path");
@@ -170,7 +170,7 @@ internal static class LongitudinalIntegrationChecks
        }, true);
        var input = new LongitudinalPlanningInput(curvedPath, TravelDirection.Forward,
            baseline.InitialProgressSpeedMetersPerSecond, baseline.InitialAccelerationMetersPerSecondSquared,
-            baseline.TerminalType, baseline.Configuration, Array.Empty<double>(), Array.Empty<double>());
+            baseline.TerminalType, baseline.Mode, baseline.Configuration, Array.Empty<double>(), Array.Empty<double>());
        EmPlanningStatus speedStatus = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
            out string speedFailure);
        Verification.Equal(EmPlanningStatus.Success, speedStatus, "curved-envelope setup: " + speedFailure);
@@ -212,7 +212,7 @@ internal static class LongitudinalIntegrationChecks
        }, true);
        var input = new LongitudinalPlanningInput(curvedPath, TravelDirection.Forward,
            baseline.InitialProgressSpeedMetersPerSecond, baseline.InitialAccelerationMetersPerSecondSquared,
-            baseline.TerminalType, baseline.Configuration, Array.Empty<double>(), Array.Empty<double>());
+            baseline.TerminalType, baseline.Mode, baseline.Configuration, Array.Empty<double>(), Array.Empty<double>());
        var solver = new FakeQpSolver(new[]
        {
            Result(QpSolveStatus.Solved, ToPrimal(toleranceCandidate), 2d),
@@ -335,7 +335,8 @@ internal static class LongitudinalIntegrationChecks
            Point(2d, terminalPathS, 0d),
        };
        return new LongitudinalScenario(name, new LongitudinalPlanningInput(new LateralPath(points, true), direction,
-            initialSpeed, initialAcceleration, EmTerminalType.Goal, configuration, Array.Empty<double>(), Array.Empty<double>()));
+            initialSpeed, initialAcceleration, EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary,
+            configuration, Array.Empty<double>(), Array.Empty<double>()));
    }

    private static void VerifyRealScenario(LongitudinalScenario scenario, LongitudinalPlanningResult result)
@@ -392,7 +393,7 @@ internal static class LongitudinalIntegrationChecks
            Point(2d, terminalPathS, 0d),
        }, true);
        return new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.10d, 0d, EmTerminalType.Goal,
-            configuration, Array.Empty<double>(), Array.Empty<double>());
+            EmLongitudinalMode.ExactStopAtBoundary, configuration, Array.Empty<double>(), Array.Empty<double>());
    }

    private static LateralPathPoint Point(double referenceS, double pathS, double curvature)
@@ -16,7 +16,7 @@ internal static class LongitudinalModelChecks
        VerifiesStoppingEnvelopeIsRefinedOnActualPathS();
        VerifiesStoppingEnvelopeUsesDiscreteTimeTailStations();
        VerifiesStoppingPrecheckBeforeQpAssembly();
-        VerifiesReferenceHorizonSelectionKeepsTheCurrentSegmentBoundary();
+        VerifiesReferenceHorizonSelectionSeparatesSpaceAndTime();
        VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints();
    }

@@ -64,7 +64,8 @@ internal static class LongitudinalModelChecks
            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>());
+            EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, 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: " +
@@ -83,7 +84,8 @@ internal static class LongitudinalModelChecks
            new PathFixture(21d, 5d, 0d, 0d),
        });
        var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0d, 0d,
-            EmTerminalType.Goal, configuration, Array.Empty<double>(), Array.Empty<double>());
+            EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
+            Array.Empty<double>(), Array.Empty<double>());

        EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
            out string failureReason);
@@ -124,7 +126,8 @@ internal static class LongitudinalModelChecks
            new PathFixture(100d, 2d, 0d, 0d),
        });
        var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0d, 0d,
-            EmTerminalType.Goal, configuration, Array.Empty<double>(), Array.Empty<double>());
+            EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
+            Array.Empty<double>(), Array.Empty<double>());

        EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
            out string failureReason);
@@ -145,7 +148,8 @@ internal static class LongitudinalModelChecks
            new PathFixture(100d, 2d, 0d, 0d),
        });
        var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0d, 0d,
-            EmTerminalType.Goal, configuration, Array.Empty<double>(), Array.Empty<double>());
+            EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
+            Array.Empty<double>(), Array.Empty<double>());

        EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
            out string failureReason);
@@ -173,7 +177,8 @@ internal static class LongitudinalModelChecks
            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>());
+            EmTerminalType.RollingSafetyStop, EmLongitudinalMode.RollingContinuation, configuration,
+            Array.Empty<double>(), Array.Empty<double>());

        EmPlanningStatus status = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
            out string failureReason);
@@ -183,24 +188,46 @@ internal static class LongitudinalModelChecks
        Verification.True(failureReason.Length != 0, "stopping-distance failure explains the rejection");
    }

-    private static void VerifiesReferenceHorizonSelectionKeepsTheCurrentSegmentBoundary()
+    private static void VerifiesReferenceHorizonSelectionSeparatesSpaceAndTime()
    {
        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);
+        configuration.Scheduling.DistanceHorizonMeters = 1d;
+        configuration.Scheduling.TimeHorizonSeconds = 2d;

-        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.Goal);
+        EmPlanningStatus status = new PlanningHorizonSelector().Select(
+            longSegment, 0d, 0d, 0d, configuration,
+            out PlanningHorizonSelection rolling, out string failure);
+        Verification.Equal(EmPlanningStatus.Success, status, "rolling selection: " + failure);
+        Verification.NearlyEqual(1d, rolling.WindowEndReferenceS,
+            "distance horizon defines the LS window");
+        Verification.Equal(EmLongitudinalMode.RollingContinuation,
+            rolling.LongitudinalMode, "far boundary rolls");

-        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");
+        DirectionSegmentView visibleButFar = CreateSegment(0.35d, EmBoundaryType.Goal);
+        status = new PlanningHorizonSelector().Select(
+            visibleButFar, 0d, 0d, 0d, configuration,
+            out PlanningHorizonSelection approach, out failure);
+        Verification.Equal(EmPlanningStatus.Success, status, "approach selection: " + failure);
+        Verification.Equal(EmLongitudinalMode.ApproachStopBoundary,
+            approach.LongitudinalMode, "visible unreachable boundary approaches");
+
+        DirectionSegmentView reachableGoal = CreateSegment(0.10d, EmBoundaryType.Goal);
+        status = new PlanningHorizonSelector().Select(
+            reachableGoal, 0d, 0d, 0d, configuration,
+            out PlanningHorizonSelection exact, out failure);
+        Verification.Equal(EmPlanningStatus.Success, status, "exact selection: " + failure);
+        Verification.Equal(EmLongitudinalMode.ExactStopAtBoundary,
+            exact.LongitudinalMode, "reachable goal stops exactly");
+        Verification.Equal(EmTerminalType.Goal, exact.TerminalType,
+            "exact stop preserves Goal identity");
+
+        IReadOnlyList<double> regularTimes = LongitudinalCandidate.CreateKnotTimes(2d, 0.1d);
+        Verification.Equal(19, LongitudinalTerminalSchedule.GetStabilizationStartIndex(
+            regularTimes, 0.1d), "regular exact stop reserves t=1.9..2.0");
+        Verification.Equal(1, LongitudinalTerminalSchedule.GetStabilizationStartIndex(
+            new[] { 0d, 0.1d, 0.2d, 0.25d }, 0.1d),
+            "short final interval moves the stop anchor earlier");
    }

    private static void VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints()
@@ -240,7 +267,8 @@ internal static class LongitudinalModelChecks
            new PathFixture(2d, 2d, 0d, 0d),
        });
        var input = new LongitudinalPlanningInput(path, TravelDirection.Forward, 0.10d, 0.02d,
-            EmTerminalType.Goal, configuration, new[] { 0d, 0.10d, 0.20d, 0.30d, 0.40d },
+            EmTerminalType.Goal, EmLongitudinalMode.ExactStopAtBoundary, configuration,
+            new[] { 0d, 0.10d, 0.20d, 0.30d, 0.40d },
            new[] { 0.20d, 0.20d, 0.20d, 0.20d, 0.20d });
        EmPlanningStatus speedStatus = new PathSpeedLimitBuilder().Build(input, out PathSpeedLimit envelope,
            out string speedFailure);