ParkingRobot/ClumsyPilot/tests/EMPlannerVerificationHost/FrenetChecks.cs

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