From 1662b680626188de4eb1c445fea6eeb3f71fdb87 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:37:45 +0800 Subject: [PATCH] fix: harden local G2 candidate splicing --- .../LocalG2/LocalG2CandidateBuilder.cs | 90 ++++++++++++++++++- .../LocalG2/LocalG2PathSplicer.cs | 7 +- ...ify_path_smoothing_local_g2_candidates.ps1 | 12 +++ 3 files changed, 107 insertions(+), 2 deletions(-) diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateBuilder.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateBuilder.cs index 2ab322a..5d25adb 100644 --- a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateBuilder.cs +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateBuilder.cs @@ -12,6 +12,7 @@ namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2; internal sealed class LocalG2CandidateBuilder { private const double MinimumDerivativeNorm = 1e-10d; + private const int MinimumCurveEvaluationIntervals = 32; private const int MaximumSubdivisionDepth = 32; private static readonly double[] DerivativeScaleMultipliers = { 1d, 0.85d, 1.15d }; @@ -194,6 +195,13 @@ 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; + } 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); @@ -361,6 +369,9 @@ internal sealed class LocalG2CandidateBuilder case "Reverse": return BuildReverse(); case "Spliced": return BuildSpliced(); case "StartBoundary": return BuildStartBoundary(); + case "InteriorStationaryCurve": return BuildInteriorStationaryCurve(); + case "ExactSpliceEndpoints": return BuildExactSpliceEndpoints(); + case "GearBoundary": return BuildGearBoundary(); default: throw new ArgumentOutOfRangeException(nameof(scenario)); } } @@ -371,7 +382,8 @@ internal sealed class LocalG2CandidateBuilder double startCurvatureError, double endCurvatureError, bool containsLocalG2Source, int outputRegionCount, bool internalConnectionsAreG2, string direction, bool vehicleAndGeometricCurvatureSignsAreOpposite, bool noDuplicateNonGearPoints, - bool endpointsUnchanged) + bool endpointsUnchanged, bool rejected = false, bool endpointsAreExact = false, + bool gearBoundaryMarkerPreserved = false) { CandidateCount = candidateCount; StartPositionError = startPositionError; @@ -385,6 +397,9 @@ internal sealed class LocalG2CandidateBuilder VehicleAndGeometricCurvatureSignsAreOpposite = vehicleAndGeometricCurvatureSignsAreOpposite; NoDuplicateNonGearPoints = noDuplicateNonGearPoints; EndpointsUnchanged = endpointsUnchanged; + Rejected = rejected; + EndpointsAreExact = endpointsAreExact; + GearBoundaryMarkerPreserved = gearBoundaryMarkerPreserved; } public int CandidateCount { get; } public double StartPositionError { get; } @@ -398,6 +413,9 @@ internal sealed class LocalG2CandidateBuilder public bool VehicleAndGeometricCurvatureSignsAreOpposite { get; } public bool NoDuplicateNonGearPoints { get; } public bool EndpointsUnchanged { get; } + public bool Rejected { get; } + public bool EndpointsAreExact { get; } + public bool GearBoundaryMarkerPreserved { get; } } private static CandidateTestSnapshot BuildIsolated() @@ -517,6 +535,73 @@ internal sealed class LocalG2CandidateBuilder ContainsLocalG2(candidate.RegionPoints), 0, false, string.Empty, false, false, false); } + private static CandidateTestSnapshot BuildInteriorStationaryCurve() + { + const double endpointX = 7d / 1500d; + if (!QuinticHermiteCurve2D.TryCreate(0d, 0d, 0.01d, 0d, 0d, 0d, + endpointX, 0d, 0.01d, 0d, 0d, 0d, out QuinticHermiteCurve2D curve, out string reason)) + { + throw new InvalidOperationException(reason); + } + var points = new[] + { + new SmoothingPoint2D(0d, 0d, 0d, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor), + new SmoothingPoint2D(endpointX, 0d, endpointX, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor), + }; + var segment = new PreparedDirectionSegment(0, TravelDirection.Forward, points, false, false); + var sampled = new List(); + bool accepted = TryAppendCurveSamples(curve, + new BoundaryNode(0d, 0d, 0d, 0d, 0d), + new BoundaryNode(endpointX, endpointX, 0d, 0d, 0d), + segment, 0.025d, sampled, CancellationToken.None); + return new CandidateTestSnapshot(0, 0d, 0d, 0d, 0d, false, 0, false, string.Empty, + false, false, false, !accepted); + } + + private static CandidateTestSnapshot BuildExactSpliceEndpoints() + { + PreparedDirectionSegment segment = CreateSegment(TravelDirection.Forward, null); + var candidate = new LocalG2CandidateGeometry(0, 0, 0.25d, 0.75d, 0.25d, 0.25d, + new[] + { + Point(0.2500000005d, 0d, 0.25d, false), + Point(0.5d, 0d, 0.5d, false), + Point(0.7499999995d, 0d, 0.75d, false), + }, 0d, 0d, 0d, 0d, true); + if (!new LocalG2PathSplicer().TryReplace(new PreparedPath(new[] { segment }), candidate, + out PreparedPath replaced, out string reason)) + { + throw new InvalidOperationException(reason); + } + PathReferenceInterpolator.TryInterpolateByArcLength(segment.Points, 0.25d, out SmoothingPoint2D start, out _); + PathReferenceInterpolator.TryInterpolateByArcLength(segment.Points, 0.75d, out SmoothingPoint2D end, out _); + IReadOnlyList result = replaced.Segments[0].Points; + bool exact = result[1].X == start.X && result[1].Y == start.Y && + result[result.Count - 2].X == end.X && result[result.Count - 2].Y == end.Y; + return new CandidateTestSnapshot(0, 0d, 0d, 0d, 0d, false, 0, false, string.Empty, + false, false, false, false, exact); + } + + private static CandidateTestSnapshot BuildGearBoundary() + { + var sourcePoints = new[] + { + Point(0d, 0d, 0d, true), Point(0.25d, 0d, 0.25d, false), Point(0.5d, 0d, 0.5d, false), + }; + var segment = new PreparedDirectionSegment(0, TravelDirection.Forward, sourcePoints, true, false); + var candidate = new LocalG2CandidateGeometry(0, 0, 0d, 0.5d, 0d, 0.5d, + new[] { Point(0d, 0d, 0d, false), Point(0.25d, 0d, 0.25d, false), Point(0.5d, 0d, 0.5d, false) }, + 0d, 0d, 0d, 0d, true); + if (!new LocalG2PathSplicer().TryReplace(new PreparedPath(new[] { segment }), candidate, + out PreparedPath replaced, out string reason)) + { + throw new InvalidOperationException(reason); + } + bool preserved = replaced.Segments[0].StartsAtGearSwitch && replaced.Segments[0].Points[0].IsGearSwitchPoint; + return new CandidateTestSnapshot(0, 0d, 0d, 0d, 0d, false, 0, false, string.Empty, + false, false, false, false, false, preserved); + } + private static PreparedDirectionSegment CreateSegment(TravelDirection direction, double? startCurvature) { double heading = direction == TravelDirection.Forward ? 0d : Math.PI; @@ -549,5 +634,8 @@ internal sealed class LocalG2CandidateBuilder } private static double Distance(SmoothingPoint2D left, SmoothingPoint2D right) => Math.Sqrt((right.X - left.X) * (right.X - left.X) + (right.Y - left.Y) * (right.Y - left.Y)); + private static SmoothingPoint2D Point(double x, double y, double arcLength, bool isGearSwitch) => + new SmoothingPoint2D(x, y, arcLength, 0d, 0d, 1d, isGearSwitch, + isGearSwitch ? SmoothedPathPointSource.GearSwitch : SmoothedPathPointSource.LocalG2Transition); } } diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2PathSplicer.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2PathSplicer.cs index 55a4b02..294efe0 100644 --- a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2PathSplicer.cs +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2PathSplicer.cs @@ -40,7 +40,12 @@ internal sealed class LocalG2PathSplicer SmoothingPoint2D point = source.Points[index]; if (point.ArcLength < candidate.StartArcLengthMeters) AddWithoutNonGearDuplicates(combined, point); } - for (int index = 0; index < candidate.RegionPoints.Count; index++) AddWithoutNonGearDuplicates(combined, candidate.RegionPoints[index]); + // Candidate endpoints are only validated within a numerical tolerance. The replacement + // itself must use the exact source-window endpoints, including a possible gear marker. + AddWithoutNonGearDuplicates(combined, start); + for (int index = 1; index < candidate.RegionPoints.Count - 1; index++) + AddWithoutNonGearDuplicates(combined, candidate.RegionPoints[index]); + AddWithoutNonGearDuplicates(combined, end); for (int index = 0; index < source.Points.Count; index++) { SmoothingPoint2D point = source.Points[index]; diff --git a/ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1 b/ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1 index 12596d3..4483bc5 100644 --- a/ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1 +++ b/ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1 @@ -58,4 +58,16 @@ Assert-True ($startBoundary.CandidateCount -gt 0) 'A start-boundary event must b Assert-Near 0.0 $startBoundary.StartCurvatureError 1e-8 ` 'A start-boundary candidate must use the preserved physical start vehicle curvature.' +$interiorDerivative = Invoke-Scenario 'InteriorStationaryCurve' +Assert-True $interiorDerivative.Rejected ` + 'A curve with a stationary interior derivative must be rejected even when its endpoint chord is short.' + +$exactSplice = Invoke-Scenario 'ExactSpliceEndpoints' +Assert-True $exactSplice.EndpointsAreExact ` + 'Splicing must write interpolated exact endpoints instead of accepting approximate candidate endpoints.' + +$gearBoundary = Invoke-Scenario 'GearBoundary' +Assert-True $gearBoundary.GearBoundaryMarkerPreserved ` + 'A window touching a gear-switch segment boundary must preserve its point-level gear marker.' + Write-Output 'Path smoothing Local G2 candidate checks passed.'