ParkingRobot/ClumsyPilot/ParkrobTrajplanner/CoarsePath/Search/MotionPrimitiveGenerator.cs

using System;
using System.Collections.Generic;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
using MultiWheelC.TrajectoryPlanning.Mapping;
using MultiWheelC.TrajectoryPlanning.Utils;

namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;

/// <summary>
/// 生成经解析积分和连续碰撞检查的前进或倒车恒曲率原语。
/// 每个积分点均依次经过有限数值检查、与前一点之间的扫掠碰撞检查和终点容差检查。
/// </summary>
public sealed class MotionPrimitiveGenerator
{
    private const double StraightCurvatureThreshold = 1e-12d;
    private readonly FootprintCollisionChecker _collisionChecker;

    /// <summary>创建使用默认车辆连续碰撞检查器的原语生成器。</summary>
    public MotionPrimitiveGenerator()
        : this(new FootprintCollisionChecker())
    {
    }

    /// <summary>创建使用指定连续车辆碰撞检查器的原语生成器。</summary>
    public MotionPrimitiveGenerator(FootprintCollisionChecker collisionChecker)
    {
        _collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
    }

    /// <summary>
    /// 使用完整规划请求生成一条原语。
    /// 参数：start 为当前连续位姿；curvaturePerMeter 为候选恒定曲率；direction 为前进或倒车；request 提供地图、车辆、配置和目标。
    /// 返回：输入无效、曲率超限或任一积分点碰撞时为 null；否则返回最大长度不超过配置上限的原语。
    /// </summary>
    public MotionPrimitive Generate(Pose2D start, double curvaturePerMeter, TravelDirection direction, PlanningRequest request)
    {
        if (request == null) return null;
        return Generate(start, curvaturePerMeter, direction, request.Map, request.Vehicle, request.Configuration, request.Goal, request.GoalDirection);
    }

    /// <summary>
    /// 使用显式地图、车辆、配置和目标生成一条原语。
    /// 参数：所有位置使用 m/rad，curvaturePerMeter 使用 1/m；goalDirection 限制末段允许的进入方向。
    /// 返回：输入无效、曲率超限或任一积分点碰撞时为 null；首次命中目标时返回 <see cref="MotionPrimitive.IsGoalTruncation"/> 为 true 的截断原语。
    /// </summary>
    public MotionPrimitive Generate(
        Pose2D start,
        double curvaturePerMeter,
        TravelDirection direction,
        PlanningGridMap map,
        VehicleParameters vehicle,
        HybridAStarConfiguration configuration,
        Pose2D goal,
        GoalDirectionConstraint goalDirection)
    {
        if (!IsValidInput(start, curvaturePerMeter, direction, map, vehicle, configuration, goal, goalDirection)) return null;

        if (GoalToleranceChecker.IsSatisfied(start, goal, configuration, direction, goalDirection))
            return new MotionPrimitive(start, direction, curvaturePerMeter, 0d, Array.Empty<Pose2D>(), Array.Empty<double>(), true);

        double pointStepMeters = Math.Min(configuration.IntegrationStepMeters,
            Math.Min(configuration.MaximumCollisionCheckStepMeters, map.ResolutionMeters / 2d));
        if (!NumericGuard.IsPositiveFinite(pointStepMeters)) return null;

        var points = new List<Pose2D>();
        var bodyClearancesMeters = new List<double>();
        Pose2D previous = start;
        double actualLengthMeters = 0d;
        while (actualLengthMeters < configuration.PrimitiveLengthMeters)
        {
            double remainingLengthMeters = configuration.PrimitiveLengthMeters - actualLengthMeters;
            double stepMeters = Math.Min(pointStepMeters, remainingLengthMeters);
            if (!NumericGuard.IsPositiveFinite(stepMeters)) return null;

            Pose2D next = Integrate(previous, curvaturePerMeter, direction, stepMeters);
            if (!IsFinitePose(next)) return null;
            if (!_collisionChecker.IsSweptMotionCollisionFree(previous, next, map, vehicle, stepMeters, out double bodyClearanceMeters)) return null;

            actualLengthMeters += stepMeters;
            points.Add(next);
            bodyClearancesMeters.Add(bodyClearanceMeters);
            if (GoalToleranceChecker.IsSatisfied(next, goal, configuration, direction, goalDirection))
                return new MotionPrimitive(start, direction, curvaturePerMeter, actualLengthMeters, points, bodyClearancesMeters, true);

            previous = next;
        }

        return new MotionPrimitive(start, direction, curvaturePerMeter, actualLengthMeters, points, bodyClearancesMeters, false);
    }

    /// <summary>
    /// 获取从最大负曲率到最大正曲率均匀分布的候选曲率等级。
    /// 参数：vehicle 提供保守最大曲率；configuration 的曲率等级数必须为不小于 3 的奇数。
    /// 返回：输入无效时为空只读列表；有效时长度等于配置等级数且中间等级恒为零曲率。
    /// </summary>
    public IReadOnlyList<double> GetCurvatureLevels(VehicleParameters vehicle, HybridAStarConfiguration configuration)
    {
        if (configuration == null || configuration.CurvatureLevelCount < 3 || configuration.CurvatureLevelCount % 2 == 0 ||
            !VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double maximumCurvaturePerMeter))
            return Array.Empty<double>();

        var levels = new double[configuration.CurvatureLevelCount];
        double increment = 2d * maximumCurvaturePerMeter / (levels.Length - 1d);
        for (int index = 0; index < levels.Length; index++) levels[index] = -maximumCurvaturePerMeter + increment * index;
        levels[levels.Length / 2] = 0d;
        return levels;
    }

    /// <summary>
    /// 判断两个曲率等级是否允许在相邻原语间直接切换。
    /// 参数：previousLevelIndex 与 nextLevelIndex 为从零开始的等级索引。
    /// 返回：两个索引均非负且最多相差一个等级时为 true。
    /// </summary>
    public static bool AreCurvatureLevelsAdjacent(int previousLevelIndex, int nextLevelIndex)
    {
        return previousLevelIndex >= 0 && nextLevelIndex >= 0 && Math.Abs(previousLevelIndex - nextLevelIndex) <= 1;
    }

    private static bool IsValidInput(
        Pose2D start,
        double curvaturePerMeter,
        TravelDirection direction,
        PlanningGridMap map,
        VehicleParameters vehicle,
        HybridAStarConfiguration configuration,
        Pose2D goal,
        GoalDirectionConstraint goalDirection)
    {
        if (!IsFinitePose(start) || !IsFinitePose(goal) || map == null || vehicle == null || configuration == null ||
            !NumericGuard.IsFinite(curvaturePerMeter) || !IsTravelDirection(direction) || !IsGoalDirection(goalDirection) ||
            !NumericGuard.IsPositiveFinite(configuration.PrimitiveLengthMeters) ||
            !NumericGuard.IsPositiveFinite(configuration.IntegrationStepMeters) ||
            !NumericGuard.IsPositiveFinite(configuration.MaximumCollisionCheckStepMeters) ||
            !NumericGuard.IsPositiveFinite(map.ResolutionMeters) ||
            !VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double maximumCurvaturePerMeter))
            return false;

        return Math.Abs(curvaturePerMeter) <= maximumCurvaturePerMeter;
    }

    private static Pose2D Integrate(Pose2D previous, double curvaturePerMeter, TravelDirection direction, double stepMeters)
    {
        double signedDistanceMeters = direction == TravelDirection.Forward ? stepMeters : -stepMeters;
        double nextHeadingRadians = AngleMath.NormalizeRadians(previous.Heading + curvaturePerMeter * signedDistanceMeters);
        if (Math.Abs(curvaturePerMeter) < StraightCurvatureThreshold)
        {
            return new Pose2D(
                previous.X + signedDistanceMeters * Math.Cos(previous.Heading),
                previous.Y + signedDistanceMeters * Math.Sin(previous.Heading),
                nextHeadingRadians);
        }

        return new Pose2D(
            previous.X + (Math.Sin(nextHeadingRadians) - Math.Sin(previous.Heading)) / curvaturePerMeter,
            previous.Y - (Math.Cos(nextHeadingRadians) - Math.Cos(previous.Heading)) / curvaturePerMeter,
            nextHeadingRadians);
    }

    private static bool IsFinitePose(Pose2D pose)
    {
        return pose != null && NumericGuard.IsFinite(pose.X) && NumericGuard.IsFinite(pose.Y) && NumericGuard.IsFinite(pose.Heading);
    }

    private static bool IsTravelDirection(TravelDirection direction)
    {
        return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
    }

    private static bool IsGoalDirection(GoalDirectionConstraint direction)
    {
        return direction == GoalDirectionConstraint.Any || direction == GoalDirectionConstraint.Forward || direction == GoalDirectionConstraint.Reverse;
    }
}