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,55 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
+
+/// <summary>同一方向原语边界两侧车辆曲率的离散跳变。</summary>
+internal sealed class CurvatureTransition
+{
+    internal CurvatureTransition(
+        int segmentIndex,
+        int leftCoarsePathIndex,
+        int rightCoarsePathIndex,
+        double localArcLengthMeters,
+        double x,
+        double y,
+        double vehicleHeadingRadians,
+        double leftVehicleCurvaturePerMeter,
+        double rightVehicleCurvaturePerMeter)
+    {
+        if (segmentIndex < 0 || leftCoarsePathIndex < 0 || rightCoarsePathIndex != leftCoarsePathIndex + 1)
+            throw new ArgumentOutOfRangeException(nameof(leftCoarsePathIndex));
+        if (!NumericGuard.IsFinite(localArcLengthMeters) || localArcLengthMeters < 0d ||
+            !NumericGuard.IsFinite(x) || !NumericGuard.IsFinite(y) || !NumericGuard.IsFinite(vehicleHeadingRadians) ||
+            !NumericGuard.IsFinite(leftVehicleCurvaturePerMeter) || !NumericGuard.IsFinite(rightVehicleCurvaturePerMeter))
+        {
+            throw new ArgumentOutOfRangeException(nameof(localArcLengthMeters));
+        }
+
+        double curvatureJumpPerMeter = Math.Abs(rightVehicleCurvaturePerMeter - leftVehicleCurvaturePerMeter);
+        if (!NumericGuard.IsFinite(curvatureJumpPerMeter))
+            throw new ArgumentOutOfRangeException(nameof(rightVehicleCurvaturePerMeter));
+
+        SegmentIndex = segmentIndex;
+        LeftCoarsePathIndex = leftCoarsePathIndex;
+        RightCoarsePathIndex = rightCoarsePathIndex;
+        LocalArcLengthMeters = localArcLengthMeters;
+        X = x;
+        Y = y;
+        VehicleHeadingRadians = vehicleHeadingRadians;
+        LeftVehicleCurvaturePerMeter = leftVehicleCurvaturePerMeter;
+        RightVehicleCurvaturePerMeter = rightVehicleCurvaturePerMeter;
+        CurvatureJumpPerMeter = curvatureJumpPerMeter;
+    }
+
+    internal int SegmentIndex { get; }
+    internal int LeftCoarsePathIndex { get; }
+    internal int RightCoarsePathIndex { get; }
+    internal double LocalArcLengthMeters { get; }
+    internal double X { get; }
+    internal double Y { get; }
+    internal double VehicleHeadingRadians { get; }
+    internal double LeftVehicleCurvaturePerMeter { get; }
+    internal double RightVehicleCurvaturePerMeter { get; }
+    internal double CurvatureJumpPerMeter { get; }
+}