ParkingRobot/.task8-sweep/ParkrobTrajplanner/PathSmoothing/Visualization/SmoothingFigureModelBuilder.cs

using System;
using System.Collections.Generic;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.Mapping;
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Visualization;

/// <summary>将不可变的比较结果、地图和端点转换为共享报告图形模型。</summary>
public sealed class SmoothingFigureModelBuilder
{
    private const double MarginPoints = 18d;
    private const double PanelGapPoints = 12d;

    public SmoothingFigureModel Build(
        PathSmoothingComparisonResult comparison,
        PlanningGridMap map,
        Pose2D start,
        Pose2D goal,
        string scenarioId,
        string scenarioLabel)
    {
        if (comparison == null) throw new ArgumentNullException(nameof(comparison));
        if (map == null) throw new ArgumentNullException(nameof(map));
        if (start == null) throw new ArgumentNullException(nameof(start));
        if (goal == null) throw new ArgumentNullException(nameof(goal));

        double worldXMin = map.Bounds.XMin / 1000d;
        double worldXMax = map.Bounds.XMax / 1000d;
        double worldYMin = map.Bounds.YMin / 1000d;
        double worldYMax = map.Bounds.YMax / 1000d;
        double innerWidth = IeeeFigureStyle.FigureWidthPoints - 2d * MarginPoints;
        double innerHeight = IeeeFigureStyle.FigureHeightPoints - 2d * MarginPoints;
        double pathWidth = innerWidth * 0.60d;
        double rightWidth = innerWidth - pathWidth - PanelGapPoints;
        double rightHeight = (innerHeight - PanelGapPoints) / 2d;

        var model = new SmoothingFigureModel(
            scenarioId, scenarioLabel, worldXMin, worldXMax, worldYMin, worldYMax,
            MarginPoints, MarginPoints, pathWidth, innerHeight,
            MarginPoints + pathWidth + PanelGapPoints, MarginPoints, rightWidth, rightHeight,
            MarginPoints + pathWidth + PanelGapPoints, MarginPoints + rightHeight + PanelGapPoints, rightWidth, rightHeight,
            BuildObstacles(map), BuildSeries(comparison, map), BuildMetricRows(comparison),
            new SmoothingFigurePoint(start.X, start.Y, 0d, 0d), new SmoothingFigurePoint(goal.X, goal.Y, 0d, 0d));

        double worldWidth = worldXMax - worldXMin;
        double worldHeight = worldYMax - worldYMin;
        if (worldWidth <= 0d || worldHeight <= 0d) throw new ArgumentOutOfRangeException(nameof(map), "地图边界必须为非退化有限范围。");
        double scale = Math.Min(pathWidth / worldWidth, innerHeight / worldHeight);
        model.PathScaleX = scale;
        model.PathScaleY = scale;
        return model;
    }

    private static IReadOnlyList<SmoothingFigureObstacle> BuildObstacles(PlanningGridMap map)
    {
        var runs = new List<ObstacleRun>();
        double resolution = map.ResolutionMeters;
        double xMin = map.Bounds.XMin / 1000d;
        double yMin = map.Bounds.YMin / 1000d;
        for (int row = 0; row < map.Rows; row++)
        {
            int runStart = -1;
            for (int column = 0; column <= map.Cols; column++)
            {
                bool occupied = column < map.Cols && map.IsOccupied(row, column);
                if (occupied && runStart < 0) { runStart = column; continue; }
                if (!occupied && runStart >= 0)
                {
                    AddOrExtendRun(runs, xMin + runStart * resolution, yMin + row * resolution,
                        (column - runStart) * resolution, resolution);
                    runStart = -1;
                }
            }
        }
        var obstacles = new List<SmoothingFigureObstacle>(runs.Count);
        for (int index = 0; index < runs.Count; index++)
        {
            ObstacleRun run = runs[index];
            obstacles.Add(new SmoothingFigureObstacle(run.X, run.Y, run.Width, run.Height));
        }
        return obstacles;
    }

    private static void AddOrExtendRun(IList<ObstacleRun> runs, double x, double y, double width, double height)
    {
        for (int index = runs.Count - 1; index >= 0; index--)
        {
            ObstacleRun candidate = runs[index];
            if (NearlyEqual(candidate.X, x) && NearlyEqual(candidate.Width, width) && NearlyEqual(candidate.Y + candidate.Height, y))
            {
                candidate.Height += height;
                return;
            }
        }
        runs.Add(new ObstacleRun(x, y, width, height));
    }

    private static bool NearlyEqual(double first, double second) { return Math.Abs(first - second) < 1e-9d; }

    private static IReadOnlyList<SmoothingFigureSeries> BuildSeries(PathSmoothingComparisonResult comparison, PlanningGridMap map)
    {
        var series = new List<SmoothingFigureSeries>(4)
        {
            CreateSeries(comparison.RawPathBaseline, null, "raw", "原始粗路径", IeeeFigureStyle.RawColor, string.Empty, true, map)
        };
        series.Add(CreateSeries(Find(comparison, SmoothingMethod.CubicBSpline), SmoothingMethod.CubicBSpline, "bspline", "三次 B 样条", IeeeFigureStyle.BSplineColor, string.Empty, false, map));
        series.Add(CreateSeries(Find(comparison, SmoothingMethod.LocalCubicBezier), SmoothingMethod.LocalCubicBezier, "bezier", "局部三次 Bézier", IeeeFigureStyle.BezierColor, string.Empty, false, map));
        series.Add(CreateSeries(Find(comparison, SmoothingMethod.PiecewiseQuintic), SmoothingMethod.PiecewiseQuintic, "quintic", "分段五次", IeeeFigureStyle.QuinticColor, string.Empty, false, map));
        return series;
    }

    private static IReadOnlyList<SmoothingFigureMetricRow> BuildMetricRows(PathSmoothingComparisonResult comparison)
    {
        return new List<SmoothingFigureMetricRow>
        {
            CreateRow(comparison.RawPathBaseline, "RawPath", "原始粗路径"),
            CreateRow(Find(comparison, SmoothingMethod.CubicBSpline), "CubicBSpline", "三次 B 样条"),
            CreateRow(Find(comparison, SmoothingMethod.LocalCubicBezier), "LocalCubicBezier", "局部三次 Bézier"),
            CreateRow(Find(comparison, SmoothingMethod.PiecewiseQuintic), "PiecewiseQuintic", "分段五次")
        };
    }

    private static PathSmoothingComparisonEntry Find(PathSmoothingComparisonResult comparison, SmoothingMethod method)
    {
        for (int index = 0; index < comparison.Entries.Count; index++)
        {
            PathSmoothingComparisonEntry entry = comparison.Entries[index];
            if (entry.Method == method) return entry;
        }
        return null;
    }

    private static SmoothingFigureMetricRow CreateRow(PathSmoothingComparisonEntry entry, string method, string label)
    {
        return entry == null
            ? new SmoothingFigureMetricRow(method, label, PathSmoothingStatus.Failed, new PathQualityMetrics(), null, 0, 0d)
            : new SmoothingFigureMetricRow(method, label, entry.Status, entry.Metrics, entry.Timing, entry.RetryCount, entry.AcceptedStrength);
    }

    private static SmoothingFigureSeries CreateSeries(PathSmoothingComparisonEntry entry, SmoothingMethod? method, string key, string label,
        string color, string dashArray, bool isRawPathBaseline, PlanningGridMap map)
    {
        var points = new List<SmoothingFigurePoint>();
        var violations = new List<SmoothingFigurePoint>();
        PathSmoothingStatus status = entry == null ? PathSmoothingStatus.Failed : entry.Status;
        if (entry != null)
        {
            for (int index = 0; index < entry.Path.Count; index++)
            {
                SmoothedPathPoint point = entry.Path[index];
                var figurePoint = new SmoothingFigurePoint(point.X, point.Y, point.ArcLength, point.VehicleCurvature);
                points.Add(figurePoint);
                if (status == PathSmoothingStatus.Infeasible && map.IsOccupiedWorld(point.X, point.Y)) violations.Add(figurePoint);
            }
        }
        if (status == PathSmoothingStatus.Infeasible && points.Count > 0 && violations.Count == 0)
            violations.Add(points[points.Count / 2]);
        return new SmoothingFigureSeries(method, key, label, status, color, dashArray, isRawPathBaseline, points, violations);
    }

    private sealed class ObstacleRun
    {
        public ObstacleRun(double x, double y, double width, double height) { X = x; Y = y; Width = width; Height = height; }
        public double X { get; }
        public double Y { get; }
        public double Width { get; }
        public double Height { get; set; }
    }
}