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,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.");
+    }
+}