ParkingRobot/ClumsyPilot/Trajectory/TrajectoryProjector.cs

using System;
using MyParking.Shared;

namespace MultiWheelC.Trajectory
{
    /// <summary>
    /// 将Detour给出的实际车体中心位姿投影到二维离散参考轨迹。
    /// </summary>
    public static class TrajectoryProjector
    {
        /// <summary>
        /// 在整条轨迹上查找距离实际车体中心最近的线段投影结果。
        /// </summary>
        public static TrajectoryProjection Project(
            Trajectory2D trajectory,
            Pose2D vehiclePoseInWorld)
        {
            if (trajectory == null)
            {
                throw new ArgumentNullException(
                    nameof(trajectory));
            }

            EnsureFinitePose(
                vehiclePoseInWorld,
                nameof(vehiclePoseInWorld));

            var bestSegmentStartIndex = 0;
            var bestInterpolationRatio = 0.0;
            var bestProjectedX = 0.0;
            var bestProjectedY = 0.0;
            var bestDistanceSquared =
                double.PositiveInfinity;

            for (var segmentStartIndex = 0;
                 segmentStartIndex < trajectory.Count - 1;
                 segmentStartIndex++)
            {
                var segmentStart =
                    trajectory[segmentStartIndex];
                var segmentEnd =
                    trajectory[segmentStartIndex + 1];

                var segmentX =
                    segmentEnd.PoseInWorld.XMeters -
                    segmentStart.PoseInWorld.XMeters;
                var segmentY =
                    segmentEnd.PoseInWorld.YMeters -
                    segmentStart.PoseInWorld.YMeters;
                var segmentLengthSquared =
                    segmentX * segmentX +
                    segmentY * segmentY;

                var vehicleFromSegmentStartX =
                    vehiclePoseInWorld.XMeters -
                    segmentStart.PoseInWorld.XMeters;
                var vehicleFromSegmentStartY =
                    vehiclePoseInWorld.YMeters -
                    segmentStart.PoseInWorld.YMeters;

                var interpolationRatio =
                    InterpolationMath.Clamp01(
                        (vehicleFromSegmentStartX * segmentX +
                         vehicleFromSegmentStartY * segmentY) /
                        segmentLengthSquared);

                var projectedX =
                    InterpolationMath.Lerp(
                        segmentStart.PoseInWorld.XMeters,
                        segmentEnd.PoseInWorld.XMeters,
                        interpolationRatio);
                var projectedY =
                    InterpolationMath.Lerp(
                        segmentStart.PoseInWorld.YMeters,
                        segmentEnd.PoseInWorld.YMeters,
                        interpolationRatio);

                var projectionErrorX =
                    projectedX -
                    vehiclePoseInWorld.XMeters;
                var projectionErrorY =
                    projectedY -
                    vehiclePoseInWorld.YMeters;
                var distanceSquared =
                    projectionErrorX * projectionErrorX +
                    projectionErrorY * projectionErrorY;

                if (distanceSquared >= bestDistanceSquared)
                {
                    continue;
                }

                bestSegmentStartIndex =
                    segmentStartIndex;
                bestInterpolationRatio =
                    interpolationRatio;
                bestProjectedX = projectedX;
                bestProjectedY = projectedY;
                bestDistanceSquared = distanceSquared;
            }

            return BuildProjection(
                trajectory,
                vehiclePoseInWorld,
                bestSegmentStartIndex,
                bestInterpolationRatio,
                bestProjectedX,
                bestProjectedY,
                bestDistanceSquared);
        }

        /// <summary>
        /// 根据最近线段和插值比例生成控制器使用的完整投影结果。
        /// </summary>
        private static TrajectoryProjection BuildProjection(
            Trajectory2D trajectory,
            Pose2D vehiclePoseInWorld,
            int segmentStartIndex,
            double interpolationRatio,
            double projectedX,
            double projectedY,
            double distanceSquared)
        {
            var segmentStart =
                trajectory[segmentStartIndex];
            var segmentEnd =
                trajectory[segmentStartIndex + 1];

            var referenceYawRadians =
                AngleMath.LerpRadians(
                    segmentStart.PoseInWorld.YawRadians,
                    segmentEnd.PoseInWorld.YawRadians,
                    interpolationRatio);
            var referenceArcLengthMeters =
                InterpolationMath.Lerp(
                    segmentStart.ArcLengthMeters,
                    segmentEnd.ArcLengthMeters,
                    interpolationRatio);
            var referenceCurvaturePerMeter =
                InterpolationMath.Lerp(
                    segmentStart.CurvaturePerMeter,
                    segmentEnd.CurvaturePerMeter,
                    interpolationRatio);
            var referenceSpeedMetersPerSecond =
                InterpolationMath.Lerp(
                    segmentStart.ReferenceSpeedMetersPerSecond,
                    segmentEnd.ReferenceSpeedMetersPerSecond,
                    interpolationRatio);

            var referencePoint =
                new TrajectoryPoint(
                    referenceArcLengthMeters,
                    new Pose2D(
                        projectedX,
                        projectedY,
                        referenceYawRadians),
                    referenceCurvaturePerMeter,
                    referenceSpeedMetersPerSecond);

            var segmentX =
                segmentEnd.PoseInWorld.XMeters -
                segmentStart.PoseInWorld.XMeters;
            var segmentY =
                segmentEnd.PoseInWorld.YMeters -
                segmentStart.PoseInWorld.YMeters;
            var segmentLength =
                Math.Sqrt(
                    segmentX * segmentX +
                    segmentY * segmentY);

            // 以轨迹线段的前进方向判断左右：
            // 从车辆指向参考轨迹的向量位于轨迹左侧时为正。
            var vehicleToProjectionX =
                projectedX -
                vehiclePoseInWorld.XMeters;
            var vehicleToProjectionY =
                projectedY -
                vehiclePoseInWorld.YMeters;
            var lateralErrorMeters =
                (segmentX * vehicleToProjectionY -
                 segmentY * vehicleToProjectionX) /
                segmentLength;

            var headingErrorRadians =
                AngleMath.ShortestDifferenceRadians(
                    referenceYawRadians,
                    vehiclePoseInWorld.YawRadians);

            return new TrajectoryProjection(
                segmentStartIndex,
                referencePoint,
                lateralErrorMeters,
                headingErrorRadians,
                Math.Sqrt(distanceSquared),
                trajectory.GetRemainingDistanceMeters(
                    referenceArcLengthMeters));
        }

        /// <summary>
        /// 检查用于投影的实际车体中心位姿是否包含有限数值。
        /// </summary>
        private static void EnsureFinitePose(
            Pose2D pose,
            string parameterName)
        {
            if (double.IsNaN(pose.XMeters) ||
                double.IsInfinity(pose.XMeters) ||
                double.IsNaN(pose.YMeters) ||
                double.IsInfinity(pose.YMeters) ||
                double.IsNaN(pose.YawRadians) ||
                double.IsInfinity(pose.YawRadians))
            {
                throw new ArgumentOutOfRangeException(
                    parameterName,
                    "用于轨迹投影的车体位姿必须是有限值。");
            }
        }
    }
}