ParkingRobot/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/RawPathBaselineBuilder.cs

using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;

/// <summary>将已验证的粗路径转换为不改变任何锚点位姿或车辆曲率的安全比较基线。</summary>
internal static class RawPathBaselineBuilder
{
    private const double MinimumArcDeltaMeters = 1e-12d;

    internal static bool TryCreate(
        PathSmoothingRequest request,
        PreparedPath preparedPath,
        SmoothedPathValidator validator,
        double maximumCollisionCheckStepMeters,
        out RawPathBaseline baseline,
        out string reason)
    {
        baseline = null;
        reason = string.Empty;
        if (request == null || preparedPath == null || validator == null)
        {
            reason = "原始粗路径基线缺少请求、预处理路径或安全验证器。";
            return false;
        }

        IReadOnlyList<SmoothedPathPoint> candidatePath = CreatePoints(request.CoarsePath);
        IReadOnlyList<SmoothedPathSegment> candidateSegments = CreateSegments(request.Segments);
        if (!validator.TryValidate(
                candidatePath,
                candidateSegments,
                preparedPath,
                request.Map,
                request.Vehicle,
                maximumCollisionCheckStepMeters,
                out IReadOnlyList<SmoothedPathPoint> safePath,
                out double minimumClearanceMeters,
                out reason))
        {
            return false;
        }

        baseline = new RawPathBaseline(safePath, candidateSegments, CreateMetrics(safePath, candidateSegments, minimumClearanceMeters));
        return true;
    }

    private static IReadOnlyList<SmoothedPathPoint> CreatePoints(IReadOnlyList<CoarsePathPoint> coarsePath)
    {
        var points = new List<SmoothedPathPoint>(coarsePath == null ? 0 : coarsePath.Count);
        if (coarsePath != null)
        {
            for (int index = 0; index < coarsePath.Count; index++)
            {
                CoarsePathPoint point = coarsePath[index];
                points.Add(new SmoothedPathPoint(
                    point.X,
                    point.Y,
                    point.Heading,
                    point.UnwrappedHeading,
                    point.ArcLength,
                    point.Direction,
                    point.VehicleCurvature,
                    point.VehicleCurvature,
                    point.BodyClearance,
                    point.IsGearSwitchPoint,
                    point.IsGearSwitchPoint ? SmoothedPathPointSource.GearSwitch : SmoothedPathPointSource.Anchor));
            }
        }
        return new ReadOnlyCollection<SmoothedPathPoint>(points);
    }

    private static IReadOnlyList<SmoothedPathSegment> CreateSegments(IReadOnlyList<PathSegment> coarseSegments)
    {
        var segments = new List<SmoothedPathSegment>(coarseSegments == null ? 0 : coarseSegments.Count);
        if (coarseSegments != null)
        {
            for (int index = 0; index < coarseSegments.Count; index++)
            {
                PathSegment segment = coarseSegments[index];
                segments.Add(new SmoothedPathSegment(
                    segment.SegmentIndex,
                    segment.Direction,
                    segment.StartIndex,
                    segment.EndIndex,
                    segment.StartsAtGearSwitch,
                    segment.EndsAtGearSwitch));
            }
        }
        return new ReadOnlyCollection<SmoothedPathSegment>(segments);
    }

    private static PathQualityMetrics CreateMetrics(
        IReadOnlyList<SmoothedPathPoint> path,
        IReadOnlyList<SmoothedPathSegment> segments,
        double minimumClearanceMeters)
    {
        double maximumAbsoluteVehicleCurvature = 0d;
        double curvatureSquareSum = 0d;
        int curvatureSampleCount = 0;
        double totalAbsoluteCurvatureVariation = 0d;
        double curvatureVariationEnergy = 0d;

        for (int segmentIndex = 0; segmentIndex < segments.Count; segmentIndex++)
        {
            SmoothedPathSegment segment = segments[segmentIndex];
            SmoothedPathPoint previous = null;
            for (int pointIndex = segment.StartIndex; pointIndex <= segment.EndIndex; pointIndex++)
            {
                SmoothedPathPoint current = path[pointIndex];
                double curvature = current.VehicleCurvature;
                maximumAbsoluteVehicleCurvature = Math.Max(maximumAbsoluteVehicleCurvature, Math.Abs(curvature));
                curvatureSquareSum += curvature * curvature;
                curvatureSampleCount++;
                if (previous != null)
                {
                    double arcDelta = current.ArcLength - previous.ArcLength;
                    if (arcDelta > MinimumArcDeltaMeters)
                    {
                        double curvatureDelta = curvature - previous.VehicleCurvature;
                        totalAbsoluteCurvatureVariation += Math.Abs(curvatureDelta);
                        curvatureVariationEnergy += curvatureDelta * curvatureDelta / arcDelta;
                    }
                }
                previous = current;
            }
        }

        double rootMeanSquareVehicleCurvature = curvatureSampleCount == 0
            ? 0d
            : Math.Sqrt(curvatureSquareSum / curvatureSampleCount);
        double pathLengthMeters = path.Count == 0 ? 0d : path[path.Count - 1].ArcLength;
        return new PathQualityMetrics(
            true,
            pathLengthMeters,
            maximumAbsoluteVehicleCurvature,
            rootMeanSquareVehicleCurvature,
            totalAbsoluteCurvatureVariation,
            curvatureVariationEnergy,
            minimumClearanceMeters,
            0d,
            0d,
            0d,
            0d);
    }
}

/// <summary>已通过完整车体复核的原始粗路径及其比较指标。</summary>
internal sealed class RawPathBaseline
{
    internal RawPathBaseline(
        IReadOnlyList<SmoothedPathPoint> path,
        IReadOnlyList<SmoothedPathSegment> segments,
        PathQualityMetrics metrics)
    {
        Path = path;
        Segments = segments;
        Metrics = metrics;
    }

    internal IReadOnlyList<SmoothedPathPoint> Path { get; }
    internal IReadOnlyList<SmoothedPathSegment> Segments { get; }
    internal PathQualityMetrics Metrics { get; }
}