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

Co-authored-by: Cursor <cursoragent@cursor.com>
2026-08-05 18:07:01 +08:00
parent 7447317812
commit 47973cc94b
38 changed files with 3982 additions and 976 deletions
@@ -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>