chore: save current workspace progress
This commit is contained in:
梁薄云
+231
@@ -0,0 +1,231 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Diagnostics;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;
|
||||
|
||||
/// <summary>以固定预热和五次测量隔离比较所有请求平滑方法的离线入口。</summary>
|
||||
public sealed class PathSmoothingComparisonService
|
||||
{
|
||||
private readonly PathSmoothingService _smoothingService = new PathSmoothingService();
|
||||
private readonly PathSmoothingPreprocessor _preprocessor = new PathSmoothingPreprocessor();
|
||||
private readonly PathGeometryAnalyzer _analyzer = new PathGeometryAnalyzer();
|
||||
private readonly SmoothedPathValidator _validator = new SmoothedPathValidator();
|
||||
|
||||
/// <summary>比较所有请求方法；一个方法的失败不会阻止其他方法，取消会停止后续启动。</summary>
|
||||
public PathSmoothingComparisonResult Compare(
|
||||
PathSmoothingComparisonRequest request,
|
||||
CancellationToken cancellationToken = default)
|
||||
{
|
||||
PathSmoothingComparisonEntry baseline = CreateRawPathBaseline(request, out string baselineReason);
|
||||
var entries = new List<PathSmoothingComparisonEntry>();
|
||||
|
||||
if (cancellationToken.IsCancellationRequested)
|
||||
return Cancelled(baseline, entries);
|
||||
|
||||
if (request == null || request.SmoothingRequest == null)
|
||||
return new PathSmoothingComparisonResult(baseline, entries, null, false, baselineReason);
|
||||
|
||||
for (int methodIndex = 0; methodIndex < request.Methods.Count; methodIndex++)
|
||||
{
|
||||
if (cancellationToken.IsCancellationRequested)
|
||||
return Cancelled(baseline, entries);
|
||||
|
||||
SmoothingMethod method = request.Methods[methodIndex];
|
||||
if (!TryCompareMethod(
|
||||
request.SmoothingRequest,
|
||||
method,
|
||||
baseline.Metrics,
|
||||
cancellationToken,
|
||||
out PathSmoothingComparisonEntry entry))
|
||||
return Cancelled(baseline, entries);
|
||||
entries.Add(entry);
|
||||
}
|
||||
|
||||
return new PathSmoothingComparisonResult(
|
||||
baseline,
|
||||
entries,
|
||||
SmoothingMethodRanker.Rank(entries),
|
||||
false,
|
||||
baselineReason);
|
||||
}
|
||||
|
||||
private bool TryCompareMethod(
|
||||
PathSmoothingRequest sourceRequest,
|
||||
SmoothingMethod method,
|
||||
PathQualityMetrics rawMetrics,
|
||||
CancellationToken cancellationToken,
|
||||
out PathSmoothingComparisonEntry entry)
|
||||
{
|
||||
entry = null;
|
||||
try
|
||||
{
|
||||
PathSmoothingRequest methodRequest = CreateMethodRequest(sourceRequest, method);
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
PathSmoothingResult warmup = _smoothingService.Smooth(methodRequest, cancellationToken);
|
||||
if (warmup.Status == PathSmoothingStatus.Cancelled || cancellationToken.IsCancellationRequested) return false;
|
||||
|
||||
var timings = new List<double>(5);
|
||||
var measuredResults = new List<PathSmoothingResult>(5);
|
||||
for (int sampleIndex = 0; sampleIndex < 5; sampleIndex++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
Stopwatch stopwatch = Stopwatch.StartNew();
|
||||
PathSmoothingResult result = _smoothingService.Smooth(methodRequest, cancellationToken);
|
||||
stopwatch.Stop();
|
||||
if (result.Status == PathSmoothingStatus.Cancelled || cancellationToken.IsCancellationRequested) return false;
|
||||
timings.Add(stopwatch.Elapsed.TotalMilliseconds);
|
||||
measuredResults.Add(result);
|
||||
}
|
||||
|
||||
PathSmoothingResult canonical = measuredResults[0];
|
||||
string digest = StableGeometryDigest.Compute(canonical);
|
||||
SmoothingTimingSummary timing = SmoothingTimingSummary.FromMeasurements(timings, measuredResults);
|
||||
string diagnostic = string.IsNullOrWhiteSpace(timing.Diagnostic)
|
||||
? canonical.Diagnostics.TerminationReason
|
||||
: timing.Diagnostic;
|
||||
|
||||
PathQualityMetrics metrics = canonical.Status == PathSmoothingStatus.Success
|
||||
? NormalizeMetrics(canonical.Diagnostics.Metrics, rawMetrics)
|
||||
: new PathQualityMetrics();
|
||||
entry = PathSmoothingComparisonEntry.CreateCandidate(
|
||||
method,
|
||||
canonical.Status,
|
||||
metrics,
|
||||
timing,
|
||||
digest,
|
||||
diagnostic,
|
||||
canonical.Diagnostics.RetryCount,
|
||||
canonical.Diagnostics.AcceptedStrength,
|
||||
canonical.Path,
|
||||
canonical.Segments);
|
||||
return true;
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
entry = PathSmoothingComparisonEntry.CreateCandidate(
|
||||
method,
|
||||
PathSmoothingStatus.Failed,
|
||||
new PathQualityMetrics(),
|
||||
new SmoothingTimingSummary(new double[] { 0d, 0d, 0d, 0d, 0d }, false, exception.GetType().Name),
|
||||
string.Empty,
|
||||
exception.Message,
|
||||
0,
|
||||
0d,
|
||||
null,
|
||||
null);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
private PathSmoothingComparisonEntry CreateRawPathBaseline(
|
||||
PathSmoothingComparisonRequest request,
|
||||
out string reason)
|
||||
{
|
||||
reason = string.Empty;
|
||||
if (request == null || request.SmoothingRequest == null)
|
||||
return FailedBaseline(PathSmoothingStatus.InvalidInput, "比较请求为空。", out reason);
|
||||
|
||||
PathSmoothingRequest smoothingRequest = request.SmoothingRequest;
|
||||
PathSmoothingConfiguration configuration = smoothingRequest.Configuration;
|
||||
if (configuration == null || smoothingRequest.Map == null || smoothingRequest.Vehicle == null)
|
||||
return FailedBaseline(PathSmoothingStatus.InvalidInput, "比较请求缺少可用的地图、车辆或配置。", out reason);
|
||||
|
||||
if (!_preprocessor.TryPrepare(smoothingRequest, out PreparedPath preparedPath, out reason))
|
||||
return FailedBaseline(PathSmoothingStatus.InvalidInput, reason, out reason);
|
||||
if (!RawPathBaselineBuilder.TryCreate(
|
||||
smoothingRequest,
|
||||
preparedPath,
|
||||
_analyzer,
|
||||
configuration.OutputSpacingMeters,
|
||||
_validator,
|
||||
configuration.MaximumCollisionCheckStepMeters,
|
||||
out RawPathBaseline rawPath,
|
||||
out reason))
|
||||
return FailedBaseline(PathSmoothingStatus.InvalidInput, reason, out reason);
|
||||
|
||||
string digest = StableGeometryDigest.Compute(PathSmoothingStatus.Success, null, rawPath.Path, rawPath.Segments);
|
||||
return PathSmoothingComparisonEntry.CreateRawPathBaseline(
|
||||
PathSmoothingStatus.Success,
|
||||
rawPath.Metrics,
|
||||
digest,
|
||||
string.Empty,
|
||||
rawPath.Path,
|
||||
rawPath.Segments);
|
||||
}
|
||||
|
||||
private static PathSmoothingComparisonEntry FailedBaseline(
|
||||
PathSmoothingStatus status,
|
||||
string failureReason,
|
||||
out string reason)
|
||||
{
|
||||
reason = failureReason ?? string.Empty;
|
||||
return PathSmoothingComparisonEntry.CreateRawPathBaseline(
|
||||
status,
|
||||
new PathQualityMetrics(),
|
||||
StableGeometryDigest.Compute(status, null, null, null),
|
||||
reason,
|
||||
null,
|
||||
null);
|
||||
}
|
||||
|
||||
private static PathSmoothingComparisonResult Cancelled(
|
||||
PathSmoothingComparisonEntry baseline,
|
||||
IReadOnlyList<PathSmoothingComparisonEntry> entries)
|
||||
{
|
||||
return new PathSmoothingComparisonResult(baseline, entries, null, true, "路径平滑比较已取消。");
|
||||
}
|
||||
|
||||
private static PathSmoothingRequest CreateMethodRequest(PathSmoothingRequest source, SmoothingMethod method)
|
||||
{
|
||||
PathSmoothingConfiguration configuration = source.Configuration;
|
||||
configuration.Method = method;
|
||||
configuration.AllowFallbackToCoarsePath = false;
|
||||
return new PathSmoothingRequest(
|
||||
source.CoarsePath,
|
||||
source.Segments,
|
||||
source.Map,
|
||||
source.Vehicle,
|
||||
configuration);
|
||||
}
|
||||
|
||||
private static PathQualityMetrics NormalizeMetrics(
|
||||
PathQualityMetrics candidate,
|
||||
PathQualityMetrics raw)
|
||||
{
|
||||
if (candidate == null || raw == null || !candidate.IsFeasible)
|
||||
return new PathQualityMetrics();
|
||||
|
||||
return new PathQualityMetrics(
|
||||
true,
|
||||
candidate.PathLengthMeters,
|
||||
candidate.MaximumAbsoluteVehicleCurvaturePerMeter,
|
||||
candidate.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
|
||||
candidate.RootMeanSquareVehicleCurvaturePerMeter,
|
||||
candidate.TotalAbsoluteCurvatureVariationPerMeter,
|
||||
candidate.CurvatureVariationEnergy,
|
||||
candidate.MinimumBodyClearanceMeters,
|
||||
RelativePercentOrAbsoluteDelta(candidate.PathLengthMeters, raw.PathLengthMeters),
|
||||
RelativePercentOrAbsoluteDelta(
|
||||
candidate.MaximumAbsoluteVehicleCurvaturePerMeter,
|
||||
raw.MaximumAbsoluteVehicleCurvaturePerMeter),
|
||||
RelativePercentOrAbsoluteDelta(
|
||||
candidate.TotalAbsoluteCurvatureVariationPerMeter,
|
||||
raw.TotalAbsoluteCurvatureVariationPerMeter),
|
||||
candidate.MinimumBodyClearanceMeters - raw.MinimumBodyClearanceMeters);
|
||||
}
|
||||
|
||||
private static double RelativePercentOrAbsoluteDelta(double candidate, double raw)
|
||||
{
|
||||
double delta = candidate - raw;
|
||||
return Math.Abs(raw) < 1e-12d ? delta : delta / raw * 100d;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,305 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Diagnostics;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Algorithms;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;
|
||||
|
||||
/// <summary>正式单算法路径平滑入口，负责输入校验、有限重试与经过复核的粗路径回退。</summary>
|
||||
public sealed class PathSmoothingService
|
||||
{
|
||||
private readonly PathSmoothingPreprocessor _preprocessor = new PathSmoothingPreprocessor();
|
||||
private readonly PathGeometryAnalyzer _analyzer = new PathGeometryAnalyzer();
|
||||
private readonly SmoothingAlgorithmRunner _runner = new SmoothingAlgorithmRunner();
|
||||
private readonly SmoothedPathValidator _validator = new SmoothedPathValidator();
|
||||
private readonly IPathSmoother _bSpline = new CubicBSplineSmoother();
|
||||
private readonly IPathSmoother _bezier = new LocalCubicBezierSmoother();
|
||||
private readonly IPathSmoother _quintic = new PiecewiseQuinticSmoother();
|
||||
private readonly LocalG2PreSmoothingPipeline _localG2Pipeline = new LocalG2PreSmoothingPipeline();
|
||||
|
||||
/// <summary>执行一次经过完整安全复核的单算法平滑。</summary>
|
||||
public PathSmoothingResult Smooth(
|
||||
PathSmoothingRequest request,
|
||||
CancellationToken cancellationToken = default)
|
||||
{
|
||||
var stopwatch = Stopwatch.StartNew();
|
||||
try
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
if (!TryValidateRequest(request, out PathSmoothingConfiguration configuration, out string reason))
|
||||
return Failure(PathSmoothingStatus.InvalidInput, stopwatch, 0, 0d, reason);
|
||||
|
||||
if (!_preprocessor.TryPrepare(request, out PreparedPath preparedPath, out reason))
|
||||
return Failure(PathSmoothingStatus.InvalidInput, stopwatch, 0, 0d, reason);
|
||||
|
||||
if (!RawPathBaselineBuilder.TryCreate(
|
||||
request,
|
||||
preparedPath,
|
||||
_analyzer,
|
||||
configuration.OutputSpacingMeters,
|
||||
_validator,
|
||||
configuration.MaximumCollisionCheckStepMeters,
|
||||
out RawPathBaseline rawBaseline,
|
||||
out reason))
|
||||
{
|
||||
return Failure(PathSmoothingStatus.InvalidInput, stopwatch, 0, 0d, reason);
|
||||
}
|
||||
|
||||
if (configuration.Method == SmoothingMethod.LocalG2Quintic)
|
||||
return _localG2Pipeline.Smooth(request, preparedPath, rawBaseline, cancellationToken);
|
||||
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
var input = new SmoothingAlgorithmInput(
|
||||
preparedPath,
|
||||
request.Map,
|
||||
request.Vehicle,
|
||||
configuration.MaximumCollisionCheckStepMeters,
|
||||
configuration.MinimumClearanceReserveMeters,
|
||||
new SmoothingOptionsSnapshot(configuration));
|
||||
SmoothingAlgorithmRunner.AlgorithmRunResult runResult = _runner.Run(
|
||||
Resolve(configuration.Method), input, configuration, cancellationToken);
|
||||
int retryCount = GetRetryCount(runResult.AttemptedStrengths);
|
||||
PathSmoothingDiagnostics diagnostics = new PathSmoothingDiagnostics(
|
||||
runResult.Metrics,
|
||||
stopwatch.Elapsed,
|
||||
retryCount,
|
||||
runResult.AcceptedStrength,
|
||||
runResult.Reason);
|
||||
|
||||
if (runResult.Status == PathSmoothingStatus.Success)
|
||||
{
|
||||
return PathSmoothingResult.Success(
|
||||
configuration.Method,
|
||||
runResult.Path,
|
||||
runResult.Segments,
|
||||
diagnostics);
|
||||
}
|
||||
|
||||
if (!configuration.AllowFallbackToCoarsePath)
|
||||
return PathSmoothingResult.Failure(runResult.Status, diagnostics);
|
||||
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
if (!TryCreateVerifiedFallback(
|
||||
request,
|
||||
configuration,
|
||||
cancellationToken,
|
||||
out IReadOnlyList<SmoothedPathPoint> fallbackPath,
|
||||
out IReadOnlyList<SmoothedPathSegment> fallbackSegments,
|
||||
out PathQualityMetrics fallbackMetrics,
|
||||
out reason))
|
||||
{
|
||||
return PathSmoothingResult.Failure(runResult.Status, diagnostics);
|
||||
}
|
||||
|
||||
var fallbackDiagnostics = new PathSmoothingDiagnostics(
|
||||
fallbackMetrics,
|
||||
stopwatch.Elapsed,
|
||||
retryCount,
|
||||
runResult.AcceptedStrength,
|
||||
runResult.Reason);
|
||||
return PathSmoothingResult.Fallback(
|
||||
configuration.Method,
|
||||
fallbackPath,
|
||||
fallbackSegments,
|
||||
fallbackDiagnostics);
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
return Failure(PathSmoothingStatus.Cancelled, stopwatch, 0, 0d, "路径平滑已取消。");
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
return Failure(PathSmoothingStatus.Failed, stopwatch, 0, 0d, exception.Message);
|
||||
}
|
||||
}
|
||||
|
||||
private bool TryCreateVerifiedFallback(
|
||||
PathSmoothingRequest request,
|
||||
PathSmoothingConfiguration configuration,
|
||||
CancellationToken cancellationToken,
|
||||
out IReadOnlyList<SmoothedPathPoint> fallbackPath,
|
||||
out IReadOnlyList<SmoothedPathSegment> fallbackSegments,
|
||||
out PathQualityMetrics fallbackMetrics,
|
||||
out string reason)
|
||||
{
|
||||
fallbackPath = null;
|
||||
fallbackSegments = null;
|
||||
fallbackMetrics = null;
|
||||
reason = string.Empty;
|
||||
|
||||
// Reprepare from the immutable request instead of reusing the algorithm input: fallback is a
|
||||
// separately published output and must repeat the coarse-path contract validation.
|
||||
if (!_preprocessor.TryPrepare(request, out PreparedPath revalidatedPath, out reason)) return false;
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
|
||||
if (!RawPathBaselineBuilder.TryCreate(
|
||||
request,
|
||||
revalidatedPath,
|
||||
_analyzer,
|
||||
configuration.OutputSpacingMeters,
|
||||
_validator,
|
||||
configuration.MaximumCollisionCheckStepMeters,
|
||||
out RawPathBaseline rawPath,
|
||||
out reason))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
fallbackPath = ToFallbackPoints(rawPath.Path);
|
||||
fallbackSegments = rawPath.Segments;
|
||||
fallbackMetrics = rawPath.Metrics;
|
||||
return true;
|
||||
}
|
||||
|
||||
private IPathSmoother Resolve(SmoothingMethod method)
|
||||
{
|
||||
return method switch
|
||||
{
|
||||
SmoothingMethod.CubicBSpline => _bSpline,
|
||||
SmoothingMethod.LocalCubicBezier => _bezier,
|
||||
SmoothingMethod.PiecewiseQuintic => _quintic,
|
||||
_ => throw new ArgumentOutOfRangeException(nameof(method)),
|
||||
};
|
||||
}
|
||||
|
||||
private static bool TryValidateRequest(
|
||||
PathSmoothingRequest request,
|
||||
out PathSmoothingConfiguration configuration,
|
||||
out string reason)
|
||||
{
|
||||
configuration = null;
|
||||
reason = string.Empty;
|
||||
if (request == null)
|
||||
{
|
||||
reason = "平滑请求为空。";
|
||||
return false;
|
||||
}
|
||||
|
||||
configuration = request.Configuration;
|
||||
VehicleParameters vehicle = request.Vehicle;
|
||||
if (configuration == null || request.Map == null || !request.Map.PlanningReady || vehicle == null)
|
||||
{
|
||||
reason = "平滑请求缺少可用的地图、车辆或配置。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!NumericGuard.IsPositiveFinite(vehicle.LengthMeters) ||
|
||||
!NumericGuard.IsPositiveFinite(vehicle.WidthMeters) ||
|
||||
!NumericGuard.IsFinite(vehicle.SafetyMarginMeters) || vehicle.SafetyMarginMeters < 0d ||
|
||||
!VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out _))
|
||||
{
|
||||
reason = "平滑请求中的车辆几何或曲率约束无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!Enum.IsDefined(typeof(SmoothingMethod), configuration.Method))
|
||||
{
|
||||
reason = "平滑方法无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!NumericGuard.IsPositiveFinite(configuration.OutputSpacingMeters) ||
|
||||
!NumericGuard.IsPositiveFinite(configuration.MaximumCollisionCheckStepMeters) ||
|
||||
!NumericGuard.IsFinite(configuration.MinimumClearanceReserveMeters) ||
|
||||
configuration.MinimumClearanceReserveMeters < 0d ||
|
||||
!NumericGuard.IsPositiveFinite(configuration.SmoothingStrength))
|
||||
{
|
||||
reason = "平滑配置包含非法数值或不满足方法契约。";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (configuration.Method == SmoothingMethod.LocalG2Quintic)
|
||||
return IsValidLocalG2Options(configuration.LocalG2Quintic, out reason);
|
||||
|
||||
if (
|
||||
!NumericGuard.IsPositiveFinite(configuration.CubicBSpline.EndpointTangentScale) ||
|
||||
!IsValidBezierThreshold(configuration.LocalCubicBezier.CornerHeadingThresholdRadians) ||
|
||||
!NumericGuard.IsPositiveFinite(configuration.LocalCubicBezier.MaximumWindowLengthMeters) ||
|
||||
!NumericGuard.IsPositiveFinite(configuration.LocalCubicBezier.HandleLengthRatio) ||
|
||||
!NumericGuard.IsPositiveFinite(configuration.PiecewiseQuintic.KnotSpacingMeters) ||
|
||||
!NumericGuard.IsPositiveFinite(configuration.PiecewiseQuintic.MinimumKnotSpacingMeters) ||
|
||||
configuration.PiecewiseQuintic.KnotSpacingMeters < configuration.PiecewiseQuintic.MinimumKnotSpacingMeters)
|
||||
{
|
||||
reason = "平滑配置包含非法数值或不满足方法契约。";
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool IsValidLocalG2Options(LocalG2QuinticOptions options, out string reason)
|
||||
{
|
||||
reason = string.Empty;
|
||||
if (options != null &&
|
||||
NumericGuard.IsPositiveFinite(options.MinimumWindowLengthMeters) &&
|
||||
NumericGuard.IsFinite(options.PreferredWindowLengthMeters) &&
|
||||
options.PreferredWindowLengthMeters >= options.MinimumWindowLengthMeters &&
|
||||
NumericGuard.IsFinite(options.MaximumWindowLengthMeters) &&
|
||||
options.MaximumWindowLengthMeters >= options.PreferredWindowLengthMeters &&
|
||||
NumericGuard.IsPositiveFinite(options.MaximumDeviationMeters) &&
|
||||
NumericGuard.IsPositiveFinite(options.AbsoluteCurvatureJumpFloorPerMeter) &&
|
||||
NumericGuard.IsFinite(options.CurvatureJumpRatioOfMaximum) &&
|
||||
options.CurvatureJumpRatioOfMaximum > 0d && options.CurvatureJumpRatioOfMaximum <= 1d &&
|
||||
NumericGuard.IsFinite(options.MinimumPeakGradientImprovementRatio) &&
|
||||
options.MinimumPeakGradientImprovementRatio > 0d && options.MinimumPeakGradientImprovementRatio < 1d &&
|
||||
NumericGuard.IsFinite(options.MaximumVariationCostRegressionRatio) &&
|
||||
options.MaximumVariationCostRegressionRatio >= 0d && options.MaximumCandidatesPerRegion >= 1)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
reason = "局部 G2 配置包含非法数值或不满足方法契约。";
|
||||
return false;
|
||||
}
|
||||
|
||||
private static bool IsValidBezierThreshold(double thresholdRadians)
|
||||
{
|
||||
return NumericGuard.IsFinite(thresholdRadians) && thresholdRadians > 0d && thresholdRadians <= Math.PI;
|
||||
}
|
||||
|
||||
private static IReadOnlyList<SmoothedPathPoint> ToFallbackPoints(IReadOnlyList<SmoothedPathPoint> path)
|
||||
{
|
||||
var points = new List<SmoothedPathPoint>(path.Count);
|
||||
for (int index = 0; index < path.Count; index++)
|
||||
{
|
||||
SmoothedPathPoint point = path[index];
|
||||
points.Add(new SmoothedPathPoint(
|
||||
point.X,
|
||||
point.Y,
|
||||
point.Heading,
|
||||
point.UnwrappedHeading,
|
||||
point.ArcLength,
|
||||
point.Direction,
|
||||
point.GeometricCurvature,
|
||||
point.VehicleCurvature,
|
||||
point.VehicleCurvatureDerivative,
|
||||
point.BodyClearance,
|
||||
point.IsGearSwitchPoint,
|
||||
SmoothedPathPointSource.CoarsePathFallback));
|
||||
}
|
||||
return points;
|
||||
}
|
||||
|
||||
private static int GetRetryCount(IReadOnlyList<double> attemptedStrengths)
|
||||
{
|
||||
return attemptedStrengths == null || attemptedStrengths.Count == 0 ? 0 : attemptedStrengths.Count - 1;
|
||||
}
|
||||
|
||||
private static PathSmoothingResult Failure(
|
||||
PathSmoothingStatus status,
|
||||
Stopwatch stopwatch,
|
||||
int retryCount,
|
||||
double acceptedStrength,
|
||||
string reason)
|
||||
{
|
||||
return PathSmoothingResult.Failure(
|
||||
status,
|
||||
new PathSmoothingDiagnostics(new PathQualityMetrics(), stopwatch.Elapsed, retryCount, acceptedStrength, reason));
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user