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

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Frenet/ReferencePathInterpolator.cs

@@ -4,11 +4,22 @@ using MultiWheelC.TrajectoryPlanning.Utils;
 
 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 
-/// <summary>Interpolates reference geometry inside exactly one direction segment.</summary>
+/// <summary>
+/// 在恰好一个方向段内按局部参考弧长插值参考几何。
+/// 输入和输出 S、X/Y 与净空均为 m，航向为 rad，曲率为 1/m；插值不会越过方向段边界。
+/// </summary>
 public static class ReferencePathInterpolator
 {
+    /// <summary>
+    /// 接受端点轻微 S 超差并判断精确节点的数值容差，单位 m。
+    /// </summary>
     private const double Epsilon = 1e-12d;
 
+    /// <summary>
+    /// 为给定局部 S 生成方向一致的 Frenet 参考样本。
+    /// 参数：segment 不可为空，referenceS 为 m；[-1e-12, Length+1e-12] 内的端点超差会夹紧到边界。
+    /// 返回：精确点直接转换，区间内对位置、展开航向和几何量线性插值；超界或退化跨度会引发异常。
+    /// </summary>
     public static FrenetReferencePoint Interpolate(DirectionSegmentView segment, double referenceS)
     {
         if (segment == null)
@@ -45,6 +56,10 @@ public static class ReferencePathInterpolator
         return FromPoint(segment.Points[segment.Points.Count - 1]);
     }
 
+    /// <summary>
+    /// 将原始平滑路径点转换为同一局部 S 的 Frenet 参考样本。
+    /// 参数：point 已携带世界 X/Y（m）、航向（rad）和曲率量；返回：保持其方向及所有几何量的不可变副本。
+    /// </summary>
     private static FrenetReferencePoint FromPoint(SmoothedPathPoint point)
     {
         return new FrenetReferencePoint(point.ArcLength, point.X, point.Y, point.Heading, point.UnwrappedHeading,
@@ -52,11 +67,19 @@ public static class ReferencePathInterpolator
             point.BodyClearance);
     }
 
+    /// <summary>
+    /// 在线性标量区间内计算插值值。
+    /// 参数：lower、upper 为同单位端点，fraction 为无单位比例；返回：同单位的未夹紧线性结果。
+    /// </summary>
     private static double Lerp(double lower, double upper, double fraction)
     {
         return lower + (upper - lower) * fraction;
     }
 
+    /// <summary>
+    /// 判定插值输入是否为有限实数。
+    /// 参数：value 为任意标量；返回：NaN 或无穷时为 false。
+    /// </summary>
     private static bool IsFinite(double value)
     {
         return !double.IsNaN(value) && !double.IsInfinity(value);