chore: save current workspace progress

2026-08-09 22:13:18 +08:00
parent 650c2ab0e3
commit 2f4fd15e52
449 changed files with 76593 additions and 971 deletions
@@ -0,0 +1,117 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Comparison;
+
+/// <summary>Immutable raw-baseline or Local G2 report entry.</summary>
+public sealed class PathSmoothingComparisonEntry
+{
+    public PathSmoothingComparisonEntry(
+        SmoothingMethod method,
+        PathSmoothingStatus status,
+        PathQualityMetrics metrics,
+        SmoothingTimingSummary timing,
+        string stableGeometryDigest,
+        string diagnostic)
+        : this(method, false, status, metrics, timing, stableGeometryDigest, diagnostic, Empty<SmoothedPathPoint>(), Empty<SmoothedPathSegment>())
+    {
+    }
+
+    private PathSmoothingComparisonEntry(
+        SmoothingMethod? method,
+        bool isRawPathBaseline,
+        PathSmoothingStatus status,
+        PathQualityMetrics metrics,
+        SmoothingTimingSummary timing,
+        string stableGeometryDigest,
+        string diagnostic,
+        IReadOnlyList<SmoothedPathPoint> path,
+        IReadOnlyList<SmoothedPathSegment> segments)
+    {
+        Method = method;
+        IsRawPathBaseline = isRawPathBaseline;
+        Status = status;
+        Metrics = metrics ?? new PathQualityMetrics();
+        Timing = timing ?? new SmoothingTimingSummary(new double[] { 0d, 0d, 0d, 0d, 0d }, false, "No timing result was supplied.");
+        StableGeometryDigest = stableGeometryDigest ?? string.Empty;
+        Diagnostic = diagnostic ?? string.Empty;
+        Path = CopyReadOnly(path);
+        Segments = CopyReadOnly(segments);
+    }
+
+    public SmoothingMethod? Method { get; }
+
+    public bool IsRawPathBaseline { get; }
+
+    public PathSmoothingStatus Status { get; }
+
+    public PathQualityMetrics Metrics { get; }
+
+    public SmoothingTimingSummary Timing { get; }
+
+    public string StableGeometryDigest { get; }
+
+    public string Diagnostic { get; }
+
+    public IReadOnlyList<SmoothedPathPoint> Path { get; }
+
+    public IReadOnlyList<SmoothedPathSegment> Segments { get; }
+
+    public bool IsEligibleForRecommendation =>
+        !IsRawPathBaseline &&
+        IsPublishedLocalG2Status(Status) &&
+        Metrics.IsFeasible &&
+        Timing.IsDeterministic;
+
+    internal static bool IsPublishedLocalG2Status(PathSmoothingStatus status)
+    {
+        return status == PathSmoothingStatus.Complete ||
+            status == PathSmoothingStatus.PartialImprovement ||
+            status == PathSmoothingStatus.NotNeeded ||
+            status == PathSmoothingStatus.Unchanged;
+    }
+
+    internal static PathSmoothingComparisonEntry CreateCandidate(
+        SmoothingMethod method,
+        PathSmoothingStatus status,
+        PathQualityMetrics metrics,
+        SmoothingTimingSummary timing,
+        string stableGeometryDigest,
+        string diagnostic,
+        IReadOnlyList<SmoothedPathPoint> path,
+        IReadOnlyList<SmoothedPathSegment> segments)
+    {
+        return new PathSmoothingComparisonEntry(
+            method, false, status, metrics, timing, stableGeometryDigest, diagnostic, path, segments);
+    }
+
+    internal static PathSmoothingComparisonEntry CreateRawPathBaseline(
+        PathSmoothingStatus status,
+        PathQualityMetrics metrics,
+        string stableGeometryDigest,
+        string diagnostic,
+        IReadOnlyList<SmoothedPathPoint> path,
+        IReadOnlyList<SmoothedPathSegment> segments)
+    {
+        return new PathSmoothingComparisonEntry(
+            null, true, status, metrics,
+            new SmoothingTimingSummary(new double[] { 0d, 0d, 0d, 0d, 0d }, true, string.Empty),
+            stableGeometryDigest, diagnostic, path, segments);
+    }
+
+    private static IReadOnlyList<T> Empty<T>()
+    {
+        return new ReadOnlyCollection<T>(new List<T>());
+    }
+
+    private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)
+    {
+        var copy = new List<T>(source == null ? 0 : source.Count);
+        if (source != null)
+        {
+            for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
+        }
+        return new ReadOnlyCollection<T>(copy);
+    }
+}
@@ -0,0 +1,33 @@
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Comparison;
+
+/// <summary>Immutable request for comparing a coarse-path baseline with Local G2 output.</summary>
+public sealed class PathSmoothingComparisonRequest
+{
+    private static readonly IReadOnlyList<SmoothingMethod> LocalG2OnlyMethods =
+        new ReadOnlyCollection<SmoothingMethod>(new[] { SmoothingMethod.LocalG2Quintic });
+
+    public PathSmoothingComparisonRequest(PathSmoothingRequest smoothingRequest)
+    {
+        SmoothingRequest = CopyRequest(smoothingRequest);
+        Methods = LocalG2OnlyMethods;
+    }
+
+    public PathSmoothingRequest SmoothingRequest { get; }
+
+    /// <summary>The report always contains the sole supported method, Local G2 quintic.</summary>
+    public IReadOnlyList<SmoothingMethod> Methods { get; }
+
+    private static PathSmoothingRequest CopyRequest(PathSmoothingRequest source)
+    {
+        if (source == null) return null;
+        return new PathSmoothingRequest(
+            source.CoarsePath,
+            source.Segments,
+            source.Map,
+            source.Vehicle,
+            source.Configuration);
+    }
+}
@@ -0,0 +1,45 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Comparison;
+
+/// <summary>Immutable raw-path and Local G2 comparison result.</summary>
+public sealed class PathSmoothingComparisonResult
+{
+    internal PathSmoothingComparisonResult(
+        PathSmoothingComparisonEntry rawPathBaseline,
+        IReadOnlyList<PathSmoothingComparisonEntry> entries,
+        SmoothingMethod? recommendedMethod,
+        bool isCancelled,
+        string diagnostic)
+    {
+        RawPathBaseline = rawPathBaseline ?? throw new ArgumentNullException(nameof(rawPathBaseline));
+        Entries = CopyReadOnly(entries);
+        RecommendedMethod = recommendedMethod;
+        IsCancelled = isCancelled;
+        Diagnostic = diagnostic ?? string.Empty;
+    }
+
+    public PathSmoothingComparisonEntry RawPathBaseline { get; }
+
+    public IReadOnlyList<PathSmoothingComparisonEntry> Entries { get; }
+
+    /// <summary>Local G2 when its published result is deterministic; otherwise null.</summary>
+    public SmoothingMethod? RecommendedMethod { get; }
+
+    public bool IsCancelled { get; }
+
+    public string Diagnostic { get; }
+
+    private static IReadOnlyList<PathSmoothingComparisonEntry> CopyReadOnly(
+        IReadOnlyList<PathSmoothingComparisonEntry> source)
+    {
+        var copy = new List<PathSmoothingComparisonEntry>(source == null ? 0 : source.Count);
+        if (source != null)
+        {
+            for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
+        }
+        return new ReadOnlyCollection<PathSmoothingComparisonEntry>(copy);
+    }
+}
@@ -0,0 +1,90 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Comparison;
+
+/// <summary>一个方法的固定五次计时样本和确定性结论。</summary>
+public sealed class SmoothingTimingSummary
+{
+    /// <summary>创建一个只接受五个已测量样本的计时汇总。</summary>
+    public SmoothingTimingSummary(
+        IReadOnlyList<double> measuredElapsedMilliseconds,
+        bool isDeterministic,
+        string diagnostic)
+    {
+        if (measuredElapsedMilliseconds == null || measuredElapsedMilliseconds.Count != 5)
+            throw new ArgumentException("计时汇总必须包含五个已测量样本。", nameof(measuredElapsedMilliseconds));
+
+        var copy = new List<double>(measuredElapsedMilliseconds.Count);
+        for (int index = 0; index < measuredElapsedMilliseconds.Count; index++)
+        {
+            double value = measuredElapsedMilliseconds[index];
+            if (double.IsNaN(value) || double.IsInfinity(value) || value < 0d)
+                throw new ArgumentOutOfRangeException(nameof(measuredElapsedMilliseconds), "计时样本必须为有限非负数。");
+            copy.Add(value);
+        }
+
+        MeasuredElapsedMilliseconds = new ReadOnlyCollection<double>(copy);
+        MedianElapsedMilliseconds = Median(copy);
+        IsDeterministic = isDeterministic;
+        Diagnostic = diagnostic ?? string.Empty;
+    }
+
+    /// <summary>不含预热执行的五个实测耗时，单位 ms。</summary>
+    public IReadOnlyList<double> MeasuredElapsedMilliseconds { get; }
+
+    /// <summary>五个实测耗时的稳定中位数，单位 ms。</summary>
+    public double MedianElapsedMilliseconds { get; }
+
+    /// <summary>五次输出是否具有相同的状态和稳定几何摘要。</summary>
+    public bool IsDeterministic { get; }
+
+    /// <summary>非确定性或测量失败的稳定诊断说明。</summary>
+    public string Diagnostic { get; }
+
+    /// <summary>从五次测量结果中生成计时汇总，并拒绝状态或几何不稳定的输出。</summary>
+    public static SmoothingTimingSummary FromMeasurements(
+        IReadOnlyList<double> measuredElapsedMilliseconds,
+        IReadOnlyList<PathSmoothingResult> measuredResults)
+    {
+        if (measuredResults == null || measuredResults.Count != 5)
+            throw new ArgumentException("确定性检查必须包含五个测量结果。", nameof(measuredResults));
+        for (int index = 0; index < measuredResults.Count; index++)
+        {
+            if (measuredResults[index] == null)
+                throw new ArgumentException("确定性检查不能包含空测量结果。", nameof(measuredResults));
+        }
+
+        PathSmoothingResult canonical = measuredResults[0];
+        string canonicalDigest = StableGeometryDigest.Compute(canonical);
+        for (int index = 1; index < measuredResults.Count; index++)
+        {
+            PathSmoothingResult measured = measuredResults[index];
+            string digest = StableGeometryDigest.Compute(measured);
+            if (measured.Status != canonical.Status ||
+                measured.Path.Count != canonical.Path.Count ||
+                measured.Segments.Count != canonical.Segments.Count ||
+                !string.Equals(digest, canonicalDigest, StringComparison.Ordinal))
+            {
+                return new SmoothingTimingSummary(
+                    measuredElapsedMilliseconds,
+                    false,
+                    "五次测量的状态、点数、分段数或稳定几何摘要不一致。");
+            }
+        }
+
+        return new SmoothingTimingSummary(measuredElapsedMilliseconds, true, string.Empty);
+    }
+
+    internal static double Median(IReadOnlyList<double> values)
+    {
+        var sorted = new double[values.Count];
+        for (int index = 0; index < values.Count; index++) sorted[index] = values[index];
+        Array.Sort(sorted);
+        int middle = sorted.Length / 2;
+        return sorted.Length % 2 == 1
+            ? sorted[middle]
+            : (sorted[middle - 1] + sorted[middle]) / 2d;
+    }
+}
@@ -0,0 +1,111 @@
+using System;
+using System.Collections.Generic;
+using System.IO;
+using System.Security.Cryptography;
+using System.Text;
+
+namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Output.Comparison;
+
+/// <summary>为重复执行结果生成与进程无关的稳定几何 SHA-256 摘要。</summary>
+public static class StableGeometryDigest
+{
+    /// <summary>计算正式平滑结果的状态、方法、分段和路径位模式摘要。</summary>
+    public static string Compute(PathSmoothingResult result)
+    {
+        if (result == null) throw new ArgumentNullException(nameof(result));
+        return Compute(result.Status, result.Method, result.Path, result.Segments);
+    }
+
+    /// <summary>计算任意已分析路径的稳定摘要。</summary>
+    public static string Compute(
+        PathSmoothingStatus status,
+        SmoothingMethod? method,
+        IReadOnlyList<SmoothedPathPoint> path,
+        IReadOnlyList<SmoothedPathSegment> segments)
+    {
+        using (var stream = new MemoryStream())
+        {
+            WriteInt32(stream, 1);
+            WriteInt32(stream, (int)status);
+            WriteBoolean(stream, method.HasValue);
+            if (method.HasValue) WriteInt32(stream, (int)method.Value);
+
+            WriteInt32(stream, path == null ? 0 : path.Count);
+            if (path != null)
+            {
+                for (int index = 0; index < path.Count; index++) WritePoint(stream, path[index]);
+            }
+
+            WriteInt32(stream, segments == null ? 0 : segments.Count);
+            if (segments != null)
+            {
+                for (int index = 0; index < segments.Count; index++) WriteSegment(stream, segments[index]);
+            }
+
+            using (SHA256 sha256 = SHA256.Create())
+            {
+                byte[] hash = sha256.ComputeHash(stream.ToArray());
+                var builder = new StringBuilder(hash.Length * 2);
+                for (int index = 0; index < hash.Length; index++) builder.Append(hash[index].ToString("x2"));
+                return builder.ToString();
+            }
+        }
+    }
+
+    private static void WritePoint(Stream stream, SmoothedPathPoint point)
+    {
+        if (point == null) throw new ArgumentException("稳定路径摘要不能包含空点。", nameof(point));
+        WriteDouble(stream, point.X);
+        WriteDouble(stream, point.Y);
+        WriteDouble(stream, point.Heading);
+        WriteDouble(stream, point.UnwrappedHeading);
+        WriteDouble(stream, point.ArcLength);
+        WriteInt32(stream, (int)point.Direction);
+        WriteDouble(stream, point.GeometricCurvature);
+        WriteDouble(stream, point.VehicleCurvature);
+        WriteDouble(stream, point.BodyClearance);
+        WriteBoolean(stream, point.IsGearSwitchPoint);
+        WriteInt32(stream, (int)point.Source);
+    }
+
+    private static void WriteSegment(Stream stream, SmoothedPathSegment segment)
+    {
+        if (segment == null) throw new ArgumentException("稳定路径摘要不能包含空方向段。", nameof(segment));
+        WriteInt32(stream, segment.SegmentIndex);
+        WriteInt32(stream, (int)segment.Direction);
+        WriteInt32(stream, segment.StartIndex);
+        WriteInt32(stream, segment.EndIndex);
+        WriteBoolean(stream, segment.StartsAtGearSwitch);
+        WriteBoolean(stream, segment.EndsAtGearSwitch);
+    }
+
+    private static void WriteDouble(Stream stream, double value)
+    {
+        WriteInt64(stream, BitConverter.DoubleToInt64Bits(value));
+    }
+
+    private static void WriteBoolean(Stream stream, bool value)
+    {
+        stream.WriteByte(value ? (byte)1 : (byte)0);
+    }
+
+    private static void WriteInt32(Stream stream, int value)
+    {
+        unchecked
+        {
+            stream.WriteByte((byte)value);
+            stream.WriteByte((byte)(value >> 8));
+            stream.WriteByte((byte)(value >> 16));
+            stream.WriteByte((byte)(value >> 24));
+        }
+    }
+
+    private static void WriteInt64(Stream stream, long value)
+    {
+        unchecked
+        {
+            ulong bits = (ulong)value;
+            for (int index = 0; index < 8; index++) stream.WriteByte((byte)(bits >> (index * 8)));
+        }
+    }
+}