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

using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
using MultiWheelC.TrajectoryPlanning.Utils;

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;

/// <summary>按单一方向段的弧长线性插值并保留精确锚点的确定性重采样器。</summary>
public sealed class ArcLengthResampler
{
    private const double Tolerance = 1e-10d;

    /// <summary>以目标间距重采样一个方向段；段末锚点始终原样保留。</summary>
    public bool TryResample(
        IReadOnlyList<SmoothingPoint2D> points,
        double spacingMeters,
        out IReadOnlyList<SmoothingPoint2D> resampled,
        out string reason)
    {
        resampled = EmptyPoints();
        reason = string.Empty;
        if (points == null || points.Count == 0 || !NumericGuard.IsPositiveFinite(spacingMeters))
        {
            reason = "重采样点集或采样间距无效。";
            return false;
        }

        for (int index = 0; index < points.Count; index++)
        {
            if (!IsValidPoint(points[index]))
            {
                reason = "重采样输入包含非法数值。";
                return false;
            }
            if (index == 0) continue;

            SmoothingPoint2D previous = points[index - 1];
            SmoothingPoint2D current = points[index];
            if (current.ArcLength <= previous.ArcLength + Tolerance)
            {
                reason = "同一方向段的弧长必须严格增加。";
                return false;
            }
            double distance = Distance(previous, current);
            if (!NumericGuard.IsFinite(distance) || distance <= Tolerance)
            {
                reason = "同一方向段中不允许重复位姿或数值溢出的距离。";
                return false;
            }
        }

        if (points.Count == 1)
        {
            resampled = CopyReadOnly(points);
            return true;
        }

        var output = new List<SmoothingPoint2D> { points[0] };
        double firstArc = points[0].ArcLength;
        double finalArc = points[points.Count - 1].ArcLength;
        int rightIndex = 1;
        for (double targetArc = firstArc + spacingMeters;
             targetArc < finalArc - Tolerance;
             targetArc += spacingMeters)
        {
            while (rightIndex < points.Count - 1 && points[rightIndex].ArcLength < targetArc)
                rightIndex++;

            SmoothingPoint2D left = points[rightIndex - 1];
            SmoothingPoint2D right = points[rightIndex];
            double ratio = (targetArc - left.ArcLength) / (right.ArcLength - left.ArcLength);
            double unwrappedHeading = left.UnwrappedHeading +
                ratio * (right.UnwrappedHeading - left.UnwrappedHeading);
            output.Add(new SmoothingPoint2D(
                left.X + ratio * (right.X - left.X),
                left.Y + ratio * (right.Y - left.Y),
                targetArc,
                AngleMath.NormalizeRadians(unwrappedHeading),
                unwrappedHeading,
                Math.Min(left.BodyClearance, right.BodyClearance),
                false,
                SmoothedPathPointSource.Interpolated));
        }

        // Appending the original object, rather than interpolating at the final arc, preserves the exact endpoint.
        output.Add(points[points.Count - 1]);
        resampled = new ReadOnlyCollection<SmoothingPoint2D>(output);
        return true;
    }

    /// <summary>重采样一个完整方向段并保留其换向拓扑标记。</summary>
    public bool TryResample(
        PreparedDirectionSegment segment,
        double spacingMeters,
        out PreparedDirectionSegment resampled,
        out string reason)
    {
        resampled = null;
        reason = string.Empty;
        if (segment == null)
        {
            reason = "待重采样方向段为空。";
            return false;
        }
        if (!TryResample(segment.Points, spacingMeters, out IReadOnlyList<SmoothingPoint2D> points, out reason))
            return false;

        resampled = new PreparedDirectionSegment(
            segment.SegmentIndex,
            segment.Direction,
            points,
            segment.StartsAtGearSwitch,
            segment.EndsAtGearSwitch,
            segment.StartVehicleCurvaturePerMeter);
        return true;
    }

    private static bool IsValidPoint(SmoothingPoint2D point)
    {
        return point != null && NumericGuard.IsFinite(point.X) && NumericGuard.IsFinite(point.Y) &&
            NumericGuard.IsFinite(point.ArcLength) && NumericGuard.IsFinite(point.Heading) &&
            NumericGuard.IsFinite(point.UnwrappedHeading) && NumericGuard.IsFinite(point.BodyClearance) &&
            point.BodyClearance >= 0d;
    }

    private static double Distance(SmoothingPoint2D left, SmoothingPoint2D right)
    {
        double x = right.X - left.X;
        double y = right.Y - left.Y;
        return Math.Sqrt(x * x + y * y);
    }

    private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)
    {
        var copy = new List<T>(source.Count);
        for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
        return new ReadOnlyCollection<T>(copy);
    }

    private static IReadOnlyList<SmoothingPoint2D> EmptyPoints()
    {
        return new ReadOnlyCollection<SmoothingPoint2D>(new List<SmoothingPoint2D>());
    }
}