feat: add reverse-safe Frenet transforms

2026-08-03 22:43:39 +08:00
parent aac48835f1
commit 06687e294c
7 changed files with 475 additions and 3 deletions
@@ -0,0 +1,58 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Immutable interpolated reference sample in a single direction segment.</summary>
+public sealed class FrenetReferencePoint
+{
+    public FrenetReferencePoint(double referenceS, double x, double y, double vehicleYaw, double unwrappedVehicleYaw,
+        TravelDirection direction, double geometricCurvature, double vehicleCurvature,
+        double vehicleCurvatureDerivative, double bodyClearance)
+    {
+        RequireFinite(referenceS, nameof(referenceS));
+        RequireFinite(x, nameof(x));
+        RequireFinite(y, nameof(y));
+        RequireFinite(vehicleYaw, nameof(vehicleYaw));
+        RequireFinite(unwrappedVehicleYaw, nameof(unwrappedVehicleYaw));
+        RequireFinite(geometricCurvature, nameof(geometricCurvature));
+        RequireFinite(vehicleCurvature, nameof(vehicleCurvature));
+        RequireFinite(vehicleCurvatureDerivative, nameof(vehicleCurvatureDerivative));
+        RequireFinite(bodyClearance, nameof(bodyClearance));
+
+        ReferenceS = referenceS;
+        X = x;
+        Y = y;
+        VehicleYaw = AngleMath.NormalizeRadians(vehicleYaw);
+        UnwrappedVehicleYaw = unwrappedVehicleYaw;
+        Direction = direction;
+        GeometricCurvature = geometricCurvature;
+        VehicleCurvature = vehicleCurvature;
+        VehicleCurvatureDerivative = vehicleCurvatureDerivative;
+        BodyClearance = bodyClearance;
+    }
+
+    public double ReferenceS { get; }
+    public double X { get; }
+    public double Y { get; }
+    public double VehicleYaw { get; }
+    public double UnwrappedVehicleYaw { get; }
+    public TravelDirection Direction { get; }
+    public double GeometricCurvature { get; }
+    public double VehicleCurvature { get; }
+    public double VehicleCurvatureDerivative { get; }
+    public double BodyClearance { get; }
+
+    /// <summary>Unwrapped direction of travel, used internally for geometry.</summary>
+    public double TravelYaw
+    {
+        get { return Direction == TravelDirection.Forward ? UnwrappedVehicleYaw : UnwrappedVehicleYaw + Math.PI; }
+    }
+
+    private static void RequireFinite(double value, string name)
+    {
+        if (double.IsNaN(value) || double.IsInfinity(value))
+            throw new ArgumentOutOfRangeException(name);
+    }
+}