拆分MultiWheelC并新增轨迹投影、Detour状态估计与Stanley跟踪控制

Co-authored-by: Cursor <cursoragent@cursor.com>

Shared/Mathematics/AngleMath.cs

@@ -81,6 +81,29 @@ namespace MyParking.Shared
             return NormalizeDegrees(targetDegrees - currentDegrees);
         }
 
+        /// <summary>
+        /// 沿圆周最短方向在两个航向角之间插值，输入和结果单位均为弧度。
+        /// ratio为0时返回起始角，ratio为1时返回终止角；本方法不限制ratio，
+        /// 轨迹线段内插值时应先使用InterpolationMath.Clamp01进行限制。
+        /// 结果归一化到[-π, π)区间；角度差恰好为π时按负方向插值。
+        /// </summary>
+        public static double LerpRadians(
+            double startRadians,
+            double endRadians,
+            double ratio)
+        {
+            EnsureFinite(startRadians, nameof(startRadians));
+            EnsureFinite(endRadians, nameof(endRadians));
+            EnsureFinite(ratio, nameof(ratio));
+
+            var shortestDifference = ShortestDifferenceRadians(
+                endRadians,
+                startRadians);
+
+            return NormalizeRadians(
+                startRadians + ratio * shortestDifference);
+        }
+
         /// <summary>
         /// 将角度从度转换为弧度，不进行归一化。
         /// </summary>

Shared/Mathematics/InterpolationMath.cs

@@ -0,0 +1,61 @@
+
+using System;
+
+namespace MyParking.Shared
+{
+    /// <summary>
+    /// 提供与具体业务和坐标系无关的基础插值功能。
+    /// </summary>
+    public static class InterpolationMath
+    {
+        /// <summary>
+        /// 将插值比例限制到[0, 1]闭区间。
+        /// </summary>
+        public static double Clamp01(double value)
+        {
+            EnsureFinite(value, nameof(value));
+
+            if (value <= 0.0)
+            {
+                return 0.0;
+            }
+
+            if (value >= 1.0)
+            {
+                return 1.0;
+            }
+
+            return value;
+        }
+
+        /// <summary>
+        /// 对两个标量执行线性插值。
+        /// ratio为0时返回start，ratio为1时返回end；本方法不限制ratio，
+        /// 因此也支持区间外的线性外插。
+        /// </summary>
+        public static double Lerp(
+            double start,
+            double end,
+            double ratio)
+        {
+            EnsureFinite(start, nameof(start));
+            EnsureFinite(end, nameof(end));
+            EnsureFinite(ratio, nameof(ratio));
+
+            return start + ratio * (end - start);
+        }
+
+        /// <summary>
+        /// 验证输入是可用于插值计算的有限数值。
+        /// </summary>
+        private static void EnsureFinite(double value, string parameterName)
+        {
+            if (double.IsNaN(value) || double.IsInfinity(value))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "插值参数必须是有限数值。");
+            }
+        }
+    }
+}