增加了路径斜线优化，效果非常好

doc/requirement/todo_features.md

@@ -4,7 +4,8 @@
 
 ### [2025/09/09]
 
-1. [ ] （功能）增加安全优先路径策略
+1. [x] （功能）增加安全优先路径策略
+2. [x]  (功能) 增加路径斜线优化
 
 ### [2025/09/08]
 

src/PathPlanning/AutoPathFinder.cs

@@ -769,6 +769,67 @@ namespace NavisworksTransport.PathPlanning
             }
         }
 
+        /// <summary>
+        /// 专门的斜线路径优化算法
+        /// 在现有优化基础上，通过更激进的斜线连接进一步减少路径点数
+        /// </summary>
+        /// <param name="optimizedPath">已经过初步优化的路径</param>
+        /// <param name="gridMap">网格地图</param>
+        /// <returns>经过斜线优化后的路径</returns>
+        private List<Point3D> ApplyDiagonalOptimization(List<Point3D> optimizedPath, GridMap gridMap)
+        {
+            if (optimizedPath.Count <= 2)
+                return optimizedPath;
+
+            try
+            {
+                LogManager.Info($"[斜线优化] 开始斜线路径优化，输入路径包含 {optimizedPath.Count} 个点");
+
+                var diagonalOptimizedPath = new List<Point3D> { optimizedPath[0] }; // 保留起点
+
+                int currentIndex = 0;
+                while (currentIndex < optimizedPath.Count - 1)
+                {
+                    int farthestIndex = currentIndex + 1; // 默认下一个点
+                    
+                    // 🔥 更激进的斜线检测：从P+2开始尝试，直到找到最远可连接点
+                    for (int testIndex = currentIndex + 2; testIndex < optimizedPath.Count; testIndex++)
+                    {
+                        if (IsDirectPathClear(optimizedPath[currentIndex], optimizedPath[testIndex], gridMap))
+                        {
+                            // 可以直线连接，更新最远索引
+                            farthestIndex = testIndex;
+                        }
+                        // 🔧 关键改进：不像现有算法那样遇到失败就停止，而是继续尝试更远的点
+                        // 这样可以发现更多斜线连接机会
+                    }
+                    
+                    // 如果找到了比相邻点更远的连接，记录优化效果
+                    if (farthestIndex > currentIndex + 1)
+                    {
+                        int skippedPoints = farthestIndex - currentIndex - 1;
+                        LogManager.Info($"[斜线优化] 从点{currentIndex}直连到点{farthestIndex}，跳过{skippedPoints}个中间点");
+                    }
+
+                    currentIndex = farthestIndex;
+                    if (currentIndex < optimizedPath.Count)
+                    {
+                        diagonalOptimizedPath.Add(optimizedPath[currentIndex]);
+                    }
+                }
+
+                int reducedPoints = optimizedPath.Count - diagonalOptimizedPath.Count;
+                LogManager.Info($"[斜线优化] 斜线优化完成，点数变化: {optimizedPath.Count} -> {diagonalOptimizedPath.Count} (减少{reducedPoints}个点)");
+                
+                return diagonalOptimizedPath;
+            }
+            catch (Exception ex)
+            {
+                LogManager.Warning($"[斜线优化] 斜线优化失败: {ex.Message}，使用输入路径");
+                return optimizedPath;
+            }
+        }
+
         /// <summary>
         /// 检查两点间的直线路径是否畅通
         /// </summary>
@@ -1346,6 +1407,19 @@ namespace NavisworksTransport.PathPlanning
                         
                         // 更新路径结果
                         pathResult.PathPoints = optimizedPoints;
+                        
+                        // 🔥 步骤2：在现有优化基础上应用专门的斜线优化
+                        var diagonalOptimizedPoints = ApplyDiagonalOptimization(optimizedPoints, gridMap);
+                        
+                        if (diagonalOptimizedPoints.Count != optimizedPoints.Count)
+                        {
+                            LogManager.Info($"[斜线优化] 二次优化点数变化：{optimizedPoints.Count} -> {diagonalOptimizedPoints.Count}");
+                            pathResult.PathPoints = diagonalOptimizedPoints;
+                        }
+                        else
+                        {
+                            LogManager.Info($"[斜线优化] 斜线优化未能进一步减少点数，保持现有结果");
+                        }
                     }
                 }
                 else
@@ -1683,7 +1757,6 @@ namespace NavisworksTransport.PathPlanning
             if (!_loggedDistances.Contains(safetyDistance))
             {
                 _loggedDistances.Add(safetyDistance);
-                LogManager.Debug($"[速度权重] 安全距离{safetyDistance} -> 速度{finalSpeed:F1}km/h");
             }
                 
             return finalSpeed;

src/PathPlanning/PathOptimizer.cs

@@ -230,7 +230,6 @@ namespace NavisworksTransport.PathPlanning
                 if (!currentDirection.Equals(prevDirection))
                 {
                     simplified.Add(dedupedGridPath[i - 1]);
-                    LogManager.Debug($"[路径优化] 检测到转弯：位置({dedupedGridPath[i-1].X},{dedupedGridPath[i-1].Y})，方向从({prevDirection.X},{prevDirection.Y})变为({currentDirection.X},{currentDirection.Y})");
                     prevDirection = currentDirection;
                 }
             }