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,49 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
+
+/// <summary>已校验并按方向拆分的粗路径输入快照。</summary>
+public sealed class PreparedPath
+{
+    /// <summary>创建不可变预处理路径。</summary>
+    public PreparedPath(IReadOnlyList<PreparedDirectionSegment> segments)
+    {
+        if (segments == null || segments.Count == 0)
+            throw new ArgumentException("A prepared path requires direction segments.", nameof(segments));
+        for (int segmentIndex = 0; segmentIndex < segments.Count; segmentIndex++)
+        {
+            if (segments[segmentIndex] == null)
+                throw new ArgumentException("A prepared path cannot contain null direction segments.", nameof(segments));
+        }
+
+        Segments = CopyReadOnly(segments);
+        Points = Flatten(Segments);
+    }
+
+    /// <summary>按原始前进/倒车拓扑排列的方向段。</summary>
+    public IReadOnlyList<PreparedDirectionSegment> Segments { get; }
+
+    /// <summary>将所有方向段顺序拼接后的点；换向重复点保留两次。</summary>
+    public IReadOnlyList<SmoothingPoint2D> Points { 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);
+    }
+
+    private static IReadOnlyList<SmoothingPoint2D> Flatten(IReadOnlyList<PreparedDirectionSegment> segments)
+    {
+        var points = new List<SmoothingPoint2D>();
+        for (int segmentIndex = 0; segmentIndex < segments.Count; segmentIndex++)
+        {
+            PreparedDirectionSegment segment = segments[segmentIndex];
+            for (int pointIndex = 0; pointIndex < segment.Points.Count; pointIndex++)
+                points.Add(segment.Points[pointIndex]);
+        }
+        return new ReadOnlyCollection<SmoothingPoint2D>(points);
+    }
+}