ParkingRobot/ClumsyPilot/Trajectory/TrajectoryProjection.cs

using System;
using MyParking.Shared;

namespace MultiWheelC.Trajectory
{
    /// <summary>
    /// 保存车体中心投影到二维参考轨迹后得到的只读结果。
    /// </summary>
    public readonly struct TrajectoryProjection
    {
        /// <summary>
        /// 创建包含轨迹进度、参考状态和跟踪误差的投影结果。
        /// </summary>
        public TrajectoryProjection(
            int segmentStartIndex,
            TrajectoryPoint referencePoint,
            double lateralErrorMeters,
            double headingErrorRadians,
            double distanceToTrajectoryMeters,
            double remainingDistanceMeters)
        {
            if (segmentStartIndex < 0)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(segmentStartIndex),
                    "投影线段起点索引不能为负数。");
            }

            EnsureFinite(
                lateralErrorMeters,
                nameof(lateralErrorMeters));
            EnsureFinite(
                headingErrorRadians,
                nameof(headingErrorRadians));
            EnsureFiniteNonNegative(
                distanceToTrajectoryMeters,
                nameof(distanceToTrajectoryMeters));
            EnsureFiniteNonNegative(
                remainingDistanceMeters,
                nameof(remainingDistanceMeters));

            SegmentStartIndex = segmentStartIndex;
            ReferencePoint = referencePoint;
            LateralErrorMeters = lateralErrorMeters;
            HeadingErrorRadians =
                AngleMath.NormalizeRadians(
                    headingErrorRadians);
            DistanceToTrajectoryMeters =
                distanceToTrajectoryMeters;
            RemainingDistanceMeters =
                remainingDistanceMeters;
        }

        /// <summary>
        /// 获取投影所在轨迹线段的起点索引，线段终点索引为该值加1。
        /// </summary>
        public int SegmentStartIndex { get; }

        /// <summary>
        /// 获取投影位置插值得到的车体中心参考轨迹点。
        /// </summary>
        public TrajectoryPoint ReferencePoint { get; }

        /// <summary>
        /// 获取有符号横向误差，单位为m，参考轨迹位于车辆左侧时为正。
        /// </summary>
        public double LateralErrorMeters { get; }

        /// <summary>
        /// 获取参考航向减实际车体航向的最短角差，单位为rad，逆时针为正。
        /// </summary>
        public double HeadingErrorRadians { get; }

        /// <summary>
        /// 获取车体中心到投影点的欧氏距离，单位为m。
        /// </summary>
        public double DistanceToTrajectoryMeters { get; }

        /// <summary>
        /// 获取投影位置沿轨迹到终点的剩余弧长，单位为m。
        /// </summary>
        public double RemainingDistanceMeters { get; }

        /// <summary>
        /// 获取投影位置从轨迹起点累计的弧长，单位为m。
        /// </summary>
        public double ArcLengthMeters =>
            ReferencePoint.ArcLengthMeters;

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

        /// <summary>
        /// 检查数值是否为非负有限值。
        /// </summary>
        private static void EnsureFiniteNonNegative(
            double value,
            string parameterName)
        {
            EnsureFinite(value, parameterName);

            if (value < 0.0)
            {
                throw new ArgumentOutOfRangeException(
                    parameterName,
                    "轨迹投影距离不能为负数。");
            }
        }
    }
}