fix: preserve trusted raw path curvature

2026-07-31 15:28:49 +08:00
parent 9173f9bfb5
commit 272a847b2a
3 changed files with 362 additions and 13 deletions
@@ -199,20 +199,20 @@ public sealed class PathGeometryAnalyzer
            {
                geometricCurvatures[index] = 0d;
            }
-            else if (index == 0)
-            {
-                geometricCurvatures[index] = (unwrappedHeadings[1] - unwrappedHeadings[0]) /
-                    (localArcLengths[1] - localArcLengths[0]);
-            }
-            else if (index == count - 1)
-            {
-                geometricCurvatures[index] = (unwrappedHeadings[index] - unwrappedHeadings[index - 1]) /
-                    (localArcLengths[index] - localArcLengths[index - 1]);
-            }
            else
            {
-                geometricCurvatures[index] = (unwrappedHeadings[index + 1] - unwrappedHeadings[index - 1]) /
-                    (localArcLengths[index + 1] - localArcLengths[index - 1]);
+                int leftIndex = index == 0 ? 0 : index - 1;
+                int rightIndex = index == count - 1 ? count - 1 : index + 1;
+                if (!TryEstimateGeometricCurvature(
+                        samples,
+                        unwrappedHeadings,
+                        leftIndex,
+                        rightIndex,
+                        out geometricCurvatures[index],
+                        out reason))
+                {
+                    return false;
+                }
            }

            if (!NumericGuard.IsFinite(geometricCurvatures[index]))
@@ -311,6 +311,44 @@ public sealed class PathGeometryAnalyzer
        return true;
    }

+    private static bool TryEstimateGeometricCurvature(
+        IReadOnlyList<SmoothingPoint2D> samples,
+        IReadOnlyList<double> unwrappedHeadings,
+        int leftIndex,
+        int rightIndex,
+        out double curvature,
+        out string reason)
+    {
+        curvature = 0d;
+        reason = string.Empty;
+        double chordLength = Distance(samples[leftIndex], samples[rightIndex]);
+        if (!NumericGuard.IsFinite(chordLength) || chordLength <= 0d)
+        {
+            reason = "候选路径曲率估计弦长无效。";
+            return false;
+        }
+
+        double deltaHeading = unwrappedHeadings[rightIndex] - unwrappedHeadings[leftIndex];
+        if (!NumericGuard.IsFinite(deltaHeading))
+        {
+            reason = "候选路径曲率估计航向差无效。";
+            return false;
+        }
+        if (Math.Abs(deltaHeading) >= Math.PI)
+        {
+            reason = "候选路径曲率估计航向差存在π歧义。";
+            return false;
+        }
+
+        curvature = 2d * Math.Sin(deltaHeading / 2d) / chordLength;
+        if (!NumericGuard.IsFinite(curvature))
+        {
+            reason = "候选路径曲率计算产生了非法数值。";
+            return false;
+        }
+        return true;
+    }
+
    private static bool TryResampleByGeometry(
        IReadOnlyList<SmoothingPoint2D> input,
        double spacingMeters,