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,45 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Coordinate conversion that keeps Frenet lateral sign relative to travel direction.</summary>
+public static class FrenetTransform
+{
+    public static bool TryReconstruct(FrenetReferencePoint referencePoint, double lateralOffset, double lateralDerivative,
+        double minimumFrenetDenominator, out Pose2D pose)
+    {
+        pose = null;
+        if (referencePoint == null || !IsFinite(lateralOffset) || !IsFinite(lateralDerivative) ||
+            !IsFinite(minimumFrenetDenominator) || minimumFrenetDenominator <= 0d)
+            return false;
+
+        double denominator = 1d - referencePoint.GeometricCurvature * lateralOffset;
+        if (!IsFinite(denominator) || denominator < minimumFrenetDenominator)
+            return false;
+
+        double travelYaw = referencePoint.TravelYaw;
+        double x = referencePoint.X - lateralOffset * Math.Sin(travelYaw);
+        double y = referencePoint.Y + lateralOffset * Math.Cos(travelYaw);
+        double optimizedTravelYaw = travelYaw + Math.Atan2(lateralDerivative, denominator);
+        double vehicleYaw = referencePoint.Direction == TravelDirection.Forward
+            ? AngleMath.NormalizeRadians(optimizedTravelYaw)
+            : AngleMath.NormalizeRadians(optimizedTravelYaw + Math.PI);
+        if (!IsFinite(x) || !IsFinite(y) || !IsFinite(vehicleYaw))
+            return false;
+
+        pose = new Pose2D(x, y, vehicleYaw);
+        return true;
+    }
+
+    internal static double GetTravelYaw(double vehicleYaw, TravelDirection direction)
+    {
+        return direction == TravelDirection.Forward ? vehicleYaw : vehicleYaw + Math.PI;
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}