feat: add path smoothing geometry foundation

2026-07-29 08:50:12 +08:00
parent 5cea617036
commit f75a3bae5e
8 changed files with 1332 additions and 0 deletions
@@ -0,0 +1,64 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing;
+
+namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
+
+/// <summary>同一几何分析器产生的路径、方向段和未验证质量统计。</summary>
+public sealed class PathGeometryAnalysis
+{
+    internal PathGeometryAnalysis(
+        IReadOnlyList<SmoothedPathPoint> path,
+        IReadOnlyList<SmoothedPathSegment> segments,
+        double pathLengthMeters,
+        double maximumAbsoluteVehicleCurvaturePerMeter,
+        double rootMeanSquareVehicleCurvaturePerMeter,
+        double totalAbsoluteCurvatureVariationPerMeter,
+        double curvatureVariationEnergy,
+        double minimumBodyClearanceMeters)
+    {
+        Path = CopyReadOnly(path);
+        Segments = CopyReadOnly(segments);
+        PathLengthMeters = pathLengthMeters;
+        MaximumAbsoluteVehicleCurvaturePerMeter = maximumAbsoluteVehicleCurvaturePerMeter;
+        RootMeanSquareVehicleCurvaturePerMeter = rootMeanSquareVehicleCurvaturePerMeter;
+        TotalAbsoluteCurvatureVariationPerMeter = totalAbsoluteCurvatureVariationPerMeter;
+        CurvatureVariationEnergy = curvatureVariationEnergy;
+        MinimumBodyClearanceMeters = minimumBodyClearanceMeters;
+    }
+
+    /// <summary>完成几何重计算的不可变路径。</summary>
+    public IReadOnlyList<SmoothedPathPoint> Path { get; }
+
+    /// <summary>完整覆盖 <see cref="Path"/> 的不可变方向段。</summary>
+    public IReadOnlyList<SmoothedPathSegment> Segments { get; }
+
+    /// <summary>路径总长度，单位 m。</summary>
+    public double PathLengthMeters { get; }
+
+    /// <summary>车辆曲率绝对值峰值，单位 1/m。</summary>
+    public double MaximumAbsoluteVehicleCurvaturePerMeter { get; }
+
+    /// <summary>车辆曲率均方根，单位 1/m。</summary>
+    public double RootMeanSquareVehicleCurvaturePerMeter { get; }
+
+    /// <summary>不跨换向点累计的绝对曲率变化，单位 1/m。</summary>
+    public double TotalAbsoluteCurvatureVariationPerMeter { get; }
+
+    /// <summary>不跨换向点累计的曲率变化能量。</summary>
+    public double CurvatureVariationEnergy { get; }
+
+    /// <summary>输入点携带的最小保守净空，单位 m。</summary>
+    public double MinimumBodyClearanceMeters { get; }
+
+    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);
+    }
+}