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,50 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
+
+/// <summary>已校验、已按单一行驶方向分割并重采样的路径段。</summary>
+public sealed class PreparedDirectionSegment
+{
+    /// <summary>创建不可变方向段。</summary>
+    public PreparedDirectionSegment(
+        int segmentIndex,
+        TravelDirection direction,
+        IReadOnlyList<SmoothingPoint2D> points,
+        bool startsAtGearSwitch,
+        bool endsAtGearSwitch)
+    {
+        if (segmentIndex < 0) throw new ArgumentOutOfRangeException(nameof(segmentIndex));
+        if (points == null || points.Count == 0) throw new ArgumentException("A prepared segment requires points.", nameof(points));
+
+        SegmentIndex = segmentIndex;
+        Direction = direction;
+        Points = CopyReadOnly(points);
+        StartsAtGearSwitch = startsAtGearSwitch;
+        EndsAtGearSwitch = endsAtGearSwitch;
+    }
+
+    /// <summary>从零开始的分段序号；在 <see cref="PreparedPath.Segments"/> 中必须与其位置一致。</summary>
+    public int SegmentIndex { get; }
+
+    /// <summary>该段的唯一行驶方向。</summary>
+    public TravelDirection Direction { get; }
+
+    /// <summary>不包含相邻段点的本段不可变采样点。</summary>
+    public IReadOnlyList<SmoothingPoint2D> Points { get; }
+
+    /// <summary>本段首点是否为换向后保留的新方向点。</summary>
+    public bool StartsAtGearSwitch { get; }
+
+    /// <summary>本段末点之后是否紧邻换向点。</summary>
+    public bool EndsAtGearSwitch { get; }
+
+    private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)
+    {
+        var copy = new List<T>(source.Count);
+        for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
+        return new ReadOnlyCollection<T>(copy);
+    }
+}