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,64 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Interpolates reference geometry inside exactly one direction segment.</summary>
+public static class ReferencePathInterpolator
+{
+    private const double Epsilon = 1e-12d;
+
+    public static FrenetReferencePoint Interpolate(DirectionSegmentView segment, double referenceS)
+    {
+        if (segment == null)
+            throw new ArgumentNullException(nameof(segment));
+        if (!IsFinite(referenceS) || referenceS < -Epsilon || referenceS > segment.LengthMeters + Epsilon)
+            throw new ArgumentOutOfRangeException(nameof(referenceS));
+
+        double clampedS = Math.Max(0d, Math.Min(segment.LengthMeters, referenceS));
+        for (int index = 0; index < segment.Points.Count; index++)
+        {
+            SmoothedPathPoint upper = segment.Points[index];
+            if (Math.Abs(upper.ArcLength - clampedS) <= Epsilon)
+                return FromPoint(upper);
+            if (upper.ArcLength > clampedS)
+            {
+                SmoothedPathPoint lower = segment.Points[index - 1];
+                double span = upper.ArcLength - lower.ArcLength;
+                if (span <= Epsilon)
+                    throw new ArgumentException("Reference points must have positive interpolation spans.", nameof(segment));
+                double fraction = (clampedS - lower.ArcLength) / span;
+                return new FrenetReferencePoint(
+                    clampedS,
+                    Lerp(lower.X, upper.X, fraction),
+                    Lerp(lower.Y, upper.Y, fraction),
+                    AngleMath.NormalizeRadians(Lerp(lower.UnwrappedHeading, upper.UnwrappedHeading, fraction)),
+                    Lerp(lower.UnwrappedHeading, upper.UnwrappedHeading, fraction),
+                    segment.Direction,
+                    Lerp(lower.GeometricCurvature, upper.GeometricCurvature, fraction),
+                    Lerp(lower.VehicleCurvature, upper.VehicleCurvature, fraction),
+                    Lerp(lower.VehicleCurvatureDerivative, upper.VehicleCurvatureDerivative, fraction),
+                    Lerp(lower.BodyClearance, upper.BodyClearance, fraction));
+            }
+        }
+        return FromPoint(segment.Points[segment.Points.Count - 1]);
+    }
+
+    private static FrenetReferencePoint FromPoint(SmoothedPathPoint point)
+    {
+        return new FrenetReferencePoint(point.ArcLength, point.X, point.Y, point.Heading, point.UnwrappedHeading,
+            point.Direction, point.GeometricCurvature, point.VehicleCurvature, point.VehicleCurvatureDerivative,
+            point.BodyClearance);
+    }
+
+    private static double Lerp(double lower, double upper, double fraction)
+    {
+        return lower + (upper - lower) * fraction;
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}