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,33 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>World pose projected onto one bounded direction segment.</summary>
+public sealed class FrenetProjection
+{
+    public FrenetProjection(FrenetReferencePoint referencePoint, double lateralOffset, double headingError,
+        double squaredDistanceMeters)
+    {
+        if (referencePoint == null)
+            throw new ArgumentNullException(nameof(referencePoint));
+        if (!IsFinite(lateralOffset) || !IsFinite(headingError) || !IsFinite(squaredDistanceMeters) || squaredDistanceMeters < 0d)
+            throw new ArgumentOutOfRangeException(nameof(lateralOffset));
+
+        ReferencePoint = referencePoint;
+        ReferenceS = referencePoint.ReferenceS;
+        LateralOffset = lateralOffset;
+        HeadingError = headingError;
+        SquaredDistanceMeters = squaredDistanceMeters;
+    }
+
+    public FrenetReferencePoint ReferencePoint { get; }
+    public double ReferenceS { get; }
+    public double LateralOffset { get; }
+    public double HeadingError { get; }
+    public double SquaredDistanceMeters { get; }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}