From 9edb80e5522e2b0c45b85172a25c73e2b0fdae63 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Thu, 30 Jul 2026 17:44:37 +0800 Subject: [PATCH] fix: certify local G2 curve derivatives --- .../LocalG2/LocalG2CandidateBuilder.cs | 171 +++++++++++++++++- 1 file changed, 162 insertions(+), 9 deletions(-) diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateBuilder.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateBuilder.cs index 5d25adb..eb17400 100644 --- a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateBuilder.cs +++ b/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 output, CancellationToken cancellationToken) { - // A short endpoint chord does not prove that a fifth-degree curve has no interior cusp. - // Check a deterministic interior grid before the chord-driven subdivision below; every - // subsequently emitted subdivision parameter is checked again by TryEvaluate. - for (int index = 0; index <= MinimumCurveEvaluationIntervals; index++) - { - if (!TryEvaluate(curve, (double)index / MinimumCurveEvaluationIntervals, out _)) return false; - } + // A short endpoint chord and a finite sample grid cannot rule out an interior cusp. + // Certify a derivative-norm lower bound over the whole parameter interval first. + if (!TryCertifyDerivativeLowerBound(curve)) 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); + } + /// 反射脚本使用的窄范围候选与拼接场景入口。 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)) {