fix: correct local bezier window constraints

ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/LocalCubicBezierSmoother.cs

@@ -178,6 +178,7 @@ internal sealed class LocalCubicBezierSmoother : IPathSmoother
         out string reason)
     {
         windows = new List<Window>();
+        var candidates = new List<Window>();
         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);
+            candidates.Add(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));
-            }
         }
+
+        MergeBoundedConnectedWindows(anchors, candidates, maximumWindowLengthMeters, windows);
         return true;
     }
 
+    private static void MergeBoundedConnectedWindows(
+        IReadOnlyList<SmoothingPoint2D> anchors,
+        IReadOnlyList<Window> candidates,
+        double maximumWindowLengthMeters,
+        List<Window> 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<SmoothingPoint2D> anchors,
         Window window,
@@ -249,10 +271,12 @@ internal sealed class LocalCubicBezierSmoother : IPathSmoother
         }
 
         double arcLength = p3.ArcLength - p0.ArcLength;
-        double handleLength = arcLength * handleLengthRatio * strength;
+        double chordLength = Distance(p0, p3);
-        if (!NumericGuard.IsPositiveFinite(arcLength) || !NumericGuard.IsPositiveFinite(handleLength))
+        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)

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.'