fix: align smoothing feasibility and option flow
This commit is contained in:
@@ -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; }
|
||||
}
|
||||
|
||||
+31
-35
@@ -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;
|
||||
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 = reason ?? string.Empty;
|
||||
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.'
|
||||
|
||||
Reference in New Issue
Block a user