ParkingRobot/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralGeometryEvaluator.cs

using System;
using System.Collections.Generic;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.Utils;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>Reconstructs world geometry and actual path arc length from a lateral candidate.</summary>
public sealed class LateralGeometryEvaluator
{
    public bool TryEvaluate(LateralPlanningInput input, LateralCandidate candidate, out LateralPath path,
        out string failureReason)
    {
        path = null;
        failureReason = string.Empty;
        if (!HasMatchingStations(input, candidate, out failureReason))
            return false;

        try
        {
            List<GeometrySample> samples = Reconstruct(input, candidate, out failureReason);
            if (samples == null)
                return false;
            CalculateActualPathSAndCurvatureDerivative(samples, out failureReason);
            if (failureReason.Length != 0)
                return false;

            var points = new List<LateralPathPoint>(samples.Count);
            for (int index = 0; index < samples.Count; index++)
            {
                GeometrySample sample = samples[index];
                double dddl = candidate.DDDL[Math.Min(index, candidate.DDDL.Count - 1)];
                points.Add(new LateralPathPoint(sample.ReferenceS, sample.PathS, sample.L, sample.DL, sample.DDL, dddl,
                    sample.X, sample.Y, sample.VehicleYaw, sample.GeometricCurvature, sample.VehicleCurvature,
                    sample.VehicleCurvatureDerivative));
            }
            path = new LateralPath(points, false);
            return true;
        }
        catch (ArgumentException exception)
        {
            failureReason = exception.Message;
            return false;
        }
    }

    private static List<GeometrySample> Reconstruct(LateralPlanningInput input, LateralCandidate candidate,
        out string failureReason)
    {
        failureReason = string.Empty;
        double minimumDenominator = input.Configuration.Frenet.MinimumFrenetDenominator;
        if (!IsFinite(minimumDenominator) || minimumDenominator <= 0d)
        {
            failureReason = "The minimum Frenet denominator is invalid.";
            return null;
        }

        double directionSign = input.ReferenceSegment.Direction == TravelDirection.Forward ? 1d : -1d;
        var samples = new List<GeometrySample>(candidate.ReferenceStations.Count);
        for (int index = 0; index < candidate.ReferenceStations.Count; index++)
        {
            FrenetReferencePoint reference = ReferencePathInterpolator.Interpolate(input.ReferenceSegment,
                candidate.ReferenceStations[index]);
            double l = candidate.L[index];
            double dl = candidate.DL[index];
            double ddl = candidate.DDL[index];
            double denominator = 1d - reference.GeometricCurvature * l;
            if (!IsFinite(denominator) || denominator < minimumDenominator)
            {
                failureReason = "Frenet denominator is below the hard minimum at station " + index + ".";
                return null;
            }

            double travelYaw = reference.TravelYaw + Math.Atan2(dl, denominator);
            double vehicleYaw = input.ReferenceSegment.Direction == TravelDirection.Forward
                ? AngleMath.NormalizeRadians(travelYaw)
                : AngleMath.NormalizeRadians(travelYaw + Math.PI);
            double x = reference.X - l * Math.Sin(reference.TravelYaw);
            double y = reference.Y + l * Math.Cos(reference.TravelYaw);
            double geometricCurvature = CalculateGeometricCurvature(reference, l, dl, ddl,
                directionSign * reference.VehicleCurvatureDerivative);
            double vehicleCurvature = directionSign * geometricCurvature;
            if (!IsFinite(travelYaw) || !IsFinite(vehicleYaw) || !IsFinite(x) || !IsFinite(y) ||
                !IsFinite(geometricCurvature) || !IsFinite(vehicleCurvature))
            {
                failureReason = "Reconstructed lateral geometry is non-finite at station " + index + ".";
                return null;
            }
            samples.Add(new GeometrySample(candidate.ReferenceStations[index], l, dl, ddl, x, y, travelYaw, vehicleYaw,
                geometricCurvature, vehicleCurvature));
        }
        return samples;
    }

    private static void CalculateActualPathSAndCurvatureDerivative(IReadOnlyList<GeometrySample> samples,
        out string failureReason)
    {
        failureReason = string.Empty;
        samples[0].PathS = 0d;
        for (int index = 1; index < samples.Count; index++)
        {
            double dx = samples[index].X - samples[index - 1].X;
            double dy = samples[index].Y - samples[index - 1].Y;
            double chord = Math.Sqrt(dx * dx + dy * dy);
            if (!IsFinite(chord) || chord <= 0d)
            {
                failureReason = "Reconstructed path S is not strictly increasing at station " + index + ".";
                return;
            }
            samples[index].PathS = samples[index - 1].PathS + chord;
        }
        for (int index = 0; index < samples.Count; index++)
        {
            int lower = index == 0 ? 0 : index - 1;
            int upper = index == samples.Count - 1 ? samples.Count - 1 : index + 1;
            double span = samples[upper].PathS - samples[lower].PathS;
            if (!IsFinite(span) || span <= 0d)
            {
                failureReason = "Path-S curvature derivative span is invalid at station " + index + ".";
                return;
            }
            double derivative = (samples[upper].VehicleCurvature - samples[lower].VehicleCurvature) / span;
            if (!IsFinite(derivative))
            {
                failureReason = "Vehicle curvature derivative is non-finite at station " + index + ".";
                return;
            }
            samples[index].VehicleCurvatureDerivative = derivative;
        }
    }

    private static bool HasMatchingStations(LateralPlanningInput input, LateralCandidate candidate, out string failureReason)
    {
        failureReason = string.Empty;
        if (input == null || candidate == null)
        {
            failureReason = "Lateral input and candidate are required.";
            return false;
        }
        if (candidate.ReferenceStations.Count != input.ReferenceStations.Count)
        {
            failureReason = "Candidate station count does not match the lateral input.";
            return false;
        }
        for (int index = 0; index < input.ReferenceStations.Count; index++)
        {
            if (Math.Abs(candidate.ReferenceStations[index] - input.ReferenceStations[index]) > 1e-12d)
            {
                failureReason = "Candidate stations do not match the lateral input.";
                return false;
            }
        }
        return true;
    }

    internal static double CalculateGeometricCurvature(FrenetReferencePoint reference, double l, double dl, double ddl,
        double referenceCurvatureDerivative)
    {
        double a = 1d - reference.GeometricCurvature * l;
        double denominatorSquared = a * a + dl * dl;
        double numerator = a * a * reference.GeometricCurvature + a * ddl +
            referenceCurvatureDerivative * l * dl + 2d * reference.GeometricCurvature * dl * dl;
        return numerator / (denominatorSquared * Math.Sqrt(denominatorSquared));
    }

    private static bool IsFinite(double value)
    {
        return !double.IsNaN(value) && !double.IsInfinity(value);
    }

    private sealed class GeometrySample
    {
        public GeometrySample(double referenceS, double l, double dl, double ddl, double x, double y, double travelYaw,
            double vehicleYaw, double geometricCurvature, double vehicleCurvature)
        {
            ReferenceS = referenceS;
            L = l;
            DL = dl;
            DDL = ddl;
            X = x;
            Y = y;
            TravelYaw = travelYaw;
            VehicleYaw = vehicleYaw;
            GeometricCurvature = geometricCurvature;
            VehicleCurvature = vehicleCurvature;
        }

        public double ReferenceS { get; }
        public double L { get; }
        public double DL { get; }
        public double DDL { get; }
        public double X { get; }
        public double Y { get; }
        public double TravelYaw { get; }
        public double VehicleYaw { get; }
        public double GeometricCurvature { get; }
        public double VehicleCurvature { get; }
        public double PathS { get; set; }
        public double VehicleCurvatureDerivative { get; set; }
    }
}