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,131 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Deterministically projects a world pose only inside the supplied direction segment and S window.</summary>
+public sealed class FrenetProjector
+{
+    private const double TieTolerance = 1e-14d;
+
+    public bool TryProject(Pose2D worldPose, DirectionSegmentView segment, double minimumReferenceS,
+        double maximumReferenceS, double maximumDistanceMeters, out FrenetProjection projection)
+    {
+        return TryProject(worldPose, segment, minimumReferenceS, maximumReferenceS, maximumDistanceMeters,
+            minimumReferenceS, out projection);
+    }
+
+    public bool TryProject(Pose2D worldPose, DirectionSegmentView segment, double minimumReferenceS,
+        double maximumReferenceS, double maximumDistanceMeters, double seedReferenceS, out FrenetProjection projection)
+    {
+        projection = null;
+        if (worldPose == null || segment == null || !IsFinite(worldPose.X) || !IsFinite(worldPose.Y) ||
+            !IsFinite(worldPose.Heading) || !IsFinite(minimumReferenceS) || !IsFinite(maximumReferenceS) ||
+            !IsFinite(maximumDistanceMeters) || !IsFinite(seedReferenceS) || maximumDistanceMeters < 0d)
+            return false;
+
+        double lowerBound = Math.Max(0d, minimumReferenceS);
+        double upperBound = Math.Min(segment.LengthMeters, maximumReferenceS);
+        if (lowerBound > upperBound)
+            return false;
+
+        Candidate best = null;
+        for (int index = 0; index + 1 < segment.Points.Count; index++)
+        {
+            double startS = Math.Max(lowerBound, segment.Points[index].ArcLength);
+            double endS = Math.Min(upperBound, segment.Points[index + 1].ArcLength);
+            if (startS > endS)
+                continue;
+
+            FrenetReferencePoint start = ReferencePathInterpolator.Interpolate(segment, startS);
+            FrenetReferencePoint end = ReferencePathInterpolator.Interpolate(segment, endS);
+            ConsiderLine(worldPose, start, end, seedReferenceS, ref best);
+        }
+
+        if (best == null && Math.Abs(lowerBound - upperBound) <= TieTolerance)
+        {
+            FrenetReferencePoint point = ReferencePathInterpolator.Interpolate(segment, lowerBound);
+            ConsiderPoint(worldPose, point, seedReferenceS, ref best);
+        }
+
+        if (best == null || best.SquaredDistanceMeters > maximumDistanceMeters * maximumDistanceMeters)
+            return false;
+
+        FrenetReferencePoint reference = ReferencePathInterpolator.Interpolate(segment, best.ReferenceS);
+        double dx = worldPose.X - reference.X;
+        double dy = worldPose.Y - reference.Y;
+        double travelYaw = reference.TravelYaw;
+        double lateralOffset = -dx * Math.Sin(travelYaw) + dy * Math.Cos(travelYaw);
+        double egoTravelYaw = FrenetTransform.GetTravelYaw(worldPose.Heading, segment.Direction);
+        double headingError = AngleMath.NormalizeRadians(egoTravelYaw - travelYaw);
+        projection = new FrenetProjection(reference, lateralOffset, headingError, best.SquaredDistanceMeters);
+        return true;
+    }
+
+    private static void ConsiderLine(Pose2D worldPose, FrenetReferencePoint start, FrenetReferencePoint end,
+        double seedReferenceS, ref Candidate best)
+    {
+        double dx = end.X - start.X;
+        double dy = end.Y - start.Y;
+        double lengthSquared = dx * dx + dy * dy;
+        if (lengthSquared <= TieTolerance)
+        {
+            ConsiderPoint(worldPose, start, seedReferenceS, ref best);
+            ConsiderPoint(worldPose, end, seedReferenceS, ref best);
+            return;
+        }
+
+        double fraction = ((worldPose.X - start.X) * dx + (worldPose.Y - start.Y) * dy) / lengthSquared;
+        fraction = Math.Max(0d, Math.Min(1d, fraction));
+        double referenceS = start.ReferenceS + (end.ReferenceS - start.ReferenceS) * fraction;
+        double x = start.X + dx * fraction;
+        double y = start.Y + dy * fraction;
+        Consider(worldPose, referenceS, x, y, seedReferenceS, ref best);
+    }
+
+    private static void ConsiderPoint(Pose2D worldPose, FrenetReferencePoint point, double seedReferenceS,
+        ref Candidate best)
+    {
+        Consider(worldPose, point.ReferenceS, point.X, point.Y, seedReferenceS, ref best);
+    }
+
+    private static void Consider(Pose2D worldPose, double referenceS, double x, double y, double seedReferenceS,
+        ref Candidate best)
+    {
+        double dx = worldPose.X - x;
+        double dy = worldPose.Y - y;
+        double squaredDistance = dx * dx + dy * dy;
+        var candidate = new Candidate(referenceS, squaredDistance, Math.Abs(referenceS - seedReferenceS));
+        if (best == null || candidate.IsPreferredTo(best))
+            best = candidate;
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+
+    private sealed class Candidate
+    {
+        public Candidate(double referenceS, double squaredDistanceMeters, double seedDistance)
+        {
+            ReferenceS = referenceS;
+            SquaredDistanceMeters = squaredDistanceMeters;
+            SeedDistance = seedDistance;
+        }
+
+        public double ReferenceS { get; }
+        public double SquaredDistanceMeters { get; }
+        public double SeedDistance { get; }
+
+        public bool IsPreferredTo(Candidate other)
+        {
+            if (SquaredDistanceMeters < other.SquaredDistanceMeters - TieTolerance) return true;
+            if (SquaredDistanceMeters > other.SquaredDistanceMeters + TieTolerance) return false;
+            if (SeedDistance < other.SeedDistance - TieTolerance) return true;
+            if (SeedDistance > other.SeedDistance + TieTolerance) return false;
+            return ReferenceS < other.ReferenceS;
+        }
+    }
+}