99 lines
5.4 KiB
C#
99 lines
5.4 KiB
C#
using System;
|
||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||
|
||
namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
|
||
|
||
/// <summary>
|
||
/// 将运动原语的长度、方向、曲率和净空转换为统一的等效米搜索代价。
|
||
/// 此类型不修改搜索状态；换向标记必须由调用方依据相邻原语的方向关系提供。
|
||
/// </summary>
|
||
public sealed class SearchCostCalculator
|
||
{
|
||
/// <summary>创建使用调用时配置参数的等效米代价计算器。</summary>
|
||
public SearchCostCalculator()
|
||
{
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算一条运动原语的等效米增量代价。
|
||
/// 参数：lengthMeters 为非负弧长 m；direction 为原语方向；isGearSwitch 表示该原语前是否换向；
|
||
/// curvaturePerMeter 与 maximumCurvaturePerMeter 的单位为 1/m；curvatureLevelDelta 为相邻曲率等级差；
|
||
/// bodyClearanceMeters 为非负车体保守净空 m，可为正无穷；configuration 提供非负权重和惩罚。
|
||
/// 返回：有限且非负的等效米代价。
|
||
/// 失败：任一数值、方向或权重无效时抛出 <see cref="ArgumentOutOfRangeException"/>。
|
||
/// </summary>
|
||
public double Calculate(
|
||
double lengthMeters,
|
||
TravelDirection direction,
|
||
bool isGearSwitch,
|
||
double curvaturePerMeter,
|
||
double maximumCurvaturePerMeter,
|
||
int curvatureLevelDelta,
|
||
double bodyClearanceMeters,
|
||
HybridAStarConfiguration configuration)
|
||
{
|
||
ValidateInput(lengthMeters, direction, curvaturePerMeter, maximumCurvaturePerMeter, bodyClearanceMeters, configuration);
|
||
|
||
double directionMultiplier = direction == TravelDirection.Reverse ? configuration.ReverseCostMultiplier : 1d;
|
||
double normalizedCurvature = Math.Abs(curvaturePerMeter / maximumCurvaturePerMeter);
|
||
double clearanceDeficit = GetClearanceDeficit(bodyClearanceMeters, configuration.ClearanceCostDistanceMeters);
|
||
double levelDelta = Math.Abs((double)curvatureLevelDelta);
|
||
double motionCost = lengthMeters * directionMultiplier *
|
||
(1d + configuration.CurvatureMagnitudeWeight * normalizedCurvature +
|
||
configuration.ClearanceCostWeight * clearanceDeficit);
|
||
double gearSwitchCost = isGearSwitch ? configuration.GearSwitchPenaltyMeters : 0d;
|
||
double curvatureChangeCost = configuration.CurvatureChangePenaltyMetersPerLevel * levelDelta;
|
||
double totalCost = motionCost + gearSwitchCost + curvatureChangeCost;
|
||
if (!NumericGuard.IsFinite(totalCost) || totalCost < 0d)
|
||
throw new ArgumentOutOfRangeException(nameof(lengthMeters), "The calculated cost must remain finite and non-negative.");
|
||
|
||
return totalCost;
|
||
}
|
||
|
||
private static void ValidateInput(
|
||
double lengthMeters,
|
||
TravelDirection direction,
|
||
double curvaturePerMeter,
|
||
double maximumCurvaturePerMeter,
|
||
double bodyClearanceMeters,
|
||
HybridAStarConfiguration configuration)
|
||
{
|
||
if (!NumericGuard.IsFinite(lengthMeters) || lengthMeters < 0d)
|
||
throw new ArgumentOutOfRangeException(nameof(lengthMeters));
|
||
if (direction != TravelDirection.Forward && direction != TravelDirection.Reverse)
|
||
throw new ArgumentOutOfRangeException(nameof(direction));
|
||
if (!NumericGuard.IsFinite(curvaturePerMeter))
|
||
throw new ArgumentOutOfRangeException(nameof(curvaturePerMeter));
|
||
if (!NumericGuard.IsPositiveFinite(maximumCurvaturePerMeter))
|
||
throw new ArgumentOutOfRangeException(nameof(maximumCurvaturePerMeter));
|
||
if (Math.Abs(curvaturePerMeter) > maximumCurvaturePerMeter)
|
||
throw new ArgumentOutOfRangeException(nameof(curvaturePerMeter));
|
||
if (double.IsNaN(bodyClearanceMeters) || bodyClearanceMeters < 0d)
|
||
throw new ArgumentOutOfRangeException(nameof(bodyClearanceMeters));
|
||
if (configuration == null) throw new ArgumentNullException(nameof(configuration));
|
||
|
||
ValidateNonNegativeFinite(configuration.HeuristicWeight, nameof(configuration.HeuristicWeight));
|
||
if (!NumericGuard.IsPositiveFinite(configuration.ReverseCostMultiplier))
|
||
throw new ArgumentOutOfRangeException(nameof(configuration.ReverseCostMultiplier));
|
||
ValidateNonNegativeFinite(configuration.GearSwitchPenaltyMeters, nameof(configuration.GearSwitchPenaltyMeters));
|
||
ValidateNonNegativeFinite(configuration.CurvatureMagnitudeWeight, nameof(configuration.CurvatureMagnitudeWeight));
|
||
ValidateNonNegativeFinite(configuration.CurvatureChangePenaltyMetersPerLevel, nameof(configuration.CurvatureChangePenaltyMetersPerLevel));
|
||
ValidateNonNegativeFinite(configuration.ClearanceCostWeight, nameof(configuration.ClearanceCostWeight));
|
||
if (!NumericGuard.IsPositiveFinite(configuration.ClearanceCostDistanceMeters))
|
||
throw new ArgumentOutOfRangeException(nameof(configuration.ClearanceCostDistanceMeters));
|
||
}
|
||
|
||
private static void ValidateNonNegativeFinite(double value, string parameterName)
|
||
{
|
||
if (!NumericGuard.IsFinite(value) || value < 0d)
|
||
throw new ArgumentOutOfRangeException(parameterName);
|
||
}
|
||
|
||
private static double GetClearanceDeficit(double bodyClearanceMeters, double clearanceCostDistanceMeters)
|
||
{
|
||
if (double.IsPositiveInfinity(bodyClearanceMeters)) return 0d;
|
||
double deficit = 1d - bodyClearanceMeters / clearanceCostDistanceMeters;
|
||
return deficit > 0d ? deficit : 0d;
|
||
}
|
||
}
|