chore: save current workspace progress

2026-08-09 22:13:18 +08:00
parent 650c2ab0e3
commit 2f4fd15e52
449 changed files with 76593 additions and 971 deletions
@@ -0,0 +1,101 @@
+using System;
+using System.Collections.Generic;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Shared affine vehicle-curvature model used by the LS objective and hard QP constraints.</summary>
+internal sealed class LateralCurvatureLinearization
+{
+    private LateralCurvatureLinearization(int[] variableIndices, double[] gradient, double constant)
+    {
+        VariableIndices = variableIndices;
+        Gradient = gradient;
+        Constant = constant;
+    }
+
+    internal IReadOnlyList<int> VariableIndices { get; }
+
+    internal IReadOnlyList<double> Gradient { get; }
+
+    internal double Constant { get; }
+
+    internal static IReadOnlyList<LateralCurvatureLinearization> Create(LateralPlanningInput input,
+        LateralVariableLayout layout, LateralCandidate linearization)
+    {
+        if (input == null) throw new ArgumentNullException(nameof(input));
+        if (layout == null) throw new ArgumentNullException(nameof(layout));
+        if (linearization == null) throw new ArgumentNullException(nameof(linearization));
+        if (layout.StationCount != input.ReferenceStations.Count ||
+            linearization.ReferenceStations.Count != layout.StationCount)
+        {
+            throw new ArgumentException("Curvature linearization stations must match the lateral layout.", nameof(linearization));
+        }
+
+        double directionSign = input.ReferenceSegment.Direction == TravelDirection.Forward ? 1d : -1d;
+        var affines = new List<LateralCurvatureLinearization>(layout.StationCount);
+        for (int station = 0; station < layout.StationCount; station++)
+        {
+            FrenetReferencePoint reference = ReferencePathInterpolator.Interpolate(input.ReferenceSegment,
+                input.ReferenceStations[station]);
+            double l = linearization.L[station];
+            double dl = linearization.DL[station];
+            double ddl = linearization.DDL[station];
+            double referenceCurvature = reference.GeometricCurvature;
+            double referenceCurvatureDerivative = directionSign * reference.VehicleCurvatureDerivative;
+            double a = 1d - referenceCurvature * l;
+            double denominatorSquared = a * a + dl * dl;
+            if (!IsFinite(denominatorSquared) || denominatorSquared <= 0d)
+                throw new ArgumentException("Curvature linearization denominator is invalid.", nameof(linearization));
+
+            double denominatorPow3Over2 = denominatorSquared * Math.Sqrt(denominatorSquared);
+            double denominatorPow5Over2 = denominatorPow3Over2 * denominatorSquared;
+            double numerator = a * a * referenceCurvature + a * ddl +
+                referenceCurvatureDerivative * l * dl + 2d * referenceCurvature * dl * dl;
+            double geometricCurvature = numerator / denominatorPow3Over2;
+            double dNumeratorDLateral = -2d * a * referenceCurvature * referenceCurvature -
+                referenceCurvature * ddl + referenceCurvatureDerivative * dl;
+            double dNumeratorDSlope = referenceCurvatureDerivative * l + 4d * referenceCurvature * dl;
+            double dDenominatorSquaredDLateral = -2d * a * referenceCurvature;
+            double dDenominatorSquaredDSlope = 2d * dl;
+            double dGeometricDLateral = dNumeratorDLateral / denominatorPow3Over2 -
+                1.5d * numerator * dDenominatorSquaredDLateral / denominatorPow5Over2;
+            double dGeometricDSlope = dNumeratorDSlope / denominatorPow3Over2 -
+                1.5d * numerator * dDenominatorSquaredDSlope / denominatorPow5Over2;
+            double dGeometricDSecondDerivative = a / denominatorPow3Over2;
+            double vehicleCurvature = directionSign * geometricCurvature;
+            double[] gradient =
+            {
+                directionSign * dGeometricDLateral,
+                directionSign * dGeometricDSlope,
+                directionSign * dGeometricDSecondDerivative,
+            };
+            double constant = vehicleCurvature - gradient[0] * l - gradient[1] * dl - gradient[2] * ddl;
+            if (!IsFinite(vehicleCurvature) || !IsFinite(constant) || !IsFinite(gradient[0]) ||
+                !IsFinite(gradient[1]) || !IsFinite(gradient[2]))
+            {
+                throw new ArgumentException("Curvature linearization is non-finite.", nameof(linearization));
+            }
+            affines.Add(new LateralCurvatureLinearization(
+                new[] { layout.L(station), layout.DL(station), layout.DDL(station) }, gradient, constant));
+        }
+        return affines;
+    }
+
+    internal static double GetMaximumVehicleCurvature(VehicleParameters vehicle)
+    {
+        if (vehicle == null) throw new ArgumentNullException(nameof(vehicle));
+        double maximum = vehicle.MaximumCurvaturePerMeter ??
+            (vehicle.MinimumTurningRadiusMeters.HasValue && vehicle.MinimumTurningRadiusMeters.Value > 0d
+                ? 1d / vehicle.MinimumTurningRadiusMeters.Value
+                : double.NaN);
+        if (!IsFinite(maximum) || maximum <= 0d)
+            throw new ArgumentException("Vehicle maximum curvature is required.", nameof(vehicle));
+        return maximum;
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}