ParkingRobot/ClumsyPilot/ParkrobTrajplanner/CoarsePath/Output/PathBacktracker.cs

using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
using MultiWheelC.TrajectoryPlanning.Utils;

namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Output;

/// <summary>
/// 成功搜索父链重新生成后的原语序列。
/// 起点位置使用 m/rad，起始曲率使用 1/m；原语集合按从起点到终点的顺序排列。
/// </summary>
public sealed class BacktrackedPath
{
    internal BacktrackedPath(Pose2D start, TravelDirection startDirection, double startCurvaturePerMeter,
        IReadOnlyList<MotionPrimitive> primitives)
    {
        Start = start;
        StartDirection = startDirection;
        StartCurvaturePerMeter = startCurvaturePerMeter;
        Primitives = primitives;
    }

    /// <summary>原始请求中的起始车辆中心位姿；位置单位 m，航向单位 rad。</summary>
    public Pose2D Start { get; }

    /// <summary>成功父链根节点记录的起步方向。</summary>
    public TravelDirection StartDirection { get; }

    /// <summary>成功父链根节点离散后的起始曲率，单位 1/m。</summary>
    public double StartCurvaturePerMeter { get; }

    /// <summary>按起点到终点顺序重新积分的恒曲率原语；每项都不包含自身起点。</summary>
    public IReadOnlyList<MotionPrimitive> Primitives { get; }
}

/// <summary>
/// 仅依据成功节点的父索引和原语描述，确定性地重建稠密原语序列。
/// 不复用搜索期保存的积分点，以保证输出与当前解析积分、扫掠检查规则一致。
/// </summary>
public sealed class PathBacktracker
{
    private const double PoseComparisonTolerance = 1e-7d;
    private const double LengthComparisonTolerance = 1e-7d;
    private readonly MotionPrimitiveGenerator _primitiveGenerator;

    /// <summary>创建使用默认解析积分器的回溯器。</summary>
    public PathBacktracker()
        : this(new MotionPrimitiveGenerator())
    {
    }

    /// <summary>创建使用指定原语生成器的回溯器。</summary>
    public PathBacktracker(MotionPrimitiveGenerator primitiveGenerator)
    {
        _primitiveGenerator = primitiveGenerator ?? throw new ArgumentNullException(nameof(primitiveGenerator));
    }

    /// <summary>
    /// 依据搜索成功节点重建从起点到终点的原语序列。
    /// 参数：searchResult 必须是携带成功节点索引的搜索结果；request 必须仍指向执行搜索的同一不可变地图和配置。
    /// 返回：父链无环、索引连续且每条原语按同一解析规则可重建时为 true；失败时 path 为 null 并返回可读原因。
    /// </summary>
    public bool TryBacktrack(HybridAStarSearchResult searchResult, PlanningRequest request,
        out BacktrackedPath path, out string failureReason)
    {
        path = null;
        failureReason = string.Empty;
        if (searchResult == null || request == null || searchResult.Status != PlanningStatus.Success ||
            !searchResult.SuccessNodeIndex.HasValue || searchResult.Nodes == null)
        {
            failureReason = "搜索结果不含可回溯的成功节点。";
            return false;
        }

        int currentIndex = searchResult.SuccessNodeIndex.Value;
        var reverseNodes = new List<HybridAStarNode>();
        var visited = new HashSet<int>();
        while (currentIndex >= 0)
        {
            if (currentIndex >= searchResult.Nodes.Count || !visited.Add(currentIndex))
            {
                failureReason = "搜索父链索引越界或存在环。";
                return false;
            }

            HybridAStarNode current = searchResult.Nodes[currentIndex];
            if (current == null || current.NodeIndex != currentIndex || current.Pose == null)
            {
                failureReason = "搜索节点索引或连续位姿不一致。";
                return false;
            }

            reverseNodes.Add(current);
            currentIndex = current.ParentNodeIndex;
        }

        if (reverseNodes.Count == 0)
        {
            failureReason = "搜索父链为空。";
            return false;
        }

        reverseNodes.Reverse();
        HybridAStarNode root = reverseNodes[0];
        if (root.ParentNodeIndex != -1 || root.IncomingPrimitive != null || !IsFinitePose(root.Pose))
        {
            failureReason = "搜索父链根节点无效。";
            return false;
        }

        var primitives = new List<MotionPrimitive>(Math.Max(0, reverseNodes.Count - 1));
        Pose2D previousPose = root.Pose;
        for (int index = 1; index < reverseNodes.Count; index++)
        {
            HybridAStarNode node = reverseNodes[index];
            MotionPrimitive descriptor = node.IncomingPrimitive;
            if (node.ParentNodeIndex != reverseNodes[index - 1].NodeIndex || descriptor == null ||
                !IsFinitePose(node.Pose) || !NumericGuard.IsFinite(descriptor.CurvaturePerMeter) ||
                !IsTravelDirection(descriptor.Direction) || descriptor.ActualLengthMeters <= 0d)
            {
                failureReason = "搜索父链中的原语描述无效。";
                return false;
            }

            // 只把恒曲率、方向和长度描述作为真源，再走一遍相同的解析积分与连续碰撞检查。
            MotionPrimitive rebuilt = _primitiveGenerator.Generate(previousPose, descriptor.CurvaturePerMeter, descriptor.Direction, request);
            if (rebuilt == null || !IsEquivalent(descriptor, rebuilt) || !IsSamePose(rebuilt.End, node.Pose))
            {
                failureReason = "搜索原语无法按当前解析规则确定性重建。";
                return false;
            }

            primitives.Add(rebuilt);
            previousPose = rebuilt.End;
        }

        path = new BacktrackedPath(root.Pose, root.Direction, root.CurvaturePerMeter,
            new ReadOnlyCollection<MotionPrimitive>(primitives));
        return true;
    }

    private static bool IsEquivalent(MotionPrimitive expected, MotionPrimitive actual)
    {
        return expected.Direction == actual.Direction && expected.IsGoalTruncation == actual.IsGoalTruncation &&
            Math.Abs(expected.CurvaturePerMeter - actual.CurvaturePerMeter) <= PoseComparisonTolerance &&
            Math.Abs(expected.ActualLengthMeters - actual.ActualLengthMeters) <= LengthComparisonTolerance;
    }

    private static bool IsSamePose(Pose2D first, Pose2D second)
    {
        return IsFinitePose(first) && IsFinitePose(second) &&
            Math.Abs(first.X - second.X) <= PoseComparisonTolerance &&
            Math.Abs(first.Y - second.Y) <= PoseComparisonTolerance &&
            Math.Abs(AngleMath.ShortestSignedDifference(first.Heading, second.Heading)) <= PoseComparisonTolerance;
    }

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