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,69 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MultiWheelC.TrajectoryPlanning.CoarsePath.Search;
+
+/// <summary>
+/// 一条经连续碰撞检查的恒曲率运动原语。
+/// 位置和长度单位为 m，航向单位为 rad，曲率单位为 1/m；<see cref="Points"/> 不包含起点，只包含按积分顺序产生的后续点。
+/// </summary>
+public sealed class MotionPrimitive
+{
+    /// <summary>
+    /// 创建不可变运动原语。
+    /// 参数：start 为原语起点；direction 为行驶方向；curvaturePerMeter 为恒定曲率；actualLengthMeters 为已实际行驶长度；
+    /// points 与 bodyClearancesMeters 按同一索引保存内部积分位姿和对应的车体净空；isGoalTruncation 表示末点是否首次命中目标容差。
+    /// </summary>
+    public MotionPrimitive(
+        Pose2D start,
+        TravelDirection direction,
+        double curvaturePerMeter,
+        double actualLengthMeters,
+        IEnumerable<Pose2D> points,
+        IEnumerable<double> bodyClearancesMeters,
+        bool isGoalTruncation)
+    {
+        if (start == null) throw new ArgumentNullException(nameof(start));
+        if (points == null) throw new ArgumentNullException(nameof(points));
+        if (bodyClearancesMeters == null) throw new ArgumentNullException(nameof(bodyClearancesMeters));
+
+        var copiedPoints = new List<Pose2D>(points);
+        var copiedClearances = new List<double>(bodyClearancesMeters);
+        if (copiedPoints.Count != copiedClearances.Count)
+            throw new ArgumentException("The point and clearance counts must match.", nameof(bodyClearancesMeters));
+
+        Start = start;
+        Direction = direction;
+        CurvaturePerMeter = curvaturePerMeter;
+        ActualLengthMeters = actualLengthMeters;
+        Points = new ReadOnlyCollection<Pose2D>(copiedPoints);
+        BodyClearancesMeters = new ReadOnlyCollection<double>(copiedClearances);
+        End = copiedPoints.Count == 0 ? start : copiedPoints[copiedPoints.Count - 1];
+        IsGoalTruncation = isGoalTruncation;
+    }
+
+    /// <summary>原语起始车辆几何中心位姿。</summary>
+    public Pose2D Start { get; }
+
+    /// <summary>原语最后一个积分点；零长度终点候选时等于 <see cref="Start"/>。</summary>
+    public Pose2D End { get; }
+
+    /// <summary>原语对应的行驶方向。</summary>
+    public TravelDirection Direction { get; }
+
+    /// <summary>原语全程采用的恒定车辆曲率，单位 1/m。</summary>
+    public double CurvaturePerMeter { get; }
+
+    /// <summary>从 <see cref="Start"/> 到 <see cref="End"/> 的实际行驶弧长，单位 m。</summary>
+    public double ActualLengthMeters { get; }
+
+    /// <summary>不含起点的连续积分位姿，只读且按行驶顺序排列。</summary>
+    public IReadOnlyList<Pose2D> Points { get; }
+
+    /// <summary>与 <see cref="Points"/> 一一对应的扩大车体保守净空下界，单位 m。</summary>
+    public IReadOnlyList<double> BodyClearancesMeters { get; }
+
+    /// <summary>末点是否因首次满足目标位置、航向和方向约束而截断。</summary>
+    public bool IsGoalTruncation { get; }
+}