简化自动寻路主函数

src/PathPlanning/AutoPathFinder.cs

@@ -99,28 +99,20 @@ namespace NavisworksTransport.PathPlanning
 
                 LogManager.Info($"[2.5D路径规划] A*算法找到原始路径，包含 {pathResult.PathPoints.Count} 个点");
 
-                // 4. 路径优化（临时禁用以避免穿越问题）
-                // var optimizedPath = OptimizePath(pathResult.PathPoints, gridMap);
-                var optimizedPath = pathResult.PathPoints; // 直接使用原始A*路径，不进行优化
-                LogManager.Info($"[2.5D路径规划] 路径优化已禁用，使用原始A*路径，包含 {optimizedPath.Count} 个点");
-
-                // 5. 对路径进行高度插值，利用2.5D高度区间信息
-                var enhancedPath = InterpolatePathHeights2_5D(optimizedPath, gridMap, vehicleHeight);
+                // 5. 根据网格点通行高度区间对路径点Z坐标调整
+                var enhancedPath = ApplyGridHeightConstraints(pathResult.PathPoints, gridMap, vehicleHeight);
 
                 // 6. 精确高度修正
                 var heightCorrectedPath = ApplyPreciseHeightCorrection(enhancedPath, gridMap);
 
-                // 7. 直接返回高度校正后的路径（不再修改XY坐标）
-                var finalPath = heightCorrectedPath;
-
                 // 8. 验证路径中所有点的高度约束（特别是门网格）
-                ValidatePathHeightConstraints(finalPath, gridMap, vehicleHeight);
+                ValidatePathHeightConstraints(heightCorrectedPath, gridMap, vehicleHeight);
 
                 // 更新PathFindingResult
-                pathResult.PathPoints = finalPath;
+                pathResult.PathPoints = heightCorrectedPath;
                 pathResult.OriginalEndPoint = originalEnd;
 
-                LogManager.Info($"[2.5D路径规划] 路径生成完成，最终包含 {finalPath.Count} 个路径点，完成度: {pathResult.CompletionPercentage:F1}%");
+                LogManager.Info($"[2.5D路径规划] 路径生成完成，最终包含 {heightCorrectedPath.Count} 个路径点，完成度: {pathResult.CompletionPercentage:F1}%");
 
                 // 路径优化 - 去除共线点
                 pathResult = OptimizePath(pathResult, gridMap);
@@ -1232,8 +1224,6 @@ namespace NavisworksTransport.PathPlanning
                 }
 
                 var cell = gridMap.Cells[gridX, gridY];
-                LogManager.Info($"[高度检查] 单元格状态：IsWalkable={cell.IsWalkable}, CellType={cell.CellType}, PassableHeights计数={cell.PassableHeights?.Count ?? 0}");
-
                 // 检查基本可行走性
                 if (!cell.IsWalkable)
                 {
@@ -1244,8 +1234,6 @@ namespace NavisworksTransport.PathPlanning
                 // 检查高度约束
                 if (cell.PassableHeights != null && cell.PassableHeights.Any())
                 {
-                    LogManager.Info($"[高度检查] 检查 {cell.PassableHeights.Count} 个高度区间，车辆高度：{vehicleHeight}模型单位)");
-
                     foreach (var interval in cell.PassableHeights)
                     {
                         bool spanOk = interval.GetSpan() >= vehicleHeight;
@@ -1256,7 +1244,6 @@ namespace NavisworksTransport.PathPlanning
 
                         if (spanOk && containsHeight)
                         {
-                            LogManager.Info($"[高度检查] ✅ 找到合适的高度区间，通过检查");
                             return true;
                         }
                     }
@@ -1275,101 +1262,44 @@ namespace NavisworksTransport.PathPlanning
         }
 
         /// <summary>
-        /// 使用2.5D高度区间信息对路径进行高度插值
+        /// 根据网格高度区间调整路径点Z坐标
         /// </summary>
         /// <param name="path">原始路径</param>
         /// <param name="gridMap">网格地图</param>
         /// <param name="vehicleHeight">车辆高度（模型单位）</param>
-        /// <returns>插值后的路径</returns>
-        private List<Point3D> InterpolatePathHeights2_5D(List<Point3D> path, GridMap gridMap, double vehicleHeight)
+        /// <returns>调整后的路径</returns>
+        private List<Point3D> ApplyGridHeightConstraints(List<Point3D> path, GridMap gridMap, double vehicleHeight)
         {
-            try
+            LogManager.Info($"[应用高度区间] 开始调整，路径点数: {path.Count}");
+
+            var adjustedPath = new List<Point3D>();
+            int adjustedCount = 0;
+
+            foreach (var point in path)
             {
-                LogManager.Info($"[2.5D高度插值] 开始插值，路径点数: {path.Count}");
-
-                var interpolatedPath = new List<Point3D>();
-                int validHeightPoints = 0;
-
-                for (int i = 0; i < path.Count; i++)
-                {
-                    var originalPoint = path[i];
-
-                    // 获取最优高度
-                    var optimalHeight = GetOptimalHeightFromGrid(originalPoint, gridMap, vehicleHeight);
-
-                    if (optimalHeight.HasValue)
-                    {
-                        var enhancedPoint = new Point3D(originalPoint.X, originalPoint.Y, optimalHeight.Value);
-                        interpolatedPath.Add(enhancedPoint);
-                        validHeightPoints++;
-                    }
-                    else
-                    {
-                        // 如果无法获取高度信息，使用原始点
-                        interpolatedPath.Add(originalPoint);
-                    }
-                }
-
-                LogManager.Info($"[2.5D高度插值] 完成，有效高度点: {validHeightPoints}/{path.Count}");
-                return interpolatedPath;
-            }
-            catch (Exception ex)
-            {
-                LogManager.Error($"[2.5D高度插值] 插值失败: {ex.Message}");
-                return path; // 返回原始路径
-            }
-        }
-
-        /// <summary>
-        /// 从网格中获取指定位置的最优高度
-        /// </summary>
-        /// <param name="point">世界坐标点</param>
-        /// <param name="gridMap">网格地图</param>
-        /// <param name="vehicleHeight">车辆高度（模型单位）</param>
-        /// <returns>最优高度，如果无法确定则返回null</returns>
-        private double? GetOptimalHeightFromGrid(Point3D point, GridMap gridMap, double vehicleHeight)
-        {
-            try
-            {
-                // 将世界坐标转换为网格坐标
                 var gridPos = gridMap.WorldToGrid(point);
-                int gridX = gridPos.X;
-                int gridY = gridPos.Y;
 
-                // 确保坐标在有效范围内
-                if (gridX < 0 || gridX >= gridMap.Width || gridY < 0 || gridY >= gridMap.Height)
+                // 检查网格有效性
+                if (!gridMap.IsValidGridPosition(gridPos))
                 {
-                    return null;
+                    adjustedPath.Add(point);
+                    continue;
                 }
 
-                var cell = gridMap.Cells[gridX, gridY];
+                var cell = gridMap.Cells[gridPos.X, gridPos.Y];
 
-                // 如果有高度区间信息
-                if (cell.PassableHeights != null && cell.PassableHeights.Any())
-                {
-                    // 查找满足车辆高度要求的最佳区间
-                    // vehicleHeight已经是模型单位，直接比较即可
-                    var validIntervals = cell.PassableHeights
-                        .Where(interval => interval.GetSpan() >= vehicleHeight)
-                        .OrderBy(interval => interval.MinZ)
-                        .ToList();
+                var bestInterval = cell.PassableHeights
+                    .Where(interval => interval.GetSpan() >= vehicleHeight)
+                    .OrderBy(interval => interval.MinZ)
+                    .FirstOrDefault();
 
-                    if (validIntervals.Any())
-                    {
-                        // 选择最低的有效区间的底部高度，转换为绝对坐标
-                        var bestInterval = validIntervals.First();
-                        return cell.WorldPosition.Z + bestInterval.MinZ;
-                    }
-                }
-
-                // 如果没有高度信息，返回原始Z坐标
-                return point.Z;
-            }
-            catch (Exception ex)
-            {
-                LogManager.Error($"[最优高度获取] 获取失败: {ex.Message}");
-                return point.Z;
+                var optimalZ = cell.WorldPosition.Z + bestInterval.MinZ;
+                adjustedPath.Add(new Point3D(point.X, point.Y, optimalZ));
+                adjustedCount++;
             }
+
+            LogManager.Info($"[应用高度区间] 完成，调整了 {adjustedCount}/{path.Count} 个点");
+            return adjustedPath;
         }
 
         /// <summary>