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);
-            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<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;
-        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)