feat: 发布 EM 轨迹规划首个版本

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/ReferenceHorizonSlicer.cs

@@ -6,8 +6,15 @@ using MultiWheelC.TrajectoryPlanning.Utils;
 
 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 
+/// <summary>
+/// 当前方向段内供一次 EM 优化使用的参考路径切片，保留全局参考站与切片局部弧长的对应关系。
+/// </summary>
 public sealed class ReferenceHorizonSlice
 {
+    /// <summary>
+    /// 创建一个以当前段起点为基准的参考地平线切片。
+    /// 参数：points 为局部 S（m）有序的参考点，terminalBoundary 必须属于 segment；返回：points 的防御性只读副本，空输入或边界不匹配会被拒绝。
+    /// </summary>
     public ReferenceHorizonSlice(DirectionSegmentView segment, IReadOnlyList<SmoothedPathPoint> points,
         ReferenceBoundary terminalBoundary)
     {
@@ -25,17 +32,38 @@ public sealed class ReferenceHorizonSlice
         TerminalBoundary = terminalBoundary;
     }
 
+    /// <summary>
+    /// 切片所属且不会跨越的单一方向段。
+    /// </summary>
     public DirectionSegmentView Segment { get; }
 
+    /// <summary>
+    /// 从段起点至精确终端点的参考点只读副本，局部 ArcLength 单位为 m。
+    /// </summary>
     public IReadOnlyList<SmoothedPathPoint> Points { get; }
 
+    /// <summary>
+    /// 切片末端的精确参考边界；中途截断时具有滚动安全停车语义。
+    /// </summary>
     public ReferenceBoundary TerminalBoundary { get; }
 }
 
+/// <summary>
+/// 按局部参考距离截取单一方向段，并在请求终端精确插值参考点。
+/// 切片只包含本段坐标；S、原始弧长与 X/Y 使用 m，航向使用 rad，终端附近以 1e-12 m 判定相等。
+/// </summary>
 public static class ReferenceHorizonSlicer
 {
+    /// <summary>
+    /// 端点复制、精确节点匹配和原段终点判定使用的局部 S 容差，单位 m。
+    /// </summary>
     private const double Epsilon = 1e-12d;
 
+    /// <summary>
+    /// 将方向段截取到请求的局部终端 S，并追加该终端的精确点。
+    /// 参数：requestedEndSegmentLocalS 为非负有限 m 制局部 S；超过段长的请求夹紧到段末。
+    /// 返回：段末保留原边界，中途截断创建 RollingSafetyStop 边界；空段或非法 S 会引发异常。
+    /// </summary>
     public static ReferenceHorizonSlice Slice(DirectionSegmentView segment, double requestedEndSegmentLocalS)
     {
         if (segment == null)
@@ -60,6 +88,10 @@ public static class ReferenceHorizonSlicer
         return new ReferenceHorizonSlice(segment, points, terminal);
     }
 
+    /// <summary>
+    /// 取得给定局部 S 的精确已有点或其相邻点之间的插值点。
+    /// 参数：terminalS 为已夹紧的 m 制局部 S；返回：与节点相差不超过 1e-12 m 时复用节点，否则在正跨度内线性插值。
+    /// </summary>
     private static SmoothedPathPoint GetExactTerminalPoint(DirectionSegmentView segment, double terminalS)
     {
         for (int index = 0; index < segment.Points.Count; index++)
@@ -76,6 +108,10 @@ public static class ReferenceHorizonSlicer
         return segment.Points[segment.Points.Count - 1];
     }
 
+    /// <summary>
+    /// 在线性弧长区间内插值一个终端平滑路径点。
+    /// 参数：lower/upper 的 ArcLength 为 m，localS 为其间的 m 制位置；返回：位置、展开航向、曲率和净空线性插值，标记为 Interpolated。
+    /// </summary>
     private static SmoothedPathPoint Interpolate(SmoothedPathPoint lower, SmoothedPathPoint upper, double localS)
     {
         double interval = upper.ArcLength - lower.ArcLength;
@@ -98,6 +134,10 @@ public static class ReferenceHorizonSlicer
             SmoothedPathPointSource.Interpolated);
     }
 
+    /// <summary>
+    /// 计算同单位标量的线性插值。
+    /// 参数：lower、upper 为端点，fraction 为无单位比例；返回：未夹紧的线性结果。
+    /// </summary>
     private static double Interpolate(double lower, double upper, double fraction)
     {
         return lower + (upper - lower) * fraction;