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

using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Visualization;

/// <summary>从一份比较模型构建六张含义固定、相机聚焦的报告图。</summary>
public sealed class SmoothingFigureSetBuilder
{
    private const double MinimumExtentMeters = 0.25d;
    private const double PaddingFraction = 0.10d;

    public SmoothingFigureSet Build(SmoothingFigureModel model)
    {
        if (model == null) throw new ArgumentNullException(nameof(model));
        SmoothingFigureSeries raw = Find(model, "raw");
        SmoothingFigureSeries bSpline = Find(model, "bspline");
        SmoothingFigureSeries bezier = Find(model, "bezier");
        SmoothingFigureSeries quintic = Find(model, "quintic");
        var figures = new List<SmoothingFigureDefinition>
        {
            BuildOverhead(SmoothingFigureKind.CoarsePathOverview, "01-coarse-path-overview", "Hybrid A* 原始粗路径", model, true, View(raw, 1d)),
            BuildOverhead(SmoothingFigureKind.AllPathsComparison, "02-all-paths-comparison", "路径平滑结果对比", model, false, View(raw, 1d), View(bSpline, 1d), View(bezier, 1d), View(quintic, 1d)),
            BuildOverhead(SmoothingFigureKind.CubicBSplineOverview, "03-cubic-bspline-overview", "三次 B 样条平滑", model, true, View(raw, 0.28d), View(bSpline, 1d)),
            BuildOverhead(SmoothingFigureKind.LocalCubicBezierOverview, "04-local-cubic-bezier-overview", "局部三次 Bézier 平滑", model, true, View(raw, 0.28d), View(bezier, 1d)),
            BuildOverhead(SmoothingFigureKind.PiecewiseQuinticOverview, "05-piecewise-quintic-overview", "分段五次平滑", model, true, View(raw, 0.28d), View(quintic, 1d)),
            BuildCurvature(model, View(raw, 1d), View(bSpline, 1d), View(bezier, 1d), View(quintic, 1d)),
        };
        return new SmoothingFigureSet(figures);
    }

    private static SmoothingFigureDefinition BuildOverhead(SmoothingFigureKind kind, string stem, string title, SmoothingFigureModel model, bool mapContext, params SmoothingFigureSeriesView[] series)
    {
        Bounds bounds = CalculateWorldBounds(model, mapContext, series);
        return new SmoothingFigureDefinition(kind, stem, title, model, mapContext, series,
            bounds.XMin, bounds.XMax, bounds.YMin, bounds.YMax,
            BuildTicks(bounds.XMin, bounds.XMax), BuildTicks(bounds.YMin, bounds.YMax),
            1d, -1d, 1d, Array.Empty<double>(), Array.Empty<double>());
    }

    private static SmoothingFigureDefinition BuildCurvature(SmoothingFigureModel model, params SmoothingFigureSeriesView[] series)
    {
        double arcMaximum = 0d;
        double curvatureMinimum = 0d;
        double curvatureMaximum = 0d;
        for (int viewIndex = 0; viewIndex < series.Length; viewIndex++)
        {
            IReadOnlyList<SmoothingFigurePoint> points = series[viewIndex].Series.Points;
            for (int pointIndex = 0; pointIndex < points.Count; pointIndex++)
            {
                SmoothingFigurePoint point = points[pointIndex];
                if (point.ArcLength > arcMaximum) arcMaximum = point.ArcLength;
                if (point.VehicleCurvature < curvatureMinimum) curvatureMinimum = point.VehicleCurvature;
                if (point.VehicleCurvature > curvatureMaximum) curvatureMaximum = point.VehicleCurvature;
            }
        }
        arcMaximum = ExpandMaximum(arcMaximum, MinimumExtentMeters);
        ExpandRange(ref curvatureMinimum, ref curvatureMaximum, MinimumExtentMeters);
        return new SmoothingFigureDefinition(SmoothingFigureKind.CurvatureComparison, "06-curvature-comparison", "车辆曲率对比", model, false, series,
            0d, 1d, 0d, 1d, Array.Empty<double>(), Array.Empty<double>(),
            arcMaximum, curvatureMinimum, curvatureMaximum, BuildTicks(0d, arcMaximum), BuildTicks(curvatureMinimum, curvatureMaximum));
    }

    private static Bounds CalculateWorldBounds(SmoothingFigureModel model, bool includesEndpoints, IReadOnlyList<SmoothingFigureSeriesView> series)
    {
        bool hasPoint = false;
        double xMin = 0d, xMax = 0d, yMin = 0d, yMax = 0d;
        for (int viewIndex = 0; viewIndex < series.Count; viewIndex++)
        {
            IReadOnlyList<SmoothingFigurePoint> points = series[viewIndex].Series.Points;
            for (int pointIndex = 0; pointIndex < points.Count; pointIndex++) Include(points[pointIndex].X, points[pointIndex].Y, ref hasPoint, ref xMin, ref xMax, ref yMin, ref yMax);
        }
        if (includesEndpoints)
        {
            Include(model.Start.X, model.Start.Y, ref hasPoint, ref xMin, ref xMax, ref yMin, ref yMax);
            Include(model.Goal.X, model.Goal.Y, ref hasPoint, ref xMin, ref xMax, ref yMin, ref yMax);
        }
        if (!hasPoint)
        {
            xMin = model.WorldXMinMeters; xMax = model.WorldXMaxMeters;
            yMin = model.WorldYMinMeters; yMax = model.WorldYMaxMeters;
        }
        ExpandRange(ref xMin, ref xMax, MinimumExtentMeters);
        ExpandRange(ref yMin, ref yMax, MinimumExtentMeters);
        double xPadding = (xMax - xMin) * PaddingFraction;
        double yPadding = (yMax - yMin) * PaddingFraction;
        xMin -= xPadding; xMax += xPadding; yMin -= yPadding; yMax += yPadding;
        double desiredAspect = 400d / 245d;
        double width = xMax - xMin;
        double height = yMax - yMin;
        if (width / height < desiredAspect)
        {
            double halfWidth = height * desiredAspect / 2d;
            double center = (xMin + xMax) / 2d;
            xMin = center - halfWidth; xMax = center + halfWidth;
        }
        else
        {
            double halfHeight = width / desiredAspect / 2d;
            double center = (yMin + yMax) / 2d;
            yMin = center - halfHeight; yMax = center + halfHeight;
        }
        return new Bounds(xMin, xMax, yMin, yMax);
    }

    private static void Include(double x, double y, ref bool hasPoint, ref double xMin, ref double xMax, ref double yMin, ref double yMax)
    {
        if (!hasPoint) { hasPoint = true; xMin = xMax = x; yMin = yMax = y; return; }
        if (x < xMin) xMin = x;
        if (x > xMax) xMax = x;
        if (y < yMin) yMin = y;
        if (y > yMax) yMax = y;
    }

    private static void ExpandRange(ref double minimum, ref double maximum, double minimumExtent)
    {
        double extent = maximum - minimum;
        if (extent >= minimumExtent) return;
        double center = (minimum + maximum) / 2d;
        minimum = center - minimumExtent / 2d;
        maximum = center + minimumExtent / 2d;
    }

    private static double ExpandMaximum(double value, double minimum) { return value < minimum ? minimum : value * (1d + PaddingFraction); }

    private static IReadOnlyList<double> BuildTicks(double minimum, double maximum)
    {
        double span = maximum - minimum;
        if (span <= 0d) return new[] { minimum, maximum };
        double roughStep = span / 5d;
        double magnitude = Math.Pow(10d, Math.Floor(Math.Log10(roughStep)));
        double normalized = roughStep / magnitude;
        double nice = normalized <= 1d ? 1d : (normalized <= 2d ? 2d : (normalized <= 5d ? 5d : 10d));
        double step = nice * magnitude;
        var ticks = new List<double>();
        double first = Math.Ceiling(minimum / step) * step;
        for (double value = first; value <= maximum + step * 0.001d; value += step) ticks.Add(value);
        if (ticks.Count < 2) { ticks.Clear(); ticks.Add(minimum); ticks.Add(maximum); }
        return new ReadOnlyCollection<double>(ticks);
    }

    private static SmoothingFigureSeries Find(SmoothingFigureModel model, string key)
    {
        for (int index = 0; index < model.Series.Count; index++) if (model.Series[index].Key == key) return model.Series[index];
        throw new InvalidOperationException("比较图形模型缺少路径序列: " + key);
    }

    private static SmoothingFigureSeriesView View(SmoothingFigureSeries series, double opacity) { return new SmoothingFigureSeriesView(series, opacity); }

    private readonly struct Bounds
    {
        public Bounds(double xMin, double xMax, double yMin, double yMax) { XMin = xMin; XMax = xMax; YMin = yMin; YMax = yMax; }
        public double XMin { get; }
        public double XMax { get; }
        public double YMin { get; }
        public double YMax { get; }
    }
}

/// <summary>六张图的稳定顺序。</summary>
public sealed class SmoothingFigureSet
{
    internal SmoothingFigureSet(IReadOnlyList<SmoothingFigureDefinition> figures)
    {
        var copy = new List<SmoothingFigureDefinition>(figures == null ? 0 : figures.Count);
        if (figures != null) for (int index = 0; index < figures.Count; index++) copy.Add(figures[index]);
        Figures = new ReadOnlyCollection<SmoothingFigureDefinition>(copy);
    }

    public IReadOnlyList<SmoothingFigureDefinition> Figures { get; }
}