fix: enforce b-spline displacement constraints

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;
 
     /// <inheritdoc />
     public SmoothingMethod Method => SmoothingMethod.CubicBSpline;
@@ -88,11 +89,9 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
             return true;
         }
 
-        Point2D[] controls = CreateControls(anchors, sourceSegment.Direction, strength, reserveMeters,
+        if (!TryCreateControls(anchors, sourceSegment.Direction, strength, reserveMeters,
-            cancellationToken);
+                cancellationToken, out Point2D[] controls, out reason))
-        if (controls == null)
         {
-            reason = "B 样条控制点构造产生非法数值。";
             return false;
         }
 
@@ -100,45 +99,73 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
         var sampled = new List<SmoothingPoint2D>();
         int spanCount = controls.Length - Degree;
         int uniformIntervals = spanCount * SamplesPerSpan;
-        AddSample(0d, anchors, controls, knots, sampled, cancellationToken);
+        if (!TryAddSample(0d, anchors, controls, knots, reserveMeters, sampled, cancellationToken, out reason))
-        AddSample(EndpointProbeParameter, anchors, controls, knots, sampled, cancellationToken);
+            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);
+        if (!TryAddSample(1d - EndpointProbeParameter, anchors, controls, knots, reserveMeters,
-        AddSample(1d, anchors, controls, knots, sampled, cancellationToken);
+                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<SmoothingPoint2D> 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(
+        if (!TryConstrainTangentHandle(
-            anchors[0].X + startHandleLength * Math.Cos(startTravelHeading),
+                Point2D.FromAnchor(anchors[0]),
-            anchors[0].Y + startHandleLength * Math.Sin(startTravelHeading));
+                Math.Cos(startTravelHeading),
-        controls[1] = ClampDisplacement(anchors[1], startProposed, GetAllowedRadius(anchors[1], reserveMeters));
+                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(
+        if (!TryConstrainTangentHandle(
-            anchors[finalIndex].X - endHandleLength * Math.Cos(endTravelHeading),
+                Point2D.FromAnchor(anchors[finalIndex]),
-            anchors[finalIndex].Y - endHandleLength * Math.Sin(endTravelHeading));
+                -Math.Cos(endTravelHeading),
-        controls[finalIndex - 1] = ClampDisplacement(
+                -Math.Sin(endTravelHeading),
-            anchors[finalIndex - 1], endProposed, GetAllowedRadius(anchors[finalIndex - 1], reserveMeters));
+                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<SmoothingPoint2D> anchors,
         Point2D[] controls,
         double[] knots,
+        double reserveMeters,
         List<SmoothingPoint2D> 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)

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) {
+function Invoke-Candidate([object[]]$Segments, [double]$ReserveMeters, [double]$Strength = 1.0) {
-    $candidate = $smoothMethod.Invoke($smoother, @(
+    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),