chore: save current workspace progress

2026-08-09 22:13:18 +08:00
parent 650c2ab0e3
commit 2f4fd15e52
449 changed files with 76593 additions and 971 deletions
@@ -0,0 +1,40 @@
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
+
+/// <summary>
+/// 从车辆参数提取运动学限制的辅助方法。
+/// 曲率单位为 1/m；当最大曲率与最小转弯半径同时给出时，始终选择更保守的较小曲率。
+/// </summary>
+public static class VehicleKinematics
+{
+    /// <summary>
+    /// 尝试获取车辆允许的最大绝对曲率。
+    /// 参数：vehicle 为车辆参数；maximumCurvaturePerMeter 为输出的正有限曲率，单位 1/m。
+    /// 返回：至少提供一种正有限曲率限制时为 true；任一已提供限制无效或两种限制均未提供时为 false。
+    /// </summary>
+    public static bool TryGetMaximumCurvaturePerMeter(VehicleParameters vehicle, out double maximumCurvaturePerMeter)
+    {
+        maximumCurvaturePerMeter = 0d;
+        if (vehicle == null) return false;
+
+        bool hasMaximumCurvature = vehicle.MaximumCurvaturePerMeter.HasValue;
+        bool hasMinimumRadius = vehicle.MinimumTurningRadiusMeters.HasValue;
+        if (hasMaximumCurvature && !NumericGuard.IsPositiveFinite(vehicle.MaximumCurvaturePerMeter.Value)) return false;
+        if (hasMinimumRadius && !NumericGuard.IsPositiveFinite(vehicle.MinimumTurningRadiusMeters.Value)) return false;
+        if (!hasMaximumCurvature && !hasMinimumRadius) return false;
+
+        if (hasMaximumCurvature && hasMinimumRadius)
+        {
+            maximumCurvaturePerMeter = System.Math.Min(
+                vehicle.MaximumCurvaturePerMeter.Value,
+                1d / vehicle.MinimumTurningRadiusMeters.Value);
+            return true;
+        }
+
+        maximumCurvaturePerMeter = hasMaximumCurvature
+            ? vehicle.MaximumCurvaturePerMeter.Value
+            : 1d / vehicle.MinimumTurningRadiusMeters.Value;
+        return NumericGuard.IsPositiveFinite(maximumCurvaturePerMeter);
+    }
+}