fix: align smoothing feasibility and option flow

ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/CubicBSplineSmoother.cs

@@ -13,7 +13,6 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
     private const int Degree = 3;
     private const int SamplesPerSpan = 64;
     private const double StraightToleranceMeters = 1e-9d;
-    private const double EndpointTangentScale = 1d / 3d;
     private const double EndpointProbeParameter = 1e-6d;
     private const double MinimumTangentHandleLengthMeters = 1e-10d;
 
@@ -27,6 +26,7 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
         CancellationToken cancellationToken)
     {
         if (input == null || input.OriginalPath == null ||
+            input.Options == null ||
             !NumericGuard.IsPositiveFinite(effectiveStrength) ||
             !NumericGuard.IsFinite(input.MinimumClearanceReserveMeters) ||
             input.MinimumClearanceReserveMeters < 0d)
@@ -39,10 +39,19 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
         {
             cancellationToken.ThrowIfCancellationRequested();
             PreparedDirectionSegment sourceSegment = input.OriginalPath.Segments[segmentIndex];
-            if (!TrySmoothSegment(sourceSegment, effectiveStrength, input.MinimumClearanceReserveMeters,
-                    cancellationToken, out IReadOnlyList<SmoothingPoint2D> points, out string reason))
+            if (!TrySmoothSegment(
+                    sourceSegment,
+                    effectiveStrength,
+                    input.MinimumClearanceReserveMeters,
+                    input.Options.CubicBSplineEndpointTangentScale,
+                    cancellationToken,
+                    out IReadOnlyList<SmoothingPoint2D> points,
+                    out string reason,
+                    out SmoothingCandidateStatus status))
             {
-                return SmoothingCandidate.Failed(reason);
+                return status == SmoothingCandidateStatus.RetryableInfeasible
+                    ? SmoothingCandidate.RetryableInfeasible(reason)
+                    : SmoothingCandidate.Failed(reason);
             }
 
             candidateSegments.Add(new PreparedDirectionSegment(
@@ -60,12 +69,15 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
         PreparedDirectionSegment sourceSegment,
         double strength,
         double reserveMeters,
+        double endpointTangentScale,
         CancellationToken cancellationToken,
         out IReadOnlyList<SmoothingPoint2D> result,
-        out string reason)
+        out string reason,
+        out SmoothingCandidateStatus status)
     {
         result = null;
         reason = string.Empty;
+        status = SmoothingCandidateStatus.Failed;
         if (sourceSegment == null || sourceSegment.Points == null || sourceSegment.Points.Count == 0)
         {
             reason = "B 样条方向段为空。";
@@ -89,7 +101,7 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
             return true;
         }
 
-        if (!TryCreateControls(anchors, sourceSegment.Direction, strength, reserveMeters,
+        if (!TryCreateControls(anchors, sourceSegment.Direction, strength, reserveMeters, endpointTangentScale,
                 cancellationToken, out Point2D[] controls, out reason))
         {
             return false;
@@ -99,23 +111,23 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
         var sampled = new List<SmoothingPoint2D>();
         int spanCount = controls.Length - Degree;
         int uniformIntervals = spanCount * SamplesPerSpan;
-        if (!TryAddSample(0d, anchors, controls, knots, reserveMeters, sampled, cancellationToken, out reason))
+        if (!TryAddSample(0d, anchors, controls, knots, reserveMeters, sampled, cancellationToken, out reason, out status))
             return false;
         if (!TryAddSample(EndpointProbeParameter, anchors, controls, knots, reserveMeters,
-                sampled, cancellationToken, out reason))
+                sampled, cancellationToken, out reason, out status))
             return false;
         for (int index = 1; index < uniformIntervals; index++)
         {
             if (!TryAddSample((double)index / uniformIntervals, anchors, controls, knots, reserveMeters,
-                    sampled, cancellationToken, out reason))
+                    sampled, cancellationToken, out reason, out status))
             {
                 return false;
             }
         }
         if (!TryAddSample(1d - EndpointProbeParameter, anchors, controls, knots, reserveMeters,
-                sampled, cancellationToken, out reason))
+                sampled, cancellationToken, out reason, out status))
             return false;
-        if (!TryAddSample(1d, anchors, controls, knots, reserveMeters, sampled, cancellationToken, out reason))
+        if (!TryAddSample(1d, anchors, controls, knots, reserveMeters, sampled, cancellationToken, out reason, out status))
             return false;
 
         result = sampled;
@@ -127,6 +139,7 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
         TravelDirection direction,
         double strength,
         double reserveMeters,
+        double endpointTangentScale,
         CancellationToken cancellationToken,
         out Point2D[] controls,
         out string reason)
@@ -136,7 +149,7 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
         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 startHandleLength = Distance(anchors[0], anchors[1]) * endpointTangentScale * strength;
         double startTravelHeading = GetTravelHeading(anchors[0], direction);
         if (!TryConstrainTangentHandle(
                 Point2D.FromAnchor(anchors[0]),
@@ -152,7 +165,7 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
         }
 
         int finalIndex = anchors.Count - 1;
-        double endHandleLength = Distance(anchors[finalIndex - 1], anchors[finalIndex]) * EndpointTangentScale * strength;
+        double endHandleLength = Distance(anchors[finalIndex - 1], anchors[finalIndex]) * endpointTangentScale * strength;
         double endTravelHeading = GetTravelHeading(anchors[finalIndex], direction);
         if (!TryConstrainTangentHandle(
                 Point2D.FromAnchor(anchors[finalIndex]),
@@ -201,16 +214,35 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
         double reserveMeters,
         List<SmoothingPoint2D> output,
         CancellationToken cancellationToken,
-        out string reason)
+        out string reason,
+        out SmoothingCandidateStatus status)
     {
         reason = string.Empty;
+        status = SmoothingCandidateStatus.Failed;
         cancellationToken.ThrowIfCancellationRequested();
         Point2D evaluated = Evaluate(controls, knots, parameter);
-        SmoothingPoint2D reference = InterpolateAnchor(anchors, parameter);
+        if (!NumericGuard.IsFinite(evaluated.X) || !NumericGuard.IsFinite(evaluated.Y))
+        {
+            reason = "B 样条评估产生非法数值。";
+            return false;
+        }
+
+        double targetArcLength = parameter * anchors[anchors.Count - 1].ArcLength;
+        if (!PathReferenceInterpolator.TryInterpolateByArcLength(anchors, targetArcLength,
+                out SmoothingPoint2D reference, out reason))
+        {
+            return false;
+        }
         double displacement = Distance(evaluated, reference);
-        if (!NumericGuard.IsFinite(displacement) || displacement > GetAllowedRadius(reference, reserveMeters))
+        if (!NumericGuard.IsFinite(displacement))
+        {
+            reason = "B 样条评估点位移产生非法数值。";
+            return false;
+        }
+        if (displacement > GetAllowedRadius(reference, reserveMeters))
         {
             reason = "B 样条评估点超过对应原始参考点的允许移动范围。";
+            status = SmoothingCandidateStatus.RetryableInfeasible;
             return false;
         }
         bool endpoint = parameter == 0d || parameter == 1d;
@@ -298,27 +330,6 @@ internal sealed class CubicBSplineSmoother : IPathSmoother
         return knots;
     }
 
-    private static SmoothingPoint2D InterpolateAnchor(IReadOnlyList<SmoothingPoint2D> anchors, double parameter)
-    {
-        if (parameter <= 0d) return anchors[0];
-        if (parameter >= 1d) return anchors[anchors.Count - 1];
-
-        double scaled = parameter * (anchors.Count - 1);
-        int leftIndex = (int)Math.Floor(scaled);
-        double ratio = scaled - leftIndex;
-        SmoothingPoint2D left = anchors[leftIndex];
-        SmoothingPoint2D right = anchors[leftIndex + 1];
-        return new SmoothingPoint2D(
-            left.X + ratio * (right.X - left.X),
-            left.Y + ratio * (right.Y - left.Y),
-            left.ArcLength + ratio * (right.ArcLength - left.ArcLength),
-            left.Heading + ratio * (right.Heading - left.Heading),
-            left.UnwrappedHeading + ratio * (right.UnwrappedHeading - left.UnwrappedHeading),
-            left.BodyClearance + ratio * (right.BodyClearance - left.BodyClearance),
-            false,
-            SmoothedPathPointSource.Interpolated);
-    }
-
     private static Point2D ClampDisplacement(SmoothingPoint2D anchor, Point2D proposed, double allowedRadius)
     {
         double deltaX = proposed.X - anchor.X;

ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/SmoothingAlgorithmInput.cs

@@ -13,13 +13,15 @@ internal sealed class SmoothingAlgorithmInput
         PlanningGridMap map,
         VehicleParameters vehicle,
         double maximumCollisionCheckStepMeters,
-        double minimumClearanceReserveMeters)
+        double minimumClearanceReserveMeters,
+        SmoothingOptionsSnapshot options)
     {
         OriginalPath = originalPath ?? throw new ArgumentNullException(nameof(originalPath));
         Map = map ?? throw new ArgumentNullException(nameof(map));
         Vehicle = vehicle ?? throw new ArgumentNullException(nameof(vehicle));
         MaximumCollisionCheckStepMeters = maximumCollisionCheckStepMeters;
         MinimumClearanceReserveMeters = minimumClearanceReserveMeters;
+        Options = options ?? throw new ArgumentNullException(nameof(options));
     }
 
     /// <summary>已校验并按方向分段的原始路径。</summary>
@@ -36,4 +38,7 @@ internal sealed class SmoothingAlgorithmInput
 
     /// <summary>候选几何必须从原始保守净空中预留的最小安全余量，单位 m。</summary>
     internal double MinimumClearanceReserveMeters { get; }
+
+    /// <summary>本次算法运行使用的已验证方法选项快照。</summary>
+    internal SmoothingOptionsSnapshot Options { get; }
 }

ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/SmoothingAlgorithmRunner.cs

@@ -12,6 +12,7 @@ namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
 /// <summary>在有限强度计划内运行单一算法，并以共享分析和安全复核决定是否接受候选。</summary>
 internal sealed class SmoothingAlgorithmRunner
 {
+    private static readonly double[] RetryStrengthScales = { 1d, 0.75d, 0.50d, 0.25d };
     private readonly PathGeometryAnalyzer _analyzer;
     private readonly SmoothedPathValidator _validator;
 
@@ -28,7 +29,7 @@ internal sealed class SmoothingAlgorithmRunner
 
     /// <summary>
     /// 依次尝试配置的有限强度比例。取消直接向上传播，由门面转换为最终状态；
-    /// 数值失败和几何分析失败均不重试，以避免以较低强度掩盖算法退化。
+    /// 只有算法明确标记为可重试的不可行性才会使用较低强度；终止失败和统一复核失败均不重试。
     /// </summary>
     internal AlgorithmRunResult Run(
         IPathSmoother smoother,
@@ -38,13 +39,10 @@ internal sealed class SmoothingAlgorithmRunner
     {
         var attemptedStrengths = new List<double>();
         var failureReasons = new List<string>();
-        if (smoother == null || input == null || configuration == null)
+        if (smoother == null || input == null || input.Options == null || configuration == null)
             return AlgorithmRunResult.Failed("平滑算法、输入或配置无效。", attemptedStrengths, failureReasons);
-        if (configuration.RetryStrengthScales == null || configuration.RetryStrengthScales.Count == 0)
-            return AlgorithmRunResult.Failed("平滑强度重试计划为空。", attemptedStrengths, failureReasons);
 
-        SmoothingCandidate rejectedCandidate = null;
-        foreach (double scale in configuration.RetryStrengthScales)
+        foreach (double scale in RetryStrengthScales)
         {
             cancellationToken.ThrowIfCancellationRequested();
             double effectiveStrength = configuration.SmoothingStrength * scale;
@@ -55,11 +53,22 @@ internal sealed class SmoothingAlgorithmRunner
             SmoothingCandidate candidate = smoother.Smooth(input, effectiveStrength, cancellationToken);
             if (candidate == null)
                 return AlgorithmRunResult.Failed("平滑算法未返回候选。", attemptedStrengths, failureReasons);
-            if (!candidate.Succeeded)
+            if (candidate.Status == SmoothingCandidateStatus.RetryableInfeasible)
+            {
+                failureReasons.Add(candidate.Reason);
+                continue;
+            }
+            if (candidate.Status == SmoothingCandidateStatus.Failed)
             {
                 failureReasons.Add(candidate.Reason);
                 return AlgorithmRunResult.Failed(candidate.Reason, attemptedStrengths, failureReasons);
             }
+            if (candidate.Status != SmoothingCandidateStatus.Success)
+            {
+                const string unknownStatusReason = "平滑算法返回未知候选状态。";
+                failureReasons.Add(unknownStatusReason);
+                return AlgorithmRunResult.Failed(unknownStatusReason, attemptedStrengths, failureReasons);
+            }
 
             if (!_analyzer.TryAnalyze(candidate.Segments, configuration.OutputSpacingMeters,
                 out PathGeometryAnalysis analysis, out string reason))
@@ -77,11 +86,11 @@ internal sealed class SmoothingAlgorithmRunner
                     attemptedStrengths, failureReasons);
             }
 
-            rejectedCandidate = candidate;
             failureReasons.Add(reason);
+            return AlgorithmRunResult.Failed(reason, attemptedStrengths, failureReasons);
         }
 
-        return AlgorithmRunResult.Infeasible(rejectedCandidate, attemptedStrengths, failureReasons);
+        return AlgorithmRunResult.Infeasible(null, attemptedStrengths, failureReasons);
     }
 
     private static PathQualityMetrics CreateMetrics(PathGeometryAnalysis analysis, double minimumClearanceMeters)
@@ -195,9 +204,9 @@ internal sealed class SmoothingAlgorithmRunner
 
         private enum TestScenario
         {
-            RejectAll,
+            RetryableInfeasible,
             AcceptFirst,
-            NumericalFailure,
+            TerminalFailed,
             CancelBeforeNextAttempt,
         }
 
@@ -223,23 +232,22 @@ internal sealed class SmoothingAlgorithmRunner
                 CancellationToken cancellationToken)
             {
                 AttemptedStrengths.Add(effectiveStrength);
-                if (_scenario == TestScenario.NumericalFailure)
+                if (_scenario == TestScenario.TerminalFailed)
                     return SmoothingCandidate.Failed("确定性数值退化。");
                 if (_scenario == TestScenario.AcceptFirst)
                     return CreateAcceptedCandidate();
 
-                SmoothingCandidate rejected = CreateRejectedCandidate();
                 if (_scenario == TestScenario.CancelBeforeNextAttempt)
                     _cancellationSource.Cancel();
-                return rejected;
+                return SmoothingCandidate.RetryableInfeasible("确定性可重试不可行。" );
             }
         }
 
         private static TestScenario ParseScenario(string scenario)
         {
-            if (string.Equals(scenario, nameof(TestScenario.RejectAll), StringComparison.Ordinal)) return TestScenario.RejectAll;
+            if (string.Equals(scenario, nameof(TestScenario.RetryableInfeasible), StringComparison.Ordinal)) return TestScenario.RetryableInfeasible;
             if (string.Equals(scenario, nameof(TestScenario.AcceptFirst), StringComparison.Ordinal)) return TestScenario.AcceptFirst;
-            if (string.Equals(scenario, nameof(TestScenario.NumericalFailure), StringComparison.Ordinal)) return TestScenario.NumericalFailure;
+            if (string.Equals(scenario, nameof(TestScenario.TerminalFailed), StringComparison.Ordinal)) return TestScenario.TerminalFailed;
             if (string.Equals(scenario, nameof(TestScenario.CancelBeforeNextAttempt), StringComparison.Ordinal)) return TestScenario.CancelBeforeNextAttempt;
             throw new ArgumentOutOfRangeException(nameof(scenario));
         }
@@ -277,7 +285,13 @@ internal sealed class SmoothingAlgorithmRunner
                 MaximumCurvaturePerMeter = 100d,
                 MinimumTurningRadiusMeters = 0.01d,
             };
-            return new SmoothingAlgorithmInput(new PreparedPath(originalSegments), mapResult.Map, vehicle, 0.05d, 0.02d);
+            return new SmoothingAlgorithmInput(
+                new PreparedPath(originalSegments),
+                mapResult.Map,
+                vehicle,
+                0.05d,
+                0.02d,
+                new SmoothingOptionsSnapshot(new PathSmoothingConfiguration()));
         }
 
         private static SmoothingCandidate CreateAcceptedCandidate()
@@ -297,24 +311,6 @@ internal sealed class SmoothingAlgorithmRunner
             });
         }
 
-        private static SmoothingCandidate CreateRejectedCandidate()
-        {
-            return SmoothingCandidate.Success(new List<PreparedDirectionSegment>
-            {
-                new PreparedDirectionSegment(
-                    0,
-                    TravelDirection.Forward,
-                    new List<SmoothingPoint2D>
-                    {
-                        CreatePoint(0.5d, 0.5d, 0d),
-                        CreatePoint(0.05d, 0.5d, 0.45d),
-                        CreatePoint(1.5d, 0.5d, 1.90d),
-                    },
-                    false,
-                    false),
-            });
-        }
-
         private static SmoothingPoint2D CreatePoint(double x, double y, double arcLength)
         {
             return new SmoothingPoint2D(

ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/SmoothingCandidate.cs

@@ -5,18 +5,47 @@ using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
 
 namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
 
+internal enum SmoothingCandidateStatus
+{
+    Success,
+    RetryableInfeasible,
+    Failed,
+}
+
 /// <summary>平滑方法产生的原始方向段候选，尚未经过统一几何或安全复核。</summary>
 internal sealed class SmoothingCandidate
 {
-    private SmoothingCandidate(bool succeeded, IReadOnlyList<PreparedDirectionSegment> segments, string reason)
+    private SmoothingCandidate(
+        SmoothingCandidateStatus status,
+        IReadOnlyList<PreparedDirectionSegment> segments,
+        string reason)
     {
-        Succeeded = succeeded;
-        Segments = CopyReadOnly(segments);
-        Reason = reason ?? string.Empty;
+        Status = status;
+        if (status == SmoothingCandidateStatus.Success)
+        {
+            if (segments == null || segments.Count == 0)
+                throw new ArgumentException("A successful smoothing candidate requires direction segments.", nameof(segments));
+            for (int index = 0; index < segments.Count; index++)
+            {
+                if (segments[index] == null || segments[index].Points == null || segments[index].Points.Count == 0)
+                    throw new ArgumentException("A successful smoothing candidate requires complete direction segments.", nameof(segments));
+            }
+            Segments = CopyReadOnly(segments);
+            Reason = string.Empty;
+            return;
+        }
+
+        if (string.IsNullOrWhiteSpace(reason))
+            throw new ArgumentException("An unsuccessful smoothing candidate requires a reason.", nameof(reason));
+        Segments = CopyReadOnly<PreparedDirectionSegment>(null);
+        Reason = reason;
     }
 
     /// <summary>候选是否成功产生有限的原始几何。</summary>
-    internal bool Succeeded { get; }
+    internal bool Succeeded => Status == SmoothingCandidateStatus.Success;
+
+    /// <summary>候选的可重试性和终止性状态。</summary>
+    internal SmoothingCandidateStatus Status { get; }
 
     /// <summary>候选方向段；失败候选始终为空。</summary>
     internal IReadOnlyList<PreparedDirectionSegment> Segments { get; }
@@ -27,17 +56,19 @@ internal sealed class SmoothingCandidate
     /// <summary>创建待统一分析和验证的成功候选。</summary>
     internal static SmoothingCandidate Success(IReadOnlyList<PreparedDirectionSegment> segments)
     {
-        if (segments == null || segments.Count == 0)
-            throw new ArgumentException("A successful smoothing candidate requires direction segments.", nameof(segments));
-        return new SmoothingCandidate(true, segments, string.Empty);
+        return new SmoothingCandidate(SmoothingCandidateStatus.Success, segments, string.Empty);
+    }
+
+    /// <summary>创建可由较低平滑强度重新尝试的不可行候选。</summary>
+    internal static SmoothingCandidate RetryableInfeasible(string reason)
+    {
+        return new SmoothingCandidate(SmoothingCandidateStatus.RetryableInfeasible, null, reason);
     }
 
     /// <summary>创建不应重试的数值或构造失败候选。</summary>
     internal static SmoothingCandidate Failed(string reason)
     {
-        if (string.IsNullOrWhiteSpace(reason))
-            throw new ArgumentException("A failed smoothing candidate requires a reason.", nameof(reason));
-        return new SmoothingCandidate(false, null, reason);
+        return new SmoothingCandidate(SmoothingCandidateStatus.Failed, null, reason);
     }
 
     private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)

ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/SmoothingOptionsSnapshot.cs

@@ -0,0 +1,50 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
+
+/// <summary>供单次平滑算法运行使用的、已校验的不可变方法选项快照。</summary>
+internal sealed class SmoothingOptionsSnapshot
+{
+    internal SmoothingOptionsSnapshot(PathSmoothingConfiguration configuration)
+    {
+        if (configuration == null) throw new ArgumentNullException(nameof(configuration));
+
+        CubicBSplineEndpointTangentScale = configuration.CubicBSpline.EndpointTangentScale;
+        BezierCornerHeadingThresholdRadians = configuration.LocalCubicBezier.CornerHeadingThresholdRadians;
+        BezierMaximumWindowLengthMeters = configuration.LocalCubicBezier.MaximumWindowLengthMeters;
+        BezierHandleLengthRatio = configuration.LocalCubicBezier.HandleLengthRatio;
+        QuinticKnotSpacingMeters = configuration.PiecewiseQuintic.KnotSpacingMeters;
+        QuinticMinimumKnotSpacingMeters = configuration.PiecewiseQuintic.MinimumKnotSpacingMeters;
+
+        ValidatePositiveFinite(CubicBSplineEndpointTangentScale, nameof(CubicBSplineEndpointTangentScale));
+        if (!NumericGuard.IsFinite(BezierCornerHeadingThresholdRadians) ||
+            BezierCornerHeadingThresholdRadians <= 0d || BezierCornerHeadingThresholdRadians > Math.PI)
+        {
+            throw new ArgumentOutOfRangeException(nameof(BezierCornerHeadingThresholdRadians));
+        }
+        ValidatePositiveFinite(BezierMaximumWindowLengthMeters, nameof(BezierMaximumWindowLengthMeters));
+        ValidatePositiveFinite(BezierHandleLengthRatio, nameof(BezierHandleLengthRatio));
+        ValidatePositiveFinite(QuinticKnotSpacingMeters, nameof(QuinticKnotSpacingMeters));
+        ValidatePositiveFinite(QuinticMinimumKnotSpacingMeters, nameof(QuinticMinimumKnotSpacingMeters));
+        if (QuinticKnotSpacingMeters < QuinticMinimumKnotSpacingMeters)
+            throw new ArgumentOutOfRangeException(nameof(QuinticKnotSpacingMeters));
+    }
+
+    internal double CubicBSplineEndpointTangentScale { get; }
+
+    internal double BezierCornerHeadingThresholdRadians { get; }
+
+    internal double BezierMaximumWindowLengthMeters { get; }
+
+    internal double BezierHandleLengthRatio { get; }
+
+    internal double QuinticKnotSpacingMeters { get; }
+
+    internal double QuinticMinimumKnotSpacingMeters { get; }
+
+    private static void ValidatePositiveFinite(double value, string name)
+    {
+        if (!NumericGuard.IsPositiveFinite(value)) throw new ArgumentOutOfRangeException(name);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathReferenceInterpolator.cs

@@ -0,0 +1,102 @@
+using System;
+using System.Collections.Generic;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
+
+/// <summary>按方向段局部弧长插值平滑算法的原始路径参考。</summary>
+internal static class PathReferenceInterpolator
+{
+    internal static bool TryInterpolateByArcLength(
+        IReadOnlyList<SmoothingPoint2D> points,
+        double targetArcLength,
+        out SmoothingPoint2D reference,
+        out string reason)
+    {
+        reference = null;
+        reason = string.Empty;
+        if (points == null || points.Count == 0 || !NumericGuard.IsFinite(targetArcLength))
+        {
+            reason = "原始路径参考点或目标弧长无效。";
+            return false;
+        }
+
+        for (int index = 0; index < points.Count; index++)
+        {
+            SmoothingPoint2D point = points[index];
+            if (!IsValid(point) || (index > 0 && point.ArcLength < points[index - 1].ArcLength))
+            {
+                reason = "原始路径参考点包含非法数值或非递增弧长。";
+                return false;
+            }
+        }
+
+        SmoothingPoint2D first = points[0];
+        SmoothingPoint2D last = points[points.Count - 1];
+        if (targetArcLength < first.ArcLength || targetArcLength > last.ArcLength)
+        {
+            reason = "目标弧长不在原始方向段范围内。";
+            return false;
+        }
+
+        if (targetArcLength == first.ArcLength)
+        {
+            reference = first;
+            return true;
+        }
+        if (targetArcLength == last.ArcLength)
+        {
+            reference = last;
+            return true;
+        }
+
+        for (int rightIndex = 1; rightIndex < points.Count; rightIndex++)
+        {
+            SmoothingPoint2D left = points[rightIndex - 1];
+            SmoothingPoint2D right = points[rightIndex];
+            if (targetArcLength > right.ArcLength) continue;
+            if (targetArcLength == right.ArcLength)
+            {
+                reference = right;
+                return true;
+            }
+
+            double interval = right.ArcLength - left.ArcLength;
+            if (!NumericGuard.IsPositiveFinite(interval))
+            {
+                reason = "原始路径参考点包含无法插值的重复弧长。";
+                return false;
+            }
+
+            double ratio = (targetArcLength - left.ArcLength) / interval;
+            reference = new SmoothingPoint2D(
+                left.X + ratio * (right.X - left.X),
+                left.Y + ratio * (right.Y - left.Y),
+                targetArcLength,
+                left.Heading + ratio * (right.Heading - left.Heading),
+                left.UnwrappedHeading + ratio * (right.UnwrappedHeading - left.UnwrappedHeading),
+                left.BodyClearance + ratio * (right.BodyClearance - left.BodyClearance),
+                false,
+                SmoothedPathPointSource.Interpolated);
+            if (!IsValid(reference))
+            {
+                reference = null;
+                reason = "原始路径参考插值产生非法数值。";
+                return false;
+            }
+            return true;
+        }
+
+        reason = "原始路径参考无法定位目标弧长。";
+        return false;
+    }
+
+    private static bool IsValid(SmoothingPoint2D point)
+    {
+        return point != null && NumericGuard.IsFinite(point.X) && NumericGuard.IsFinite(point.Y) &&
+            NumericGuard.IsFinite(point.ArcLength) && NumericGuard.IsFinite(point.Heading) &&
+            NumericGuard.IsFinite(point.UnwrappedHeading) && NumericGuard.IsFinite(point.BodyClearance) &&
+            point.ArcLength >= 0d && point.BodyClearance >= 0d;
+    }
+}