ParkingRobot/.task8-sweep/ParkrobTrajplanner/CoarsePath/Search/SearchCostCalculator.cs

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;
    }
}