feat: add lateral optimization model

2026-08-04 00:50:10 +08:00
parent 113d0c9d9b
commit c225b17d37
8 changed files with 625 additions and 2 deletions
@@ -0,0 +1,63 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Deterministic variable ranges for one lateral QP discretized on N stations.</summary>
+public sealed class LateralVariableLayout
+{
+    public LateralVariableLayout(int stationCount)
+    {
+        if (stationCount < 2)
+            throw new ArgumentOutOfRangeException(nameof(stationCount), "At least two reference-S stations are required.");
+
+        StationCount = stationCount;
+        LStart = 0;
+        DLStart = stationCount;
+        DDLStart = 2 * stationCount;
+        DDDLStart = 3 * stationCount;
+        VariableCount = 4 * stationCount - 1;
+    }
+
+    public int StationCount { get; }
+
+    public int LStart { get; }
+
+    public int DLStart { get; }
+
+    public int DDLStart { get; }
+
+    public int DDDLStart { get; }
+
+    public int VariableCount { get; }
+
+    public int L(int stationIndex)
+    {
+        RequireStationIndex(stationIndex);
+        return LStart + stationIndex;
+    }
+
+    public int DL(int stationIndex)
+    {
+        RequireStationIndex(stationIndex);
+        return DLStart + stationIndex;
+    }
+
+    public int DDL(int stationIndex)
+    {
+        RequireStationIndex(stationIndex);
+        return DDLStart + stationIndex;
+    }
+
+    public int DDDL(int intervalIndex)
+    {
+        if (intervalIndex < 0 || intervalIndex >= StationCount - 1)
+            throw new ArgumentOutOfRangeException(nameof(intervalIndex));
+        return DDDLStart + intervalIndex;
+    }
+
+    private void RequireStationIndex(int stationIndex)
+    {
+        if (stationIndex < 0 || stationIndex >= StationCount)
+            throw new ArgumentOutOfRangeException(nameof(stationIndex));
+    }
+}