From 5e6e068165f3bc44bb49adfeb8ae05129e7bb609 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Wed, 29 Jul 2026 11:01:28 +0800 Subject: [PATCH] fix: enforce b-spline displacement constraints --- .../Algorithms/CubicBSplineSmoother.cs | 128 ++++++++++++++---- .../tests/verify_path_smoothing_bspline.ps1 | 28 +++- 2 files changed, 129 insertions(+), 27 deletions(-) diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/CubicBSplineSmoother.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/CubicBSplineSmoother.cs index 168ac04..f984430 100644 --- a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/CubicBSplineSmoother.cs +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/CubicBSplineSmoother.cs @@ -15,6 +15,7 @@ internal sealed class CubicBSplineSmoother : IPathSmoother private const double StraightToleranceMeters = 1e-9d; private const double EndpointTangentScale = 1d / 3d; private const double EndpointProbeParameter = 1e-6d; + private const double MinimumTangentHandleLengthMeters = 1e-10d; /// public SmoothingMethod Method => SmoothingMethod.CubicBSpline; @@ -88,11 +89,9 @@ internal sealed class CubicBSplineSmoother : IPathSmoother return true; } - Point2D[] controls = CreateControls(anchors, sourceSegment.Direction, strength, reserveMeters, - cancellationToken); - if (controls == null) + if (!TryCreateControls(anchors, sourceSegment.Direction, strength, reserveMeters, + cancellationToken, out Point2D[] controls, out reason)) { - reason = "B 样条控制点构造产生非法数值。"; return false; } @@ -100,45 +99,73 @@ internal sealed class CubicBSplineSmoother : IPathSmoother var sampled = new List(); int spanCount = controls.Length - Degree; int uniformIntervals = spanCount * SamplesPerSpan; - AddSample(0d, anchors, controls, knots, sampled, cancellationToken); - AddSample(EndpointProbeParameter, anchors, controls, knots, sampled, cancellationToken); + if (!TryAddSample(0d, anchors, controls, knots, reserveMeters, sampled, cancellationToken, out reason)) + return false; + if (!TryAddSample(EndpointProbeParameter, anchors, controls, knots, reserveMeters, + sampled, cancellationToken, out reason)) + return false; for (int index = 1; index < uniformIntervals; index++) { - AddSample((double)index / uniformIntervals, anchors, controls, knots, sampled, cancellationToken); + if (!TryAddSample((double)index / uniformIntervals, anchors, controls, knots, reserveMeters, + sampled, cancellationToken, out reason)) + { + return false; + } } - AddSample(1d - EndpointProbeParameter, anchors, controls, knots, sampled, cancellationToken); - AddSample(1d, anchors, controls, knots, sampled, cancellationToken); + if (!TryAddSample(1d - EndpointProbeParameter, anchors, controls, knots, reserveMeters, + sampled, cancellationToken, out reason)) + return false; + if (!TryAddSample(1d, anchors, controls, knots, reserveMeters, sampled, cancellationToken, out reason)) + return false; result = sampled; return true; } - private static Point2D[] CreateControls( + private static bool TryCreateControls( IReadOnlyList anchors, TravelDirection direction, double strength, double reserveMeters, - CancellationToken cancellationToken) + CancellationToken cancellationToken, + out Point2D[] controls, + out string reason) { - var controls = new Point2D[anchors.Count]; + reason = string.Empty; + controls = new Point2D[anchors.Count]; controls[0] = Point2D.FromAnchor(anchors[0]); controls[controls.Length - 1] = Point2D.FromAnchor(anchors[anchors.Count - 1]); double startHandleLength = Distance(anchors[0], anchors[1]) * EndpointTangentScale * strength; double startTravelHeading = GetTravelHeading(anchors[0], direction); - Point2D startProposed = new Point2D( - anchors[0].X + startHandleLength * Math.Cos(startTravelHeading), - anchors[0].Y + startHandleLength * Math.Sin(startTravelHeading)); - controls[1] = ClampDisplacement(anchors[1], startProposed, GetAllowedRadius(anchors[1], reserveMeters)); + if (!TryConstrainTangentHandle( + Point2D.FromAnchor(anchors[0]), + Math.Cos(startTravelHeading), + Math.Sin(startTravelHeading), + startHandleLength, + anchors[1], + GetAllowedRadius(anchors[1], reserveMeters), + out controls[1])) + { + reason = "B 样条起点切向手柄无法同时满足相邻锚点移动范围。"; + return false; + } int finalIndex = anchors.Count - 1; double endHandleLength = Distance(anchors[finalIndex - 1], anchors[finalIndex]) * EndpointTangentScale * strength; double endTravelHeading = GetTravelHeading(anchors[finalIndex], direction); - Point2D endProposed = new Point2D( - anchors[finalIndex].X - endHandleLength * Math.Cos(endTravelHeading), - anchors[finalIndex].Y - endHandleLength * Math.Sin(endTravelHeading)); - controls[finalIndex - 1] = ClampDisplacement( - anchors[finalIndex - 1], endProposed, GetAllowedRadius(anchors[finalIndex - 1], reserveMeters)); + if (!TryConstrainTangentHandle( + Point2D.FromAnchor(anchors[finalIndex]), + -Math.Cos(endTravelHeading), + -Math.Sin(endTravelHeading), + endHandleLength, + anchors[finalIndex - 1], + GetAllowedRadius(anchors[finalIndex - 1], reserveMeters), + out controls[finalIndex - 1])) + { + reason = "B 样条终点切向手柄无法同时满足相邻锚点移动范围。"; + return false; + } for (int index = 2; index < finalIndex - 1; index++) { @@ -158,22 +185,34 @@ internal sealed class CubicBSplineSmoother : IPathSmoother for (int index = 0; index < controls.Length; index++) { if (!NumericGuard.IsFinite(controls[index].X) || !NumericGuard.IsFinite(controls[index].Y)) - return null; + { + reason = "B 样条控制点构造产生非法数值。"; + return false; + } } - return controls; + return true; } - private static void AddSample( + private static bool TryAddSample( double parameter, IReadOnlyList anchors, Point2D[] controls, double[] knots, + double reserveMeters, List output, - CancellationToken cancellationToken) + CancellationToken cancellationToken, + out string reason) { + reason = string.Empty; cancellationToken.ThrowIfCancellationRequested(); Point2D evaluated = Evaluate(controls, knots, parameter); SmoothingPoint2D reference = InterpolateAnchor(anchors, parameter); + double displacement = Distance(evaluated, reference); + if (!NumericGuard.IsFinite(displacement) || displacement > GetAllowedRadius(reference, reserveMeters)) + { + reason = "B 样条评估点超过对应原始参考点的允许移动范围。"; + return false; + } bool endpoint = parameter == 0d || parameter == 1d; output.Add(new SmoothingPoint2D( evaluated.X, @@ -184,6 +223,38 @@ internal sealed class CubicBSplineSmoother : IPathSmoother reference.BodyClearance, endpoint && reference.IsGearSwitchPoint, endpoint ? reference.Source : SmoothedPathPointSource.Interpolated)); + return true; + } + + private static bool TryConstrainTangentHandle( + Point2D endpoint, + double rayDirectionX, + double rayDirectionY, + double desiredLength, + SmoothingPoint2D adjacentAnchor, + double allowedRadius, + out Point2D control) + { + control = default; + double offsetX = adjacentAnchor.X - endpoint.X; + double offsetY = adjacentAnchor.Y - endpoint.Y; + double projectedLength = offsetX * rayDirectionX + offsetY * rayDirectionY; + double perpendicularX = offsetX - projectedLength * rayDirectionX; + double perpendicularY = offsetY - projectedLength * rayDirectionY; + double discriminant = allowedRadius * allowedRadius - + (perpendicularX * perpendicularX + perpendicularY * perpendicularY); + if (!NumericGuard.IsFinite(discriminant) || discriminant < 0d) return false; + + double halfInterval = Math.Sqrt(discriminant); + double minimumLength = Math.Max(MinimumTangentHandleLengthMeters, projectedLength - halfInterval); + double maximumLength = projectedLength + halfInterval; + if (!NumericGuard.IsFinite(maximumLength) || maximumLength < minimumLength) return false; + + double constrainedLength = Math.Max(minimumLength, Math.Min(desiredLength, maximumLength)); + control = new Point2D( + endpoint.X + constrainedLength * rayDirectionX, + endpoint.Y + constrainedLength * rayDirectionY); + return NumericGuard.IsFinite(control.X) && NumericGuard.IsFinite(control.Y); } private static Point2D Evaluate(Point2D[] controls, double[] knots, double parameter) @@ -303,6 +374,13 @@ internal sealed class CubicBSplineSmoother : IPathSmoother return Math.Sqrt(deltaX * deltaX + deltaY * deltaY); } + private static double Distance(Point2D left, SmoothingPoint2D right) + { + double deltaX = right.X - left.X; + double deltaY = right.Y - left.Y; + return Math.Sqrt(deltaX * deltaX + deltaY * deltaY); + } + private readonly struct Point2D { internal Point2D(double x, double y) diff --git a/ClumsyPilot/tests/verify_path_smoothing_bspline.ps1 b/ClumsyPilot/tests/verify_path_smoothing_bspline.ps1 index ee5395d..1dd75a2 100644 --- a/ClumsyPilot/tests/verify_path_smoothing_bspline.ps1 +++ b/ClumsyPilot/tests/verify_path_smoothing_bspline.ps1 @@ -77,9 +77,13 @@ function New-AlgorithmInput([object[]]$Segments, [double]$ReserveMeters) { return $inputConstructor.Invoke(@($preparedPath, (New-EmptyMap), $vehicle, [double]0.05, $ReserveMeters)) } -function Invoke-Smoothing([object[]]$Segments, [double]$ReserveMeters, [double]$Strength = 1.0) { - $candidate = $smoothMethod.Invoke($smoother, @( +function Invoke-Candidate([object[]]$Segments, [double]$ReserveMeters, [double]$Strength = 1.0) { + return $smoothMethod.Invoke($smoother, @( (New-AlgorithmInput $Segments $ReserveMeters), $Strength, [Threading.CancellationToken]::None)) +} + +function Invoke-Smoothing([object[]]$Segments, [double]$ReserveMeters, [double]$Strength = 1.0) { + $candidate = Invoke-Candidate $Segments $ReserveMeters $Strength Assert-True (Get-PropertyValue $candidate 'Succeeded') 'B-spline smoothing must produce a candidate for the deterministic fixture.' return @(Get-PropertyValue $candidate 'Segments') } @@ -198,6 +202,26 @@ foreach ($point in $cornerPoints) { Assert-True ((Get-DistanceToPolyline $point $cornerSource) -le ($allowedRadius + 0.000000001)) 'Every B-spline displacement must stay inside the per-anchor clearance reserve radius.' } +# A tight reserve may not publish an evaluated B-spline that leaves its 0.005 m movement radius. +$tightReserveCandidate = Invoke-Candidate @((New-DirectionSegment 0 $forward $cornerSource)) 0.075 + +# A tight endpoint circle that cannot meet the heading=0.2 rad tangent ray must fail, not silently rotate the handle. +$misalignedHeadingSource = @( + (New-Point 0.0 0.0 0.0 0.2 0.08), + (New-Point 0.05 0.0 0.05 0.0 0.08), + (New-Point 0.10 0.0 0.10 0.0 0.08), + (New-Point 0.10 0.05 0.15 ([Math]::PI / 2.0) 0.08), + (New-Point 0.10 0.10 0.20 ([Math]::PI / 2.0) 0.08)) +$misalignedHeadingCandidate = Invoke-Candidate @((New-DirectionSegment 0 $forward $misalignedHeadingSource)) 0.075 +$requiredFailures = New-Object System.Collections.Generic.List[string] +if (Get-PropertyValue $tightReserveCandidate 'Succeeded') { + [void]$requiredFailures.Add('A tight reserve published an evaluated candidate outside its permitted movement radius.') +} +if (Get-PropertyValue $misalignedHeadingCandidate 'Succeeded') { + [void]$requiredFailures.Add('A tight reserve silently accepted an endpoint handle that cannot follow the supplied travel tangent.') +} +Assert-Equal 0 $requiredFailures.Count ([string]::Join(' ', $requiredFailures)) + # Adjacent direction segments retain their duplicated switch pose and independent topology; no fit may cross the switch. $reverseSource = @( (New-Point 0.10 0.10 0.0 ([Math]::PI / 2.0) 0.08 $true),