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,29 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;
+
+internal static class CurvatureLimitPolicy
+{
+    internal static bool TryGetAllowedMaximumVehicleCurvaturePerMeter(
+        VehicleParameters vehicle,
+        double radiusToleranceMeters,
+        out double allowedMaximumCurvaturePerMeter)
+    {
+        allowedMaximumCurvaturePerMeter = 0d;
+        if (!NumericGuard.IsFinite(radiusToleranceMeters) || radiusToleranceMeters < 0d ||
+            !VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double nominalMaximumCurvature))
+        {
+            return false;
+        }
+
+        double nominalMinimumRadius = 1d / nominalMaximumCurvature;
+        double toleratedMinimumRadius = nominalMinimumRadius - radiusToleranceMeters;
+        if (!NumericGuard.IsPositiveFinite(toleratedMinimumRadius)) return false;
+
+        allowedMaximumCurvaturePerMeter = 1d / toleratedMinimumRadius;
+        return NumericGuard.IsPositiveFinite(allowedMaximumCurvaturePerMeter);
+    }
+}