diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/LocalCubicBezierSmoother.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/LocalCubicBezierSmoother.cs index 6593359..1bdb8c3 100644 --- a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/LocalCubicBezierSmoother.cs +++ b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/LocalCubicBezierSmoother.cs @@ -178,6 +178,7 @@ internal sealed class LocalCubicBezierSmoother : IPathSmoother out string reason) { windows = new List(); + var candidates = new List(); reason = string.Empty; for (int cornerIndex = 1; cornerIndex < anchors.Count - 1; cornerIndex++) { @@ -210,22 +211,43 @@ internal sealed class LocalCubicBezierSmoother : IPathSmoother if (windowLength > maximumWindowLengthMeters + WindowToleranceMeters) continue; if (ContainsGearSwitch(anchors, startIndex, endIndex)) continue; - var proposed = new Window(startIndex, endIndex); - if (windows.Count == 0 || proposed.StartIndex > windows[windows.Count - 1].EndIndex + 1) - { - windows.Add(proposed); - } - else - { - Window previous = windows[windows.Count - 1]; - windows[windows.Count - 1] = new Window( - previous.StartIndex, - Math.Max(previous.EndIndex, proposed.EndIndex)); - } + candidates.Add(new Window(startIndex, endIndex)); } + + MergeBoundedConnectedWindows(anchors, candidates, maximumWindowLengthMeters, windows); return true; } + private static void MergeBoundedConnectedWindows( + IReadOnlyList anchors, + IReadOnlyList candidates, + double maximumWindowLengthMeters, + List windows) + { + int candidateIndex = 0; + while (candidateIndex < candidates.Count) + { + Window merged = candidates[candidateIndex]; + candidateIndex++; + while (candidateIndex < candidates.Count && + candidates[candidateIndex].StartIndex <= merged.EndIndex + 1) + { + merged = new Window(merged.StartIndex, + Math.Max(merged.EndIndex, candidates[candidateIndex].EndIndex)); + candidateIndex++; + } + + double mergedLength = anchors[merged.EndIndex].ArcLength - anchors[merged.StartIndex].ArcLength; + if (mergedLength <= maximumWindowLengthMeters + WindowToleranceMeters) + { + windows.Add(merged); + } + // A connected group that exceeds the cap is declined as a whole. Splitting it into + // adjacent local curves would introduce unrequested joins; accepting it would violate + // the maximum-window contract. Its original anchors therefore remain unchanged. + } + } + private static bool TryAppendWindowInterior( IReadOnlyList anchors, Window window, @@ -249,10 +271,12 @@ internal sealed class LocalCubicBezierSmoother : IPathSmoother } double arcLength = p3.ArcLength - p0.ArcLength; - double handleLength = arcLength * handleLengthRatio * strength; - if (!NumericGuard.IsPositiveFinite(arcLength) || !NumericGuard.IsPositiveFinite(handleLength)) + double chordLength = Distance(p0, p3); + double handleLength = chordLength * handleLengthRatio * strength; + if (!NumericGuard.IsPositiveFinite(arcLength) || !NumericGuard.IsPositiveFinite(chordLength) || + !NumericGuard.IsPositiveFinite(handleLength)) { - reason = "Bézier 窗口弧长或控制柄长度无效。"; + reason = "Bézier 窗口弧长、端点弦长或控制柄长度无效。"; return false; } @@ -367,6 +391,13 @@ internal sealed class LocalCubicBezierSmoother : IPathSmoother return Math.Sqrt(deltaX * deltaX + deltaY * deltaY); } + private static double Distance(SmoothingPoint2D left, SmoothingPoint2D right) + { + double deltaX = left.X - right.X; + double deltaY = left.Y - right.Y; + return Math.Sqrt(deltaX * deltaX + deltaY * deltaY); + } + private readonly struct Window { internal Window(int startIndex, int endIndex) diff --git a/ClumsyPilot/tests/verify_path_smoothing_bezier.ps1 b/ClumsyPilot/tests/verify_path_smoothing_bezier.ps1 index 24fa391..0debea4 100644 --- a/ClumsyPilot/tests/verify_path_smoothing_bezier.ps1 +++ b/ClumsyPilot/tests/verify_path_smoothing_bezier.ps1 @@ -211,6 +211,23 @@ Assert-Equal $cornerSource.Count $cornerOutput.Count 'One local replacement must Assert-True (($cornerOutput[2].X -ne $cornerSource[2].X) -or ($cornerOutput[2].Y -ne $cornerSource[2].Y)) 'The corner sample must be replaced by cubic Bézier geometry.' Assert-PointBitwiseEqual $cornerSource[1] $cornerOutput[1] 'Bézier entry anchor must remain fixed.' Assert-PointBitwiseEqual $cornerSource[3] $cornerOutput[3] 'Bézier exit anchor must remain fixed.' +$cornerChordLength = [Math]::Sqrt( + [Math]::Pow($cornerSource[3].X - $cornerSource[1].X, 2.0) + + [Math]::Pow($cornerSource[3].Y - $cornerSource[1].Y, 2.0)) +$cornerHandleLength = $cornerChordLength / 3.0 +$expectedCornerX = + 0.125 * $cornerSource[1].X + + 0.375 * ($cornerSource[1].X + $cornerHandleLength) + + 0.375 * $cornerSource[3].X + + 0.125 * $cornerSource[3].X +$expectedCornerY = + 0.125 * $cornerSource[1].Y + + 0.375 * $cornerSource[1].Y + + 0.375 * ($cornerSource[3].Y - $cornerHandleLength) + + 0.125 * $cornerSource[3].Y +$cornerInterpolated = Get-FirstInterpolatedPoint $cornerOutput +Assert-Near $expectedCornerX $cornerInterpolated.X 0.000000000001 'Bézier control handles must use the local endpoint chord length for X geometry.' +Assert-Near $expectedCornerY $cornerInterpolated.Y 0.000000000001 'Bézier control handles must use the local endpoint chord length for Y geometry.' # Adjacent corner windows touch/overlap and must become one merged cubic replacement, not two sequential fits. $overlappingSource = @( @@ -227,6 +244,15 @@ Assert-PointBitwiseEqual $overlappingSource[4] $overlappingOutput[4] 'Merged Bé Assert-True (($overlappingOutput[2].X -ne $overlappingSource[2].X) -or ($overlappingOutput[2].Y -ne $overlappingSource[2].Y)) 'Merged window must replace the first interior corner sample.' Assert-True (($overlappingOutput[3].X -ne $overlappingSource[3].X) -or ($overlappingOutput[3].Y -ne $overlappingSource[3].Y)) 'Merged window must replace the second interior corner sample.' +# Safe bounded policy: decline an entire connected set when its merged interval exceeds the cap. +# The two candidate windows below are each 0.20 m, but their merged 0.30 m interval must not +# produce one over-length curve or be split into new unrequested joins. +$overCapMergedOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $overlappingSource)) 0.02 ([Math]::PI / 18.0) 0.20)[0].Points +Assert-Equal 0 (Get-InterpolatedRunCount $overCapMergedOutput) 'An oversized connected Bézier window set must be declined instead of emitting an over-cap replacement.' +for ($index = 0; $index -lt $overlappingSource.Count; $index++) { + Assert-PointBitwiseEqual $overlappingSource[$index] $overCapMergedOutput[$index] 'Declining an oversized connected set must preserve its anchors.' +} + # Samples outside a local window must remain bitwise unchanged rather than be globally re-fit. $isolatedSource = @( (New-Point 0.0 0.0 0.0 0.0), @@ -275,4 +301,19 @@ Assert-Equal 'RetryableInfeasible' (Get-PropertyValue $infeasible 'Status').ToSt Assert-True (-not (Get-PropertyValue $infeasible 'Succeeded')) 'An infeasible Bézier curve must not be executable.' Assert-Equal 0 (Get-PropertyValue $infeasible 'Segments').Count 'A retryable Bézier infeasibility must publish no executable geometry.' +# This nonuniform, offset window evaluates its only interior point at t=0.25 and local s=6. +# A wrong global/index mapping would instead compare to s=5 and accept the 0.50 m clearance; +# the required local-arc reference at s=6 must reject the roughly 0.65 m displacement. +$nonuniformOffsetSource = @( + (New-Point 0.0 0.0 0.0 0.0 0.50), + (New-Point 1.0 0.0 4.0 0.0 0.50), + (New-Point 2.0 0.0 5.0 0.0 0.50), + (New-Point 3.0 0.0 6.0 0.0 0.50), + (New-Point 3.0 1.0 9.0 ([Math]::PI / 2.0) 0.50), + (New-Point 3.0 2.0 20.0 ([Math]::PI / 2.0) 0.50)) +$nonuniformOffsetInfeasible = Invoke-Candidate @((New-DirectionSegment 0 $forward $nonuniformOffsetSource)) 0.0 ([Math]::PI / 18.0) 5.0 +Assert-Equal 'RetryableInfeasible' (Get-PropertyValue $nonuniformOffsetInfeasible 'Status').ToString() 'A nonuniform offset window must use local arc-length mapping for retryable clearance rejection.' +Assert-True (-not (Get-PropertyValue $nonuniformOffsetInfeasible 'Succeeded')) 'The nonuniform local-arc infeasibility must not be executable.' +Assert-Equal 0 (Get-PropertyValue $nonuniformOffsetInfeasible 'Segments').Count 'The nonuniform local-arc infeasibility must publish no geometry.' + Write-Output 'Path smoothing local cubic Bézier checks passed.'