feat: add reverse-safe Frenet transforms

ClumsyPilot/tests/EMPlannerVerificationHost/FrenetChecks.cs

@@ -0,0 +1,136 @@
+using System;
+using System.Collections.Generic;
+using EMPlannerVerificationHost;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+internal static class FrenetChecks
+{
+    public static void Run()
+    {
+        VerifiesForwardProjectionAndReconstruction();
+        VerifiesReverseProjectionAcrossYawWrap();
+        VerifiesBoundedProjectionOnALoop();
+    }
+
+    private static void VerifiesForwardProjectionAndReconstruction()
+    {
+        DirectionSegmentView segment = CreateStraightSegment(TravelDirection.Forward, 0d, 0d);
+        var world = new Pose2D(1.5d, 0.2d, 0d);
+        var projector = new FrenetProjector();
+
+        Verification.True(projector.TryProject(world, segment, 0d, 4d, 0.5d, out FrenetProjection projection),
+            "forward projection succeeds");
+        Verification.NearlyEqual(1.5d, projection.ReferenceS, "forward reference s");
+        Verification.NearlyEqual(0.2d, projection.LateralOffset, "forward positive l is travel-left");
+        Verification.True(FrenetTransform.TryReconstruct(projection.ReferencePoint, projection.LateralOffset, 0d, 0.2d,
+            out Pose2D reconstructed), "forward reconstruction succeeds");
+        Verification.NearlyEqual(world.X, reconstructed.X, "forward reconstructed x");
+        Verification.NearlyEqual(world.Y, reconstructed.Y, "forward reconstructed y");
+        Verification.NearlyEqual(0d, reconstructed.Heading, "forward reconstructed vehicle yaw");
+    }
+
+    private static void VerifiesReverseProjectionAcrossYawWrap()
+    {
+        const double vehicleYaw = -Math.PI + 0.01d;
+        const double travelYaw = 0.01d;
+        DirectionSegmentView segment = CreateStraightSegment(TravelDirection.Reverse, vehicleYaw, travelYaw);
+        double expectedS = 1.5d;
+        double expectedL = 0.2d;
+        var world = new Pose2D(
+            expectedS * Math.Cos(travelYaw) - expectedL * Math.Sin(travelYaw),
+            expectedS * Math.Sin(travelYaw) + expectedL * Math.Cos(travelYaw),
+            vehicleYaw);
+        var projector = new FrenetProjector();
+
+        Verification.True(projector.TryProject(world, segment, 0d, 4d, 0.5d, out FrenetProjection projection),
+            "reverse projection succeeds");
+        Verification.NearlyEqual(expectedS, projection.ReferenceS, "reverse reference s");
+        Verification.NearlyEqual(expectedL, projection.LateralOffset, "reverse positive l is travel-left and body-right");
+        Verification.True(FrenetTransform.TryReconstruct(projection.ReferencePoint, projection.LateralOffset, 0d, 0.2d,
+            out Pose2D reconstructed), "reverse reconstruction succeeds");
+        Verification.NearlyEqual(world.X, reconstructed.X, "reverse reconstructed x");
+        Verification.NearlyEqual(world.Y, reconstructed.Y, "reverse reconstructed y");
+        Verification.NearlyEqual(AngleMath.NormalizeRadians(vehicleYaw), reconstructed.Heading,
+            "reverse reconstructed vehicle yaw");
+
+        DirectionSegmentView wrapped = CreateWrappedForwardSegment();
+        FrenetReferencePoint wrappedPoint = ReferencePathInterpolator.Interpolate(wrapped, 1.5d);
+        Verification.NearlyEqual(3.18d, wrappedPoint.UnwrappedVehicleYaw, "interpolated yaw remains unwrapped internally");
+        Verification.NearlyEqual(AngleMath.NormalizeRadians(3.18d), wrappedPoint.VehicleYaw,
+            "interpolated public yaw is normalized");
+        Verification.True(!FrenetTransform.TryReconstruct(new FrenetReferencePoint(0d, 0d, 0d, 0d, 0d,
+            TravelDirection.Forward, 1d, 0d, 0d, 0d), 1d, 0d, 0.2d, out _),
+            "singular Frenet reconstruction is rejected");
+    }
+
+    private static void VerifiesBoundedProjectionOnALoop()
+    {
+        DirectionSegmentView segment = CreateLoopSegment();
+        var projector = new FrenetProjector();
+        var world = new Pose2D(1d, 0.2d, Math.PI);
+
+        Verification.True(projector.TryProject(world, segment, 2.2d, 4.2d, 0.1d, out FrenetProjection projection),
+            "bounded loop projection succeeds");
+        Verification.NearlyEqual(3.2d, projection.ReferenceS, "bounded loop picks the local upper branch");
+        Verification.True(projection.ReferenceS >= 2.2d && projection.ReferenceS <= 4.2d,
+            "projection remains in supplied reference-s interval");
+        Verification.True(FrenetTransform.TryReconstruct(projection.ReferencePoint, projection.LateralOffset, 0d, 0.2d,
+            out Pose2D reconstructed), "loop reconstruction succeeds");
+        Verification.NearlyEqual(world.X, reconstructed.X, "loop reconstructed x");
+        Verification.NearlyEqual(world.Y, reconstructed.Y, "loop reconstructed y");
+    }
+
+    private static DirectionSegmentView CreateStraightSegment(TravelDirection direction, double vehicleYaw,
+        double travelYaw)
+    {
+        var points = new List<SmoothedPathPoint>
+        {
+            Point(0d, 0d, AngleMath.NormalizeRadians(vehicleYaw), vehicleYaw, 0d, direction),
+            Point(4d * Math.Cos(travelYaw), 4d * Math.Sin(travelYaw), AngleMath.NormalizeRadians(vehicleYaw),
+                vehicleYaw, 4d, direction),
+        };
+        return CreateSegment(direction, points);
+    }
+
+    private static DirectionSegmentView CreateWrappedForwardSegment()
+    {
+        var points = new List<SmoothedPathPoint>
+        {
+            Point(0d, 0d, AngleMath.NormalizeRadians(3.12d), 3.12d, 0d, TravelDirection.Forward),
+            Point(1d, 0d, AngleMath.NormalizeRadians(3.16d), 3.16d, 1d, TravelDirection.Forward),
+            Point(2d, 0d, AngleMath.NormalizeRadians(3.20d), 3.20d, 2d, TravelDirection.Forward),
+        };
+        return CreateSegment(TravelDirection.Forward, points);
+    }
+
+    private static DirectionSegmentView CreateLoopSegment()
+    {
+        var points = new List<SmoothedPathPoint>
+        {
+            Point(0d, 0d, 0d, 0d, 0d, TravelDirection.Forward),
+            Point(2d, 0d, 0d, 0d, 2d, TravelDirection.Forward),
+            Point(2d, 0.2d, Math.PI / 2d, Math.PI / 2d, 2.2d, TravelDirection.Forward),
+            Point(0d, 0.2d, Math.PI, Math.PI, 4.2d, TravelDirection.Forward),
+        };
+        return CreateSegment(TravelDirection.Forward, points);
+    }
+
+    private static DirectionSegmentView CreateSegment(TravelDirection direction, IReadOnlyList<SmoothedPathPoint> points)
+    {
+        double length = points[points.Count - 1].ArcLength;
+        return new DirectionSegmentView(0, direction, points,
+            new ReferenceBoundary(0, 0d, EmBoundaryType.None, 0d),
+            new ReferenceBoundary(0, length, EmBoundaryType.Goal, length), 0d);
+    }
+
+    private static SmoothedPathPoint Point(double x, double y, double heading, double unwrappedHeading, double s,
+        TravelDirection direction)
+    {
+        return new SmoothedPathPoint(x, y, heading, unwrappedHeading, s, direction, 0d, 0d, 0d, 1d,
+            false, SmoothedPathPointSource.Anchor);
+    }
+}

ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs

@@ -6,9 +6,9 @@ internal static class Program
 {
     private static int Main(string[] args)
     {
-        if (args.Length != 1 || (args[0] != "foundation" && args[0] != "segmentation"))
+        if (args.Length != 1 || (args[0] != "foundation" && args[0] != "segmentation" && args[0] != "frenet"))
         {
-            Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation");
+            Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation|frenet");
             return 2;
         }
 
@@ -19,11 +19,16 @@ internal static class Program
                 MultiWheelC.TrajectoryPlanning.EMPlanner.FoundationChecks.Run();
                 Console.WriteLine("PASS foundation");
             }
-            else
+            else if (args[0] == "segmentation")
             {
                 MultiWheelC.TrajectoryPlanning.EMPlanner.SegmentationChecks.Run();
                 Console.WriteLine("PASS segmentation");
             }
+            else
+            {
+                MultiWheelC.TrajectoryPlanning.EMPlanner.FrenetChecks.Run();
+                Console.WriteLine("PASS frenet");
+            }
             return 0;
         }
         catch (Exception exception)