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,53 @@
+using System;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing;
+
+namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
+
+/// <summary>供平滑算法处理的二维路径采样点；所有长度单位均为 m，航向单位为 rad。</summary>
+public sealed class SmoothingPoint2D
+{
+    /// <summary>创建不可变二维路径点。</summary>
+    public SmoothingPoint2D(
+        double xMeters,
+        double yMeters,
+        double arcLengthMeters,
+        double headingRadians,
+        double unwrappedHeadingRadians,
+        double bodyClearanceMeters,
+        bool isGearSwitchPoint,
+        SmoothedPathPointSource source)
+    {
+        X = xMeters;
+        Y = yMeters;
+        ArcLength = arcLengthMeters;
+        Heading = headingRadians;
+        UnwrappedHeading = unwrappedHeadingRadians;
+        BodyClearance = bodyClearanceMeters;
+        IsGearSwitchPoint = isGearSwitchPoint;
+        Source = source;
+    }
+
+    /// <summary>世界 X 坐标，单位 m。</summary>
+    public double X { get; }
+
+    /// <summary>世界 Y 坐标，单位 m。</summary>
+    public double Y { get; }
+
+    /// <summary>本方向段中的累计弧长，单位 m。</summary>
+    public double ArcLength { get; }
+
+    /// <summary>归一化的车辆航向，单位 rad。</summary>
+    public double Heading { get; }
+
+    /// <summary>连续展开的车辆航向，单位 rad。</summary>
+    public double UnwrappedHeading { get; }
+
+    /// <summary>输入路径携带的保守净空，单位 m。</summary>
+    public double BodyClearance { get; }
+
+    /// <summary>该点是否为新方向段开始处的换向点。</summary>
+    public bool IsGearSwitchPoint { get; }
+
+    /// <summary>该点在平滑流程中的来源。</summary>
+    public SmoothedPathPointSource Source { get; }
+}