ParkingRobot/MultiWheelC/Trajectory/Trajectory2D.cs

using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using System.Linq;

namespace MultiWheelC.Trajectory
{
    /// <summary>
    /// 保存一条经过基本合法性检查的只读二维参考轨迹。
    /// </summary>
    public sealed class Trajectory2D
    {
        private const double StartArcLengthToleranceMeters = 1e-9;
        private const double MinimumSegmentLengthMeters = 1e-6;

        private readonly TrajectoryPoint[] _points;
        private readonly ReadOnlyCollection<TrajectoryPoint> _readOnlyPoints;

        /// <summary>
        /// 复制并验证按累计弧长升序排列的参考轨迹点。
        /// </summary>
        public Trajectory2D(
            IEnumerable<TrajectoryPoint> points)
        {
            if (points == null)
            {
                throw new ArgumentNullException(
                    nameof(points));
            }

            _points = points.ToArray();

            if (_points.Length < 2)
            {
                throw new ArgumentException(
                    "二维轨迹至少需要两个轨迹点。",
                    nameof(points));
            }

            if (Math.Abs(_points[0].ArcLengthMeters) >
                StartArcLengthToleranceMeters)
            {
                throw new ArgumentException(
                    "二维轨迹起点的累计弧长必须为0m。",
                    nameof(points));
            }

            for (var index = 1;
                 index < _points.Length;
                 index++)
            {
                ValidateSegment(
                    _points[index - 1],
                    _points[index],
                    index,
                    nameof(points));
            }

            _readOnlyPoints =
                Array.AsReadOnly(_points);
        }

        /// <summary>
        /// 获取轨迹点数量。
        /// </summary>
        public int Count => _points.Length;

        /// <summary>
        /// 获取指定索引处的轨迹点。
        /// </summary>
        public TrajectoryPoint this[int index] =>
            _points[index];

        /// <summary>
        /// 获取不可修改的有序轨迹点集合。
        /// </summary>
        public IReadOnlyList<TrajectoryPoint> Points =>
            _readOnlyPoints;

        /// <summary>
        /// 获取轨迹起点。
        /// </summary>
        public TrajectoryPoint StartPoint =>
            _points[0];

        /// <summary>
        /// 获取轨迹终点。
        /// </summary>
        public TrajectoryPoint EndPoint =>
            _points[_points.Length - 1];

        /// <summary>
        /// 获取轨迹总弧长，单位为m。
        /// </summary>
        public double TotalLengthMeters =>
            EndPoint.ArcLengthMeters;

        /// <summary>
        /// 根据当前累计弧长计算到轨迹终点的剩余距离。
        /// </summary>
        public double GetRemainingDistanceMeters(
            double arcLengthMeters)
        {
            EnsureFinite(
                arcLengthMeters,
                nameof(arcLengthMeters));

            if (arcLengthMeters <= 0.0)
                return TotalLengthMeters;

            if (arcLengthMeters >= TotalLengthMeters)
                return 0.0;

            return TotalLengthMeters - arcLengthMeters;
        }

        /// <summary>
        /// 检查相邻轨迹点是否构成有效的非零长度有序线段。
        /// </summary>
        private static void ValidateSegment(
            TrajectoryPoint previous,
            TrajectoryPoint current,
            int currentIndex,
            string parameterName)
        {
            if (current.ArcLengthMeters <=
                previous.ArcLengthMeters)
            {
                throw new ArgumentException(
                    $"轨迹点{currentIndex}的累计弧长必须严格大于前一个点。",
                    parameterName);
            }

            var deltaX =
                current.PoseInWorld.XMeters -
                previous.PoseInWorld.XMeters;
            var deltaY =
                current.PoseInWorld.YMeters -
                previous.PoseInWorld.YMeters;
            var segmentLengthSquared =
                deltaX * deltaX +
                deltaY * deltaY;
            var minimumLengthSquared =
                MinimumSegmentLengthMeters *
                MinimumSegmentLengthMeters;

            if (segmentLengthSquared <
                minimumLengthSquared)
            {
                throw new ArgumentException(
                    $"轨迹点{currentIndex}与前一个点的位置过近，无法构成有效投影线段。",
                    parameterName);
            }
        }

        /// <summary>
        /// 检查数值是否为有限值。
        /// </summary>
        private static void EnsureFinite(
            double value,
            string parameterName)
        {
            if (double.IsNaN(value) ||
                double.IsInfinity(value))
            {
                throw new ArgumentOutOfRangeException(
                    parameterName,
                    "轨迹弧长必须是有限值。");
            }
        }
    }
}