新增倒车以及项目结构优化

MultiWheelC/Trajectory/Trajectory2D.cs

@@ -7,18 +7,22 @@ using MyParking.Shared;
 namespace MultiWheelC.Trajectory
 {
     /// <summary>
-    /// 保存一条经过基本合法性检查的只读二维参考轨迹。
+    /// 保存一条按预定执行点序排列、以累计弧长参数化的只读二维参考轨迹。
     /// </summary>
     public sealed class Trajectory2D
     {
         private const double StartArcLengthToleranceMeters = 1e-9;
         private const double MinimumSegmentLengthMeters = 1e-6;
+        private const double ArcLengthConsistencyAbsoluteToleranceMeters =
+            1e-6;
+        private const double ArcLengthConsistencyRelativeTolerance =
+            0.01;
 
         private readonly TrajectoryPoint[] _points;
         private readonly ReadOnlyCollection<TrajectoryPoint> _readOnlyPoints;
 
         /// <summary>
-        /// 复制并验证按累计弧长升序排列的参考轨迹点。
+        /// 复制并验证按实际执行顺序及累计弧长升序排列的参考轨迹点。
         /// </summary>
         public Trajectory2D(
             IEnumerable<TrajectoryPoint> points)
@@ -102,7 +106,7 @@ namespace MultiWheelC.Trajectory
         public double GetRemainingDistanceMeters(
             double arcLengthMeters)
         {
-            EnsureFinite(
+            NumericGuard.EnsureFinite(
                 arcLengthMeters,
                 nameof(arcLengthMeters));
 
@@ -121,7 +125,7 @@ namespace MultiWheelC.Trajectory
         public TrajectoryPoint SampleAtArcLength(
             double arcLengthMeters)
         {
-            EnsureFinite(
+            NumericGuard.EnsureFinite(
                 arcLengthMeters,
                 nameof(arcLengthMeters));
 
@@ -148,6 +152,48 @@ namespace MultiWheelC.Trajectory
                 (segmentEnd.ArcLengthMeters -
                  segmentStart.ArcLengthMeters);
 
+            return InterpolateSegment(
+                segmentStartIndex,
+                interpolationRatio);
+        }
+
+        /// <summary>
+        /// 在指定线段上统一插值位置、车头航向、曲率和有符号参考速度。
+        /// </summary>
+        internal TrajectoryPoint InterpolateSegment(
+            int segmentStartIndex,
+            double interpolationRatio)
+        {
+            if (segmentStartIndex < 0 ||
+                segmentStartIndex >= _points.Length - 1)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(segmentStartIndex),
+                    "轨迹插值线段索引必须指向一条有效线段的起点。");
+            }
+
+            NumericGuard.EnsureFinite(
+                interpolationRatio,
+                nameof(interpolationRatio));
+
+            if (interpolationRatio < 0.0 ||
+                interpolationRatio > 1.0)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(interpolationRatio),
+                    "轨迹线段插值比例必须位于[0,1]范围内。");
+            }
+
+            var segmentStart =
+                _points[segmentStartIndex];
+            var segmentEnd =
+                _points[segmentStartIndex + 1];
+            var arcLengthMeters =
+                InterpolationMath.Lerp(
+                    segmentStart.ArcLengthMeters,
+                    segmentEnd.ArcLengthMeters,
+                    interpolationRatio);
+
             return new TrajectoryPoint(
                 arcLengthMeters,
                 new Pose2D(
@@ -176,9 +222,23 @@ namespace MultiWheelC.Trajectory
         /// <summary>
         /// 使用二分查找获取包含指定累计弧长的线段起点索引。
         /// </summary>
-        private int FindSegmentStartIndex(
+        internal int FindSegmentStartIndex(
             double arcLengthMeters)
         {
+            NumericGuard.EnsureFinite(
+                arcLengthMeters,
+                nameof(arcLengthMeters));
+
+            if (arcLengthMeters <= 0.0)
+            {
+                return 0;
+            }
+
+            if (arcLengthMeters >= TotalLengthMeters)
+            {
+                return _points.Length - 2;
+            }
+
             var lowerIndex = 0;
             var upperIndex = _points.Length - 1;
 
@@ -239,21 +299,32 @@ namespace MultiWheelC.Trajectory
                     $"轨迹点{currentIndex}与前一个点的位置过近，无法构成有效投影线段。",
                     parameterName);
             }
-        }
 
-        /// <summary>
-        /// 检查数值是否为有限值。
-        /// </summary>
-        private static void EnsureFinite(
-            double value,
-            string parameterName)
-        {
-            if (double.IsNaN(value) ||
-                double.IsInfinity(value))
+            var segmentLengthMeters =
+                Math.Sqrt(segmentLengthSquared);
+            var arcLengthIncrementMeters =
+                current.ArcLengthMeters -
+                previous.ArcLengthMeters;
+            var maximumAllowedDifferenceMeters =
+                Math.Max(
+                    ArcLengthConsistencyAbsoluteToleranceMeters,
+                    ArcLengthConsistencyRelativeTolerance *
+                    Math.Max(
+                        segmentLengthMeters,
+                        arcLengthIncrementMeters));
+
+            // 当前轨迹在相邻采样点之间按直线段投影，因此累计弧长增量
+            // 必须与该离散线段长度近似一致，防止进度和实际几何脱节。
+            if (Math.Abs(
+                    arcLengthIncrementMeters -
+                    segmentLengthMeters) >
+                maximumAllowedDifferenceMeters)
             {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "轨迹弧长必须是有限值。");
+                throw new ArgumentException(
+                    $"轨迹点{currentIndex}的累计弧长增量" +
+                    $"{arcLengthIncrementMeters:F6}m与离散线段长度" +
+                    $"{segmentLengthMeters:F6}m不一致。",
+                    parameterName);
             }
         }
     }