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,61 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Immutable world-space sample reconstructed from one lateral candidate.</summary>
+public sealed class LateralPathPoint
+{
+    public LateralPathPoint(double referenceS, double pathS, double l, double dl, double ddl, double dddl,
+        double x, double y, double vehicleYaw, double geometricCurvature, double vehicleCurvature,
+        double vehicleCurvatureDerivative)
+    {
+        RequireFinite(referenceS, nameof(referenceS));
+        RequireFinite(pathS, nameof(pathS));
+        RequireFinite(l, nameof(l));
+        RequireFinite(dl, nameof(dl));
+        RequireFinite(ddl, nameof(ddl));
+        RequireFinite(dddl, nameof(dddl));
+        RequireFinite(x, nameof(x));
+        RequireFinite(y, nameof(y));
+        RequireFinite(vehicleYaw, nameof(vehicleYaw));
+        RequireFinite(geometricCurvature, nameof(geometricCurvature));
+        RequireFinite(vehicleCurvature, nameof(vehicleCurvature));
+        RequireFinite(vehicleCurvatureDerivative, nameof(vehicleCurvatureDerivative));
+        if (referenceS < 0d)
+            throw new ArgumentOutOfRangeException(nameof(referenceS));
+        if (pathS < 0d)
+            throw new ArgumentOutOfRangeException(nameof(pathS));
+
+        ReferenceS = referenceS;
+        PathS = pathS;
+        L = l;
+        DL = dl;
+        DDL = ddl;
+        DDDL = dddl;
+        X = x;
+        Y = y;
+        VehicleYaw = vehicleYaw;
+        GeometricCurvature = geometricCurvature;
+        VehicleCurvature = vehicleCurvature;
+        VehicleCurvatureDerivative = vehicleCurvatureDerivative;
+    }
+
+    public double ReferenceS { get; }
+    public double PathS { get; }
+    public double L { get; }
+    public double DL { get; }
+    public double DDL { get; }
+    public double DDDL { get; }
+    public double X { get; }
+    public double Y { get; }
+    public double VehicleYaw { get; }
+    public double GeometricCurvature { get; }
+    public double VehicleCurvature { get; }
+    public double VehicleCurvatureDerivative { get; }
+
+    private static void RequireFinite(double value, string parameterName)
+    {
+        if (double.IsNaN(value) || double.IsInfinity(value))
+            throw new ArgumentOutOfRangeException(parameterName);
+    }
+}