feat: assemble longitudinal ST quadratic programs

2026-08-04 09:21:07 +08:00
parent 62ea9db8cd
commit c01d0d5b47
6 changed files with 642 additions and 0 deletions
@@ -0,0 +1,52 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Deterministic contiguous variable ranges for one time-domain longitudinal QP.</summary>
+public sealed class LongitudinalVariableLayout
+{
+    public LongitudinalVariableLayout(int knotCount)
+    {
+        if (knotCount < 2)
+            throw new ArgumentOutOfRangeException(nameof(knotCount), "At least two time knots are required.");
+
+        KnotCount = knotCount;
+        SStart = 0;
+        UStart = knotCount;
+        AStart = 2 * knotCount;
+        JStart = 3 * knotCount;
+        VariableCount = 4 * knotCount - 1;
+    }
+
+    public int KnotCount { get; }
+
+    public int SStart { get; }
+
+    public int UStart { get; }
+
+    public int AStart { get; }
+
+    public int JStart { get; }
+
+    public int VariableCount { get; }
+
+    public int S(int knotIndex) { return RequireKnotIndex(knotIndex, SStart); }
+
+    public int U(int knotIndex) { return RequireKnotIndex(knotIndex, UStart); }
+
+    public int A(int knotIndex) { return RequireKnotIndex(knotIndex, AStart); }
+
+    public int J(int intervalIndex)
+    {
+        if (intervalIndex < 0 || intervalIndex >= KnotCount - 1)
+            throw new ArgumentOutOfRangeException(nameof(intervalIndex));
+        return JStart + intervalIndex;
+    }
+
+    private int RequireKnotIndex(int knotIndex, int start)
+    {
+        if (knotIndex < 0 || knotIndex >= KnotCount)
+            throw new ArgumentOutOfRangeException(nameof(knotIndex));
+        return start + knotIndex;
+    }
+}