diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/PiecewiseQuinticSmoother.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/PiecewiseQuinticSmoother.cs index aa31a0b..5407a0f 100644 --- a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/PiecewiseQuinticSmoother.cs +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/PiecewiseQuinticSmoother.cs @@ -11,6 +11,8 @@ namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms; internal sealed class PiecewiseQuinticSmoother : IPathSmoother { private const int SamplesPerInterval = 8; + private const double DoubleMachineEpsilon = 2.2204460492503131e-16d; + private const double EndpointNormalizationUlps = 32d; /// public SmoothingMethod Method => SmoothingMethod.PiecewiseQuintic; @@ -237,15 +239,26 @@ internal sealed class PiecewiseQuinticSmoother : IPathSmoother } knots.Add(first); - double targetArcLength = startArcLength + knotSpacingMeters; - while (targetArcLength < endArcLength) + double endpointTolerance = GetEndpointNormalizationTolerance( + startArcLength, + endArcLength, + knotSpacingMeters); + double previousArcLength = startArcLength; + for (long knotOrdinal = 1L; ; knotOrdinal++) { cancellationToken.ThrowIfCancellationRequested(); + double targetArcLength = startArcLength + knotOrdinal * knotSpacingMeters; if (!NumericGuard.IsFinite(targetArcLength)) { reason = "五次 Hermite 内部结点弧长无效。"; return false; } + if (targetArcLength >= endArcLength - endpointTolerance) break; + if (targetArcLength <= previousArcLength) + { + reason = "五次 Hermite 内部结点无法在浮点弧长尺度上保持递增。"; + return false; + } if (!PathReferenceInterpolator.TryInterpolateByArcLength( anchors, targetArcLength, @@ -257,7 +270,7 @@ internal sealed class PiecewiseQuinticSmoother : IPathSmoother return false; } knots.Add(knot); - targetArcLength += knotSpacingMeters; + previousArcLength = targetArcLength; } if (!TryCreateKnot(anchors[anchors.Count - 1], direction, effectiveStrength, out Knot last)) @@ -279,6 +292,17 @@ internal sealed class PiecewiseQuinticSmoother : IPathSmoother return true; } + private static double GetEndpointNormalizationTolerance( + double startArcLength, + double endArcLength, + double knotSpacingMeters) + { + double magnitude = Math.Max( + Math.Abs(startArcLength), + Math.Max(Math.Abs(endArcLength), Math.Abs(knotSpacingMeters))); + return EndpointNormalizationUlps * DoubleMachineEpsilon * magnitude; + } + private static bool TryCreateKnot( SmoothingPoint2D reference, TravelDirection direction, diff --git a/ClumsyPilot/tests/verify_path_smoothing_quintic.ps1 b/ClumsyPilot/tests/verify_path_smoothing_quintic.ps1 index 59f027d..3a0f210 100644 --- a/ClumsyPilot/tests/verify_path_smoothing_quintic.ps1 +++ b/ClumsyPilot/tests/verify_path_smoothing_quintic.ps1 @@ -297,6 +297,31 @@ Assert-Equal 'Failed' (Get-PropertyValue $shortCandidate 'Status').ToString() 'A Assert-True (-not (Get-PropertyValue $shortCandidate 'Succeeded')) 'A degenerate short quintic segment must not be executable.' Assert-Equal 0 (Get-PropertyValue $shortCandidate 'Segments').Count 'A terminal quintic degeneracy must publish no geometry.' +# Repeated decimal addition must not create a spurious 1e-16 m terminal residual when the +# requested spacing divides the local length exactly. A real 0.005 m remainder remains below +# the 0.01 m minimum and must still fail terminally. +$decimalMultipleSource = @( + (New-Point 0.0 0.0 0.0 0.0), + (New-Point 1.0 0.0 1.0 0.0)) +$decimalMultipleCandidate = Invoke-Candidate @((New-DirectionSegment 0 $forward $decimalMultipleSource)) 0.0 0.10 0.01 +Assert-Equal 'Success' (Get-PropertyValue $decimalMultipleCandidate 'Status').ToString() 'Decimal-exact knot multiples must not become a terminal under-minimum residual failure.' +$decimalMultipleOutput = @(Get-PropertyValue $decimalMultipleCandidate 'Segments')[0].Points +Assert-Equal 81 $decimalMultipleOutput.Count 'A 1.0 m segment with 0.1 m spacing must create exactly ten valid quintic intervals.' +Assert-Near 1.0 $decimalMultipleOutput[$decimalMultipleOutput.Count - 1].ArcLength 0.0 'Decimal-multiple knot normalization must retain the exact terminal arc length.' + +$meaningfulShortResidualSource = @( + (New-Point 0.0 0.0 0.0 0.0), + (New-Point 1.005 0.0 1.005 0.0)) +$meaningfulShortResidualCandidate = Invoke-Candidate @((New-DirectionSegment 0 $forward $meaningfulShortResidualSource)) 0.0 0.10 0.01 +Assert-Equal 'Failed' (Get-PropertyValue $meaningfulShortResidualCandidate 'Status').ToString() 'A genuine 0.005 m terminal residual must remain a terminal spacing failure.' +Assert-Equal 0 (Get-PropertyValue $meaningfulShortResidualCandidate 'Segments').Count 'A genuine under-minimum residual must publish no geometry.' + +$smallScaleResidualSource = @( + (New-Point 0.0 0.0 0.0 0.0), + (New-Point 0.000000000000105 0.0 0.000000000000105 0.0)) +$smallScaleResidualCandidate = Invoke-Candidate @((New-DirectionSegment 0 $forward $smallScaleResidualSource)) 0.0 0.00000000000001 0.000000000000006 +Assert-Equal 'Failed' (Get-PropertyValue $smallScaleResidualCandidate 'Status').ToString() 'Endpoint normalization tolerance must scale with local arc magnitude and preserve a genuine small residual failure.' + # Local-arc reference mapping, rather than sample index/global distance, must reject this unsafe nonuniform path. $nonuniformUnsafeSource = @( (New-Point 0.0 0.0 0.0 0.0 0.50),