85 lines
2.9 KiB
C#
85 lines
2.9 KiB
C#
using System.Collections.Generic;
|
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
|
|
|
/// <summary>通过候选路径同一几何分析器和验证器构建安全、可公平比较的原始基线。</summary>
|
|
internal static class RawPathBaselineBuilder
|
|
{
|
|
internal static bool TryCreate(
|
|
PathSmoothingRequest request,
|
|
PreparedPath preparedPath,
|
|
PathGeometryAnalyzer analyzer,
|
|
double outputSpacingMeters,
|
|
SmoothedPathValidator validator,
|
|
double maximumCollisionCheckStepMeters,
|
|
out RawPathBaseline baseline,
|
|
out string reason)
|
|
{
|
|
baseline = null;
|
|
reason = string.Empty;
|
|
if (request == null || preparedPath == null || analyzer == null || validator == null)
|
|
{
|
|
reason = "原始粗路径基线缺少请求、预处理路径、几何分析器或安全验证器。";
|
|
return false;
|
|
}
|
|
|
|
if (!analyzer.TryAnalyze(preparedPath.Segments, outputSpacingMeters, out PathGeometryAnalysis analysis, out reason))
|
|
return false;
|
|
|
|
if (!validator.TryValidate(
|
|
analysis.Path,
|
|
analysis.Segments,
|
|
preparedPath,
|
|
request.Map,
|
|
request.Vehicle,
|
|
maximumCollisionCheckStepMeters,
|
|
out IReadOnlyList<SmoothedPathPoint> safePath,
|
|
out double minimumClearanceMeters,
|
|
out reason))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
baseline = new RawPathBaseline(safePath, analysis.Segments, CreateMetrics(analysis, minimumClearanceMeters));
|
|
return true;
|
|
}
|
|
|
|
private static PathQualityMetrics CreateMetrics(
|
|
PathGeometryAnalysis analysis,
|
|
double minimumClearanceMeters)
|
|
{
|
|
return new PathQualityMetrics(
|
|
true,
|
|
analysis.PathLengthMeters,
|
|
analysis.MaximumAbsoluteVehicleCurvaturePerMeter,
|
|
analysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter,
|
|
analysis.RootMeanSquareVehicleCurvaturePerMeter,
|
|
analysis.TotalAbsoluteCurvatureVariationPerMeter,
|
|
analysis.CurvatureVariationEnergy,
|
|
minimumClearanceMeters,
|
|
0d,
|
|
0d,
|
|
0d,
|
|
0d);
|
|
}
|
|
}
|
|
|
|
/// <summary>已通过完整车体复核的原始粗路径及其比较指标。</summary>
|
|
internal sealed class RawPathBaseline
|
|
{
|
|
internal RawPathBaseline(
|
|
IReadOnlyList<SmoothedPathPoint> path,
|
|
IReadOnlyList<SmoothedPathSegment> segments,
|
|
PathQualityMetrics metrics)
|
|
{
|
|
Path = path;
|
|
Segments = segments;
|
|
Metrics = metrics;
|
|
}
|
|
|
|
internal IReadOnlyList<SmoothedPathPoint> Path { get; }
|
|
internal IReadOnlyList<SmoothedPathSegment> Segments { get; }
|
|
internal PathQualityMetrics Metrics { get; }
|
|
}
|