61 lines
2.4 KiB
C#
61 lines
2.4 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;
|
|
|
|
/// <summary>按公开字典序选择唯一的推荐平滑方法。</summary>
|
|
public static class SmoothingMethodRanker
|
|
{
|
|
/// <summary>从当前场景的可行且确定性候选中选择最佳方法；没有合格候选时返回空。</summary>
|
|
public static SmoothingMethod? Rank(IReadOnlyList<PathSmoothingComparisonEntry> entries)
|
|
{
|
|
PathSmoothingComparisonEntry best = null;
|
|
if (entries == null) return null;
|
|
|
|
for (int index = 0; index < entries.Count; index++)
|
|
{
|
|
PathSmoothingComparisonEntry candidate = entries[index];
|
|
if (candidate == null || !candidate.IsEligibleForRecommendation) continue;
|
|
if (best == null || Compare(candidate, best) < 0) best = candidate;
|
|
}
|
|
return best == null ? (SmoothingMethod?)null : best.Method;
|
|
}
|
|
|
|
private static int Compare(PathSmoothingComparisonEntry left, PathSmoothingComparisonEntry right)
|
|
{
|
|
int comparison = CompareAscending(left.Metrics.CurvatureVariationEnergy, right.Metrics.CurvatureVariationEnergy);
|
|
if (comparison != 0) return comparison;
|
|
|
|
comparison = CompareAscending(
|
|
left.Metrics.MaximumAbsoluteVehicleCurvaturePerMeter,
|
|
right.Metrics.MaximumAbsoluteVehicleCurvaturePerMeter);
|
|
if (comparison != 0) return comparison;
|
|
|
|
comparison = CompareDescending(left.Metrics.MinimumBodyClearanceMeters, right.Metrics.MinimumBodyClearanceMeters);
|
|
if (comparison != 0) return comparison;
|
|
|
|
comparison = CompareAscending(left.Metrics.LengthChangePercent, right.Metrics.LengthChangePercent);
|
|
if (comparison != 0) return comparison;
|
|
|
|
comparison = CompareAscending(left.Timing.MedianElapsedMilliseconds, right.Timing.MedianElapsedMilliseconds);
|
|
if (comparison != 0) return comparison;
|
|
|
|
return ((int)left.Method.Value).CompareTo((int)right.Method.Value);
|
|
}
|
|
|
|
private static int CompareAscending(double left, double right)
|
|
{
|
|
return Normalize(left).CompareTo(Normalize(right));
|
|
}
|
|
|
|
private static int CompareDescending(double left, double right)
|
|
{
|
|
return Normalize(right).CompareTo(Normalize(left));
|
|
}
|
|
|
|
private static double Normalize(double value)
|
|
{
|
|
return double.IsNaN(value) || double.IsInfinity(value) ? double.PositiveInfinity : value;
|
|
}
|
|
}
|