fix: align smoothing feasibility and option flow

This commit is contained in:
梁薄云
2026-07-29 12:39:46 +08:00
parent fc9aff4d84
commit d670a9c821
9 changed files with 466 additions and 102 deletions
@@ -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;
@@ -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; }
}
@@ -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(
@@ -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)
@@ -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);
}
}
@@ -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;
}
}
@@ -0,0 +1,121 @@
param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll'))
$ErrorActionPreference = 'Stop'
$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath))
function Assert-True($Actual, [string]$Message) {
if (-not $Actual) { throw $Message }
}
function Assert-Near([double]$Expected, [double]$Actual, [string]$Message) {
if ([Math]::Abs($Expected - $Actual) -gt 0.000000001) {
throw "$Message Expected=$Expected Actual=$Actual"
}
}
function Assert-Throws([scriptblock]$Action, [string]$Message) {
try {
& $Action
}
catch {
return
}
throw $Message
}
function Get-RequiredType([string]$Name) {
return $assembly.GetType($Name, $true)
}
function Get-RequiredProperty($Type, [string]$Name) {
$property = $Type.GetProperty($Name, [Reflection.BindingFlags]'Instance,Public,NonPublic')
Assert-True ($null -ne $property) ("Missing property: " + $Name)
Assert-True (-not $property.CanWrite) ("Snapshot property must be get-only: " + $Name)
return $property
}
$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
$algorithms = $root + 'Algorithms.'
$coarsePath = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'
$mapping = 'MultiWheelC.TrajectoryPlanning.Mapping.'
$snapshotType = Get-RequiredType ($algorithms + 'SmoothingOptionsSnapshot')
$inputType = Get-RequiredType ($algorithms + 'SmoothingAlgorithmInput')
$configurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration')
$requestType = Get-RequiredType ($root + 'PathSmoothingRequest')
$preparedPathType = Get-RequiredType ($root + 'Processing.PreparedPath')
$mapType = Get-RequiredType ($mapping + 'PlanningGridMap')
$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
$coarsePointType = Get-RequiredType ($coarsePath + 'CoarsePathPoint')
$pathSegmentType = Get-RequiredType ($coarsePath + 'PathSegment')
$snapshotConstructor = $snapshotType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null,
@($configurationType), $null)
Assert-True ($null -ne $snapshotConstructor) 'SmoothingOptionsSnapshot must be created from PathSmoothingConfiguration.'
$inputConstructor = $inputType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null,
@($preparedPathType, $mapType, $vehicleType, [double], [double], $snapshotType), $null)
Assert-True ($null -ne $inputConstructor) 'SmoothingAlgorithmInput must accept the immutable smoothing-options snapshot at its construction boundary.'
$optionNames = @(
'CubicBSplineEndpointTangentScale',
'BezierCornerHeadingThresholdRadians',
'BezierMaximumWindowLengthMeters',
'BezierHandleLengthRatio',
'QuinticKnotSpacingMeters',
'QuinticMinimumKnotSpacingMeters')
$optionProperties = @{}
foreach ($optionName in $optionNames) {
$optionProperties[$optionName] = Get-RequiredProperty $snapshotType $optionName
}
function New-Configuration {
return [Activator]::CreateInstance($configurationType)
}
function New-Snapshot($Configuration) {
return $snapshotConstructor.Invoke(@($Configuration))
}
# Request creation takes a configuration copy. Later mutations to either the source configuration
# or a configuration copy returned by the request must not alter the algorithm snapshot.
$configuration = New-Configuration
$configuration.CubicBSpline.EndpointTangentScale = [double]0.20
$configuration.LocalCubicBezier.CornerHeadingThresholdRadians = [double]0.40
$configuration.LocalCubicBezier.MaximumWindowLengthMeters = [double]0.80
$configuration.LocalCubicBezier.HandleLengthRatio = [double]0.25
$configuration.PiecewiseQuintic.KnotSpacingMeters = [double]0.60
$configuration.PiecewiseQuintic.MinimumKnotSpacingMeters = [double]0.15
$emptyCoarsePath = [Array]::CreateInstance($coarsePointType, 0)
$emptySegments = [Array]::CreateInstance($pathSegmentType, 0)
$request = [Activator]::CreateInstance($requestType, @($emptyCoarsePath, $emptySegments, $null, $null, $configuration))
$configuration.CubicBSpline.EndpointTangentScale = [double]0.90
$requestConfiguration = $request.Configuration
Assert-Near 0.20 $requestConfiguration.CubicBSpline.EndpointTangentScale 'Request configuration must remain independent from source-config mutations.'
$snapshot = New-Snapshot $requestConfiguration
$requestConfiguration.CubicBSpline.EndpointTangentScale = [double]0.70
Assert-Near 0.20 $optionProperties['CubicBSplineEndpointTangentScale'].GetValue($snapshot) 'Algorithm options must remain independent from request-configuration mutations.'
Assert-Near 0.40 $optionProperties['BezierCornerHeadingThresholdRadians'].GetValue($snapshot) 'Snapshot must preserve the request configuration values.'
Assert-Near 0.80 $optionProperties['BezierMaximumWindowLengthMeters'].GetValue($snapshot) 'Snapshot must preserve the request configuration values.'
Assert-Near 0.25 $optionProperties['BezierHandleLengthRatio'].GetValue($snapshot) 'Snapshot must preserve the request configuration values.'
Assert-Near 0.60 $optionProperties['QuinticKnotSpacingMeters'].GetValue($snapshot) 'Snapshot must preserve the request configuration values.'
Assert-Near 0.15 $optionProperties['QuinticMinimumKnotSpacingMeters'].GetValue($snapshot) 'Snapshot must preserve the request configuration values.'
function Assert-InvalidSnapshot([scriptblock]$Mutate, [string]$Message) {
$invalidConfiguration = New-Configuration
& $Mutate $invalidConfiguration
Assert-Throws { New-Snapshot $invalidConfiguration } $Message
}
Assert-InvalidSnapshot { param($c) $c.CubicBSpline.EndpointTangentScale = [double]::NaN } 'Non-finite B-spline tangent scale must be rejected before retry.'
Assert-InvalidSnapshot { param($c) $c.CubicBSpline.EndpointTangentScale = [double]0.0 } 'Non-positive B-spline tangent scale must be rejected before retry.'
Assert-InvalidSnapshot { param($c) $c.LocalCubicBezier.CornerHeadingThresholdRadians = [double]0.0 } 'A zero Bézier heading threshold must be rejected before retry.'
Assert-InvalidSnapshot { param($c) $c.LocalCubicBezier.CornerHeadingThresholdRadians = [Math]::PI + 0.0001 } 'A Bézier heading threshold above pi must be rejected before retry.'
Assert-InvalidSnapshot { param($c) $c.LocalCubicBezier.MaximumWindowLengthMeters = [double]::PositiveInfinity } 'A non-finite Bézier window length must be rejected before retry.'
Assert-InvalidSnapshot { param($c) $c.LocalCubicBezier.HandleLengthRatio = [double]0.0 } 'A non-positive Bézier handle ratio must be rejected before retry.'
Assert-InvalidSnapshot { param($c) $c.PiecewiseQuintic.KnotSpacingMeters = [double]0.0 } 'A non-positive quintic knot spacing must be rejected before retry.'
Assert-InvalidSnapshot { param($c) $c.PiecewiseQuintic.MinimumKnotSpacingMeters = [double]::NaN } 'A non-finite quintic minimum knot spacing must be rejected before retry.'
Assert-InvalidSnapshot { param($c) $c.PiecewiseQuintic.KnotSpacingMeters = [double]0.05; $c.PiecewiseQuintic.MinimumKnotSpacingMeters = [double]0.10 } 'Quintic knot spacing below the configured minimum must be rejected before retry.'
Write-Output 'Path smoothing algorithm-input checks passed.'
@@ -62,7 +62,10 @@ function New-EmptyMap {
return $map
}
function New-AlgorithmInput([object[]]$Segments, [double]$ReserveMeters) {
function New-AlgorithmInput(
[object[]]$Segments,
[double]$ReserveMeters,
[double]$EndpointTangentScale = (1.0 / 3.0)) {
$typedSegments = [Array]::CreateInstance($segmentType, $Segments.Count)
for ($index = 0; $index -lt $Segments.Count; $index++) {
$typedSegments.SetValue($Segments[$index], $index)
@@ -74,16 +77,27 @@ function New-AlgorithmInput([object[]]$Segments, [double]$ReserveMeters) {
$vehicle.SafetyMarginMeters = [double]0.0
$vehicle.MaximumCurvaturePerMeter = [double]100.0
$vehicle.MinimumTurningRadiusMeters = [double]0.01
return $inputConstructor.Invoke(@($preparedPath, (New-EmptyMap), $vehicle, [double]0.05, $ReserveMeters))
$configuration = [Activator]::CreateInstance($configurationType)
$configuration.CubicBSpline.EndpointTangentScale = $EndpointTangentScale
$options = $optionsConstructor.Invoke(@($configuration))
return $inputConstructor.Invoke(@($preparedPath, (New-EmptyMap), $vehicle, [double]0.05, $ReserveMeters, $options))
}
function Invoke-Candidate([object[]]$Segments, [double]$ReserveMeters, [double]$Strength = 1.0) {
function Invoke-Candidate(
[object[]]$Segments,
[double]$ReserveMeters,
[double]$Strength = 1.0,
[double]$EndpointTangentScale = (1.0 / 3.0)) {
return $smoothMethod.Invoke($smoother, @(
(New-AlgorithmInput $Segments $ReserveMeters), $Strength, [Threading.CancellationToken]::None))
(New-AlgorithmInput $Segments $ReserveMeters $EndpointTangentScale), $Strength, [Threading.CancellationToken]::None))
}
function Invoke-Smoothing([object[]]$Segments, [double]$ReserveMeters, [double]$Strength = 1.0) {
$candidate = Invoke-Candidate $Segments $ReserveMeters $Strength
function Invoke-Smoothing(
[object[]]$Segments,
[double]$ReserveMeters,
[double]$Strength = 1.0,
[double]$EndpointTangentScale = (1.0 / 3.0)) {
$candidate = Invoke-Candidate $Segments $ReserveMeters $Strength $EndpointTangentScale
Assert-True (Get-PropertyValue $candidate 'Succeeded') 'B-spline smoothing must produce a candidate for the deterministic fixture.'
return @(Get-PropertyValue $candidate 'Segments')
}
@@ -137,6 +151,9 @@ $pointType = Get-RequiredType ($processing + 'SmoothingPoint2D')
$segmentType = Get-RequiredType ($processing + 'PreparedDirectionSegment')
$preparedPathType = Get-RequiredType ($processing + 'PreparedPath')
$inputType = Get-RequiredType ($algorithms + 'SmoothingAlgorithmInput')
$optionsType = Get-RequiredType ($algorithms + 'SmoothingOptionsSnapshot')
$interpolatorType = Get-RequiredType ($processing + 'PathReferenceInterpolator')
$configurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration')
$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
$directionType = Get-RequiredType ($coarsePath + 'TravelDirection')
$sourceType = Get-RequiredType ($root + 'SmoothedPathPointSource')
@@ -146,8 +163,12 @@ $mapRequestType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.Plann
$mapFactoryType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapFactory'
$inputConstructor = $inputType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null,
@($preparedPathType, $mapType, $vehicleType, [double], [double]), $null)
Assert-True ($null -ne $inputConstructor) 'Algorithm input must carry the minimum clearance reserve for per-anchor movement limits.'
@($preparedPathType, $mapType, $vehicleType, [double], [double], $optionsType), $null)
Assert-True ($null -ne $inputConstructor) 'Algorithm input must carry immutable smoothing options and the minimum clearance reserve for per-anchor movement limits.'
$optionsConstructor = $optionsType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null, @($configurationType), $null)
Assert-True ($null -ne $optionsConstructor) 'B-spline tests must create an immutable options snapshot.'
$interpolateMethod = $interpolatorType.GetMethod('TryInterpolateByArcLength', [Reflection.BindingFlags]'Static,Public,NonPublic')
Assert-True ($null -ne $interpolateMethod) 'PathReferenceInterpolator must expose arc-length interpolation.'
$smoother = [Activator]::CreateInstance($smootherType, $true)
$smoothMethod = $smootherType.GetMethod('Smooth', [Reflection.BindingFlags]'Instance,Public')
Assert-True ($null -ne $smoothMethod) 'CubicBSplineSmoother must implement the internal smoother contract.'
@@ -157,6 +178,20 @@ $forward = [Enum]::Parse($directionType, 'Forward')
$reverse = [Enum]::Parse($directionType, 'Reverse')
$anchor = [Enum]::Parse($sourceType, 'Anchor')
# Arc-length interpolation must bracket by local arc rather than by sample index.
$nonUniformPoints = [Array]::CreateInstance($pointType, 4)
$nonUniformPoints.SetValue((New-Point 0.0 0.0 0.000 0.0 1.0), 0)
$nonUniformPoints.SetValue((New-Point 5.0 0.0 0.050 0.1 0.9), 1)
$nonUniformPoints.SetValue((New-Point 10.0 0.0 0.100 0.2 0.8), 2)
$nonUniformPoints.SetValue((New-Point 20.0 0.0 0.125 0.3 0.7), 3)
$interpolateArguments = [object[]]@($nonUniformPoints, [double]0.1125, $null, $null)
Assert-True $interpolateMethod.Invoke($null, $interpolateArguments) 'Arc-length interpolation must accept a target within the final non-uniform interval.'
$arcReference = $interpolateArguments[2]
Assert-Near 15.0 $arcReference.X 0.000000001 'Target arc length 0.1125 must lie halfway through the final 0.100-0.125 interval, independent of point count.'
Assert-Near 0.1125 $arcReference.ArcLength 0.000000001 'Arc-length interpolation must preserve the requested target arc length.'
Assert-Near 0.25 $arcReference.Heading 0.000000001 'Arc-length interpolation must linearly interpolate heading.'
Assert-Near 0.75 $arcReference.BodyClearance 0.000000001 'Arc-length interpolation must linearly interpolate clearance.'
# Straight samples are returned exactly, so a straight is never distorted or densified.
$straightSource = @(
(New-Point 0.0 0.0 0.0 0.0 0.03),
@@ -196,6 +231,11 @@ for ($index = 2; $index -lt $cornerPoints.Count; $index++) {
Assert-True ((Get-AngleDifference $previousAngle $currentAngle) -lt 0.08) 'B-spline corner samples must turn without a tangent discontinuity.'
}
# Endpoint tangent scale is an immutable option and must influence the clamped B-spline endpoint handle.
$shortHandlePoints = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02 1.0 0.20)[0].Points
$longHandlePoints = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02 1.0 0.60)[0].Points
Assert-True (($longHandlePoints[2].X - $shortHandlePoints[2].X) -gt 0.0001) 'A custom endpoint tangent scale must change the B-spline start handle and early curve samples.'
# Every evaluated point may deviate only by BodyClearance - reserve, never by raw BodyClearance.
$allowedRadius = 0.06
foreach ($point in $cornerPoints) {
@@ -36,7 +36,15 @@ $root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
$algorithms = $root + 'Algorithms.'
$runnerType = Get-RequiredType ($algorithms + 'SmoothingAlgorithmRunner')
$smootherType = Get-RequiredType ($algorithms + 'IPathSmoother')
$candidateType = Get-RequiredType ($algorithms + 'SmoothingCandidate')
$candidateStatusType = Get-RequiredType ($algorithms + 'SmoothingCandidateStatus')
Assert-False $smootherType.IsPublic 'IPathSmoother must remain internal to the algorithm assembly.'
Assert-Equal 3 ([Enum]::GetNames($candidateStatusType).Length) 'Smoothing candidate status must contain only the three defined feasibility states.'
Assert-Equal 'Success' ([Enum]::GetNames($candidateStatusType)[0]) 'Candidate status must expose Success.'
Assert-Equal 'RetryableInfeasible' ([Enum]::GetNames($candidateStatusType)[1]) 'Candidate status must expose RetryableInfeasible.'
Assert-Equal 'Failed' ([Enum]::GetNames($candidateStatusType)[2]) 'Candidate status must expose Failed.'
$retryableFactory = $candidateType.GetMethod('RetryableInfeasible', [Reflection.BindingFlags]'Static,NonPublic')
Assert-True ($null -ne $retryableFactory) 'SmoothingCandidate must create retryable infeasibility without executable geometry.'
$hooksType = $runnerType.GetNestedType('TestHooks', [Reflection.BindingFlags]'Public,NonPublic')
Assert-True ($null -ne $hooksType) 'SmoothingAlgorithmRunner must expose its narrowly scoped nested TestHooks helper.'
@@ -47,12 +55,12 @@ function Invoke-Scenario([string]$Scenario) {
return $executeMethod.Invoke($null, @($Scenario))
}
$allRejected = Invoke-Scenario 'RejectAll'
Assert-Equal 'Infeasible' $allRejected.Status 'All rejected finite candidates must produce an Infeasible runner result.'
Assert-AttemptedStrengths $allRejected @(1.00, 0.75, 0.50, 0.25) 'Rejected candidates must use the finite retry schedule exactly.'
Assert-Equal 0 $allRejected.AcceptedPathPointCount 'An infeasible runner result must not retain a rejected candidate as an accepted path.'
Assert-True ($allRejected.RejectedComparisonCandidatePointCount -gt 0) 'Only comparison diagnostics may retain the last rejected candidate geometry.'
Assert-Equal 4 $allRejected.FailureCount 'Every rejected validation attempt must retain its failure reason.'
$allRetryable = Invoke-Scenario 'RetryableInfeasible'
Assert-Equal 'Infeasible' $allRetryable.Status 'Exhausted retryable infeasibility must produce an Infeasible runner result.'
Assert-AttemptedStrengths $allRetryable @(1.00, 0.75, 0.50, 0.25) 'Retryable infeasibility must use the finite retry schedule exactly.'
Assert-Equal 0 $allRetryable.AcceptedPathPointCount 'An infeasible runner result must not retain a retryable candidate as an accepted path.'
Assert-Equal 0 $allRetryable.RejectedComparisonCandidatePointCount 'Retryable infeasibility must not retain executable candidate geometry.'
Assert-Equal 4 $allRetryable.FailureCount 'Every retryable attempt must retain its failure reason.'
$accepted = Invoke-Scenario 'AcceptFirst'
Assert-Equal 'Success' $accepted.Status 'The first safe candidate must be accepted.'
@@ -60,10 +68,10 @@ Assert-AttemptedStrengths $accepted @(1.00) 'The runner must stop immediately af
Assert-True ($accepted.AcceptedPathPointCount -gt 0) 'A successful runner result must publish the validated path internally.'
Assert-Equal 0 $accepted.RejectedComparisonCandidatePointCount 'An accepted candidate must not create rejected comparison geometry.'
$numericalFailure = Invoke-Scenario 'NumericalFailure'
Assert-Equal 'Failed' $numericalFailure.Status 'A numerical candidate failure must stop the runner as Failed.'
Assert-AttemptedStrengths $numericalFailure @(1.00) 'Numerical candidate failure must not retry at lower strength.'
Assert-Equal 0 $numericalFailure.RejectedComparisonCandidatePointCount 'A non-candidate numerical failure must not retain comparison geometry.'
$terminalFailure = Invoke-Scenario 'TerminalFailed'
Assert-Equal 'Failed' $terminalFailure.Status 'A terminal candidate failure must stop the runner as Failed.'
Assert-AttemptedStrengths $terminalFailure @(1.00) 'Terminal candidate failure must run exactly once.'
Assert-Equal 0 $terminalFailure.RejectedComparisonCandidatePointCount 'A terminal failure must not retain comparison geometry.'
$cancelled = Invoke-Scenario 'CancelBeforeNextAttempt'
Assert-True $cancelled.CancellationPropagated 'Cancellation between attempts must propagate out of the runner.'