fix: certify local G2 curve derivatives

ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateBuilder.cs

@@ -12,7 +12,9 @@ namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
 internal sealed class LocalG2CandidateBuilder
 {
     private const double MinimumDerivativeNorm = 1e-10d;
-    private const int MinimumCurveEvaluationIntervals = 32;
+    private const int MaximumDerivativeCertificationDepth = 40;
+    private const int MaximumDerivativeCertificationIntervals = 8192;
+    private const double DerivativeCertificationMargin = 1e-12d;
     private const int MaximumSubdivisionDepth = 32;
     private static readonly double[] DerivativeScaleMultipliers = { 1d, 0.85d, 1.15d };
 
@@ -195,13 +197,9 @@ internal sealed class LocalG2CandidateBuilder
         List<SmoothingPoint2D> output,
         CancellationToken cancellationToken)
     {
-        // A short endpoint chord does not prove that a fifth-degree curve has no interior cusp.
+        // A short endpoint chord and a finite sample grid cannot rule out an interior cusp.
-        // Check a deterministic interior grid before the chord-driven subdivision below; every
+        // Certify a derivative-norm lower bound over the whole parameter interval first.
-        // subsequently emitted subdivision parameter is checked again by TryEvaluate.
+        if (!TryCertifyDerivativeLowerBound(curve)) return false;
-        for (int index = 0; index <= MinimumCurveEvaluationIntervals; index++)
-        {
-            if (!TryEvaluate(curve, (double)index / MinimumCurveEvaluationIntervals, out _)) return false;
-        }
         if (!TryEvaluate(curve, 0d, out CurveSample start) || !TryEvaluate(curve, 1d, out CurveSample end)) return false;
         if (output.Count == 0 && !TryAppendPoint(start, left, right, segment, output)) return false;
         return TrySubdivide(curve, left, right, segment, outputSpacingMeters, 0d, start, 1d, end, 0, output, cancellationToken);
@@ -278,6 +276,148 @@ internal sealed class LocalG2CandidateBuilder
         return true;
     }
 
+    private static bool TryCertifyDerivativeLowerBound(QuinticHermiteCurve2D curve)
+    {
+        if (!TryCreateDerivativeBezierControls(curve, out DerivativeControlPoint d0, out DerivativeControlPoint d1,
+                out DerivativeControlPoint d2, out DerivativeControlPoint d3, out DerivativeControlPoint d4))
+        {
+            return false;
+        }
+        int visitedIntervals = 0;
+        return TryCertifyDerivativeLowerBound(d0, d1, d2, d3, d4, 0, ref visitedIntervals);
+    }
+
+    private static bool TryCreateDerivativeBezierControls(
+        QuinticHermiteCurve2D curve,
+        out DerivativeControlPoint d0,
+        out DerivativeControlPoint d1,
+        out DerivativeControlPoint d2,
+        out DerivativeControlPoint d3,
+        out DerivativeControlPoint d4)
+    {
+        d0 = d1 = d2 = d3 = d4 = default;
+        curve.Evaluate(0d, out _, out _, out double dx0, out double dy0, out double ddx0, out double ddy0);
+        curve.Evaluate(0.5d, out _, out _, out double dxMiddle, out double dyMiddle, out _, out _);
+        curve.Evaluate(1d, out _, out _, out double dxEnd, out double dyEnd, out double ddxEnd, out double ddyEnd);
+        d0 = new DerivativeControlPoint(dx0, dy0);
+        d1 = new DerivativeControlPoint(dx0 + ddx0 / 4d, dy0 + ddy0 / 4d);
+        d4 = new DerivativeControlPoint(dxEnd, dyEnd);
+        d3 = new DerivativeControlPoint(dxEnd - ddxEnd / 4d, dyEnd - ddyEnd / 4d);
+        d2 = new DerivativeControlPoint(
+            (16d * dxMiddle - d0.X - 4d * d1.X - 4d * d3.X - d4.X) / 6d,
+            (16d * dyMiddle - d0.Y - 4d * d1.Y - 4d * d3.Y - d4.Y) / 6d);
+        return d0.IsFinite && d1.IsFinite && d2.IsFinite && d3.IsFinite && d4.IsFinite;
+    }
+
+    private static bool TryCertifyDerivativeLowerBound(
+        DerivativeControlPoint d0,
+        DerivativeControlPoint d1,
+        DerivativeControlPoint d2,
+        DerivativeControlPoint d3,
+        DerivativeControlPoint d4,
+        int depth,
+        ref int visitedIntervals)
+    {
+        if (++visitedIntervals > MaximumDerivativeCertificationIntervals) return false;
+        if (!TryGetOriginToConvexHullDistance(d0, d1, d2, d3, d4, out double lowerBound, out double margin))
+            return false;
+        if (lowerBound > MinimumDerivativeNorm + margin) return true;
+        if (depth >= MaximumDerivativeCertificationDepth) return false;
+
+        DerivativeControlPoint d01 = Midpoint(d0, d1);
+        DerivativeControlPoint d12 = Midpoint(d1, d2);
+        DerivativeControlPoint d23 = Midpoint(d2, d3);
+        DerivativeControlPoint d34 = Midpoint(d3, d4);
+        DerivativeControlPoint d012 = Midpoint(d01, d12);
+        DerivativeControlPoint d123 = Midpoint(d12, d23);
+        DerivativeControlPoint d234 = Midpoint(d23, d34);
+        DerivativeControlPoint d0123 = Midpoint(d012, d123);
+        DerivativeControlPoint d1234 = Midpoint(d123, d234);
+        DerivativeControlPoint middle = Midpoint(d0123, d1234);
+        if (!d01.IsFinite || !d12.IsFinite || !d23.IsFinite || !d34.IsFinite || !d012.IsFinite || !d123.IsFinite ||
+            !d234.IsFinite || !d0123.IsFinite || !d1234.IsFinite || !middle.IsFinite)
+        {
+            return false;
+        }
+
+        return TryCertifyDerivativeLowerBound(d0, d01, d012, d0123, middle, depth + 1, ref visitedIntervals) &&
+            TryCertifyDerivativeLowerBound(middle, d1234, d234, d34, d4, depth + 1, ref visitedIntervals);
+    }
+
+    private static bool TryGetOriginToConvexHullDistance(
+        DerivativeControlPoint d0,
+        DerivativeControlPoint d1,
+        DerivativeControlPoint d2,
+        DerivativeControlPoint d3,
+        DerivativeControlPoint d4,
+        out double distance,
+        out double margin)
+    {
+        distance = 0d;
+        margin = 0d;
+        var controls = new[] { d0, d1, d2, d3, d4 };
+        double maximumCoordinate = 0d;
+        for (int index = 0; index < controls.Length; index++)
+        {
+            if (!controls[index].IsFinite) return false;
+            maximumCoordinate = Math.Max(maximumCoordinate, Math.Max(Math.Abs(controls[index].X), Math.Abs(controls[index].Y)));
+        }
+        if (!NumericGuard.IsFinite(maximumCoordinate)) return false;
+        margin = DerivativeCertificationMargin * Math.Max(1d, maximumCoordinate);
+
+        for (int first = 0; first < controls.Length - 2; first++)
+        {
+            for (int second = first + 1; second < controls.Length - 1; second++)
+            {
+                for (int third = second + 1; third < controls.Length; third++)
+                {
+                    if (ContainsOrigin(controls[first], controls[second], controls[third], margin)) return true;
+                }
+            }
+        }
+
+        distance = double.PositiveInfinity;
+        for (int first = 0; first < controls.Length; first++)
+        {
+            distance = Math.Min(distance, controls[first].Norm);
+            for (int second = first + 1; second < controls.Length; second++)
+                distance = Math.Min(distance, DistanceToSegment(controls[first], controls[second]));
+        }
+        return NumericGuard.IsFinite(distance);
+    }
+
+    private static bool ContainsOrigin(DerivativeControlPoint first, DerivativeControlPoint second,
+        DerivativeControlPoint third, double margin)
+    {
+        double crossFirstSecond = Cross(first, second);
+        double crossSecondThird = Cross(second, third);
+        double crossThirdFirst = Cross(third, first);
+        if (!NumericGuard.IsFinite(crossFirstSecond) || !NumericGuard.IsFinite(crossSecondThird) || !NumericGuard.IsFinite(crossThirdFirst))
+            return true;
+        double areaMargin = margin * Math.Max(1d, Math.Max(first.Norm, Math.Max(second.Norm, third.Norm)));
+        return (crossFirstSecond >= -areaMargin && crossSecondThird >= -areaMargin && crossThirdFirst >= -areaMargin) ||
+            (crossFirstSecond <= areaMargin && crossSecondThird <= areaMargin && crossThirdFirst <= areaMargin);
+    }
+
+    private static double DistanceToSegment(DerivativeControlPoint start, DerivativeControlPoint end)
+    {
+        double dx = end.X - start.X;
+        double dy = end.Y - start.Y;
+        double denominator = dx * dx + dy * dy;
+        if (!NumericGuard.IsFinite(denominator)) return double.NaN;
+        if (denominator == 0d) return start.Norm;
+        double parameter = -(start.X * dx + start.Y * dy) / denominator;
+        if (!NumericGuard.IsFinite(parameter)) return double.NaN;
+        parameter = Math.Max(0d, Math.Min(1d, parameter));
+        double x = start.X + parameter * dx;
+        double y = start.Y + parameter * dy;
+        return Math.Sqrt(x * x + y * y);
+    }
+
+    private static DerivativeControlPoint Midpoint(DerivativeControlPoint left, DerivativeControlPoint right) =>
+        new DerivativeControlPoint((left.X + right.X) / 2d, (left.Y + right.Y) / 2d);
+    private static double Cross(DerivativeControlPoint left, DerivativeControlPoint right) => left.X * right.Y - left.Y * right.X;
+
     private static bool IsValidInput(PreparedDirectionSegment segment, LocalG2SmoothingRegion region,
         double outputSpacingMeters, LocalG2OptionsSnapshot options)
     {
@@ -356,6 +496,19 @@ internal sealed class LocalG2CandidateBuilder
         internal double Dy { get; }
     }
 
+    private readonly struct DerivativeControlPoint
+    {
+        internal DerivativeControlPoint(double x, double y)
+        {
+            X = x;
+            Y = y;
+        }
+        internal double X { get; }
+        internal double Y { get; }
+        internal bool IsFinite => NumericGuard.IsFinite(X) && NumericGuard.IsFinite(Y);
+        internal double Norm => Math.Sqrt(X * X + Y * Y);
+    }
+
     /// <summary>反射脚本使用的窄范围候选与拼接场景入口。</summary>
     public static class TestHooks
     {
@@ -537,7 +690,7 @@ internal sealed class LocalG2CandidateBuilder
 
         private static CandidateTestSnapshot BuildInteriorStationaryCurve()
         {
-            const double endpointX = 7d / 1500d;
+            const double endpointX = 0.00325d;
             if (!QuinticHermiteCurve2D.TryCreate(0d, 0d, 0.01d, 0d, 0d, 0d,
                     endpointX, 0d, 0.01d, 0d, 0d, 0d, out QuinticHermiteCurve2D curve, out string reason))
             {