fix: stabilize quintic decimal knot spacing

ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/PiecewiseQuinticSmoother.cs

@@ -11,6 +11,8 @@ namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
 internal sealed class PiecewiseQuinticSmoother : IPathSmoother
 {
     private const int SamplesPerInterval = 8;
+    private const double DoubleMachineEpsilon = 2.2204460492503131e-16d;
+    private const double EndpointNormalizationUlps = 32d;
 
     /// <inheritdoc />
     public SmoothingMethod Method => SmoothingMethod.PiecewiseQuintic;
@@ -237,15 +239,26 @@ internal sealed class PiecewiseQuinticSmoother : IPathSmoother
         }
         knots.Add(first);
 
-        double targetArcLength = startArcLength + knotSpacingMeters;
-        while (targetArcLength < endArcLength)
+        double endpointTolerance = GetEndpointNormalizationTolerance(
+            startArcLength,
+            endArcLength,
+            knotSpacingMeters);
+        double previousArcLength = startArcLength;
+        for (long knotOrdinal = 1L; ; knotOrdinal++)
         {
             cancellationToken.ThrowIfCancellationRequested();
+            double targetArcLength = startArcLength + knotOrdinal * knotSpacingMeters;
             if (!NumericGuard.IsFinite(targetArcLength))
             {
                 reason = "五次 Hermite 内部结点弧长无效。";
                 return false;
             }
+            if (targetArcLength >= endArcLength - endpointTolerance) break;
+            if (targetArcLength <= previousArcLength)
+            {
+                reason = "五次 Hermite 内部结点无法在浮点弧长尺度上保持递增。";
+                return false;
+            }
             if (!PathReferenceInterpolator.TryInterpolateByArcLength(
                     anchors,
                     targetArcLength,
@@ -257,7 +270,7 @@ internal sealed class PiecewiseQuinticSmoother : IPathSmoother
                 return false;
             }
             knots.Add(knot);
-            targetArcLength += knotSpacingMeters;
+            previousArcLength = targetArcLength;
         }
 
         if (!TryCreateKnot(anchors[anchors.Count - 1], direction, effectiveStrength, out Knot last))
@@ -279,6 +292,17 @@ internal sealed class PiecewiseQuinticSmoother : IPathSmoother
         return true;
     }
 
+    private static double GetEndpointNormalizationTolerance(
+        double startArcLength,
+        double endArcLength,
+        double knotSpacingMeters)
+    {
+        double magnitude = Math.Max(
+            Math.Abs(startArcLength),
+            Math.Max(Math.Abs(endArcLength), Math.Abs(knotSpacingMeters)));
+        return EndpointNormalizationUlps * DoubleMachineEpsilon * magnitude;
+    }
+
     private static bool TryCreateKnot(
         SmoothingPoint2D reference,
         TravelDirection direction,