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

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Frenet/FrenetProjector.cs

@@ -4,11 +4,21 @@ using MultiWheelC.TrajectoryPlanning.Utils;
 
 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 
-/// <summary>Deterministically projects a world pose only inside the supplied direction segment and S window.</summary>
+/// <summary>
+/// 在指定方向段及局部 S 窗口内确定性地投影世界位姿。
+/// 投影不跨越换向边界；世界 X/Y 与距离为 m，航向为 rad，并以种子 S 消除等距候选的拓扑歧义。
+/// </summary>
 public sealed class FrenetProjector
 {
+    /// <summary>
+    /// 等距候选比较和零长度线段识别使用的 S/距离平方数值容差 1e-14。
+    /// </summary>
     private const double TieTolerance = 1e-14d;
 
+    /// <summary>
+    /// 在窗口内投影世界位姿，并以窗口起点作为等距候选的种子 S。
+    /// 参数：窗口与 maximumDistanceMeters 均为当前段局部 m 制距离；返回：命中距离不超过阈值时返回 true，否则 projection 为 null。
+    /// </summary>
     public bool TryProject(Pose2D worldPose, DirectionSegmentView segment, double minimumReferenceS,
         double maximumReferenceS, double maximumDistanceMeters, out FrenetProjection projection)
     {
@@ -16,6 +26,10 @@ public sealed class FrenetProjector
             minimumReferenceS, out projection);
     }
 
+    /// <summary>
+    /// 在窗口内投影世界位姿，按距离、种子 S 距离及较小 S 的顺序稳定选择候选。
+    /// 参数：worldPose 使用世界 X/Y（m）和车身航向（rad），S 窗口、距离阈值和种子均为 m；返回：窗口非法、无候选或候选超阈值时返回 false。
+    /// </summary>
     public bool TryProject(Pose2D worldPose, DirectionSegmentView segment, double minimumReferenceS,
         double maximumReferenceS, double maximumDistanceMeters, double seedReferenceS, out FrenetProjection projection)
     {
@@ -63,6 +77,10 @@ public sealed class FrenetProjector
         return true;
     }
 
+    /// <summary>
+    /// 考察被 S 窗口裁剪后的参考折线边，并将世界点的正交投影加入最优候选比较。
+    /// 参数：端点为世界 X/Y（m）和局部 S（m），seedReferenceS 为 m；零长度（不超过 1e-14）边退化为两个端点比较。
+    /// </summary>
     private static void ConsiderLine(Pose2D worldPose, FrenetReferencePoint start, FrenetReferencePoint end,
         double seedReferenceS, ref Candidate best)
     {
@@ -84,12 +102,20 @@ public sealed class FrenetProjector
         Consider(worldPose, referenceS, x, y, seedReferenceS, ref best);
     }
 
+    /// <summary>
+    /// 将一个离散参考点作为投影候选参与比较。
+    /// 参数：point 的位置为世界 m 制坐标，seedReferenceS 为局部 m 制种子；结果通过 best 原位更新。
+    /// </summary>
     private static void ConsiderPoint(Pose2D worldPose, FrenetReferencePoint point, double seedReferenceS,
         ref Candidate best)
     {
         Consider(worldPose, point.ReferenceS, point.X, point.Y, seedReferenceS, ref best);
     }
 
+    /// <summary>
+    /// 依据世界平面平方距离登记候选，并按确定性优先级替换当前最佳值。
+    /// 参数：referenceS、x、y 与 seedReferenceS 分别为局部 S（m）和世界坐标（m）；平方距离由内部计算，单位 m²。
+    /// </summary>
     private static void Consider(Pose2D worldPose, double referenceS, double x, double y, double seedReferenceS,
         ref Candidate best)
     {
@@ -101,13 +127,25 @@ public sealed class FrenetProjector
             best = candidate;
     }
 
+    /// <summary>
+    /// 判定投影计算的标量是否有限。
+    /// 参数：value 为任意实数；返回：NaN 与无穷均返回 false。
+    /// </summary>
     private static bool IsFinite(double value)
     {
         return !double.IsNaN(value) && !double.IsInfinity(value);
     }
 
+    /// <summary>
+    /// 保存一个待比较的折线投影候选。
+    /// ReferenceS 和 SeedDistance 使用 m，SquaredDistanceMeters 使用 m²，优先级由距离、种子距离及较小 S 依次决定。
+    /// </summary>
     private sealed class Candidate
     {
+        /// <summary>
+        /// 创建投影候选。
+        /// 参数：referenceS 与 seedDistance 为 m，squaredDistanceMeters 为 m²；调用方仅传入有限的已计算值。
+        /// </summary>
         public Candidate(double referenceS, double squaredDistanceMeters, double seedDistance)
         {
             ReferenceS = referenceS;
@@ -115,10 +153,25 @@ public sealed class FrenetProjector
             SeedDistance = seedDistance;
         }
 
+        /// <summary>
+        /// 候选在当前方向段的局部参考弧长 S，单位 m。
+        /// </summary>
         public double ReferenceS { get; }
+
+        /// <summary>
+        /// 候选世界位置到车辆位置的距离平方，单位 m²。
+        /// </summary>
         public double SquaredDistanceMeters { get; }
+
+        /// <summary>
+        /// 候选 S 与调用方种子 S 的绝对距离，单位 m。
+        /// </summary>
         public double SeedDistance { get; }
 
+        /// <summary>
+        /// 比较两个候选的稳定优先级。
+        /// 参数：other 为非空候选；返回：平方距离差超过 1e-14 时取较小者，随后取较近种子，仍相等时取较小 S。
+        /// </summary>
         public bool IsPreferredTo(Candidate other)
         {
             if (SquaredDistanceMeters < other.SquaredDistanceMeters - TieTolerance) return true;
@@ -129,3 +182,7 @@ public sealed class FrenetProjector
         }
     }
 }
+    /// <summary>
+    /// 在窗口内投影世界位姿，按距离、种子 S 距离及较小 S 的顺序稳定选择候选。
+    /// 参数：worldPose 使用世界 X/Y（m）和车身航向（rad），S 窗口、距离阈值和种子均为 m；返回：窗口非法、无候选或候选超阈值时返回 false。
+    /// </summary>