ParkingRobot/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Frenet/FrenetProjector.cs

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;
        }
    }
}