去掉了传统路径算法

src/PathPlanning/AutoPathFinder.cs

@@ -37,7 +37,7 @@ namespace NavisworksTransport.PathPlanning
         /// <param name="channelCoverage">通道覆盖数据（可选，用于2.5D路径规划）</param>
         /// <param name="vehicleHeight">车辆高度（用于高度约束检查）</param>
         /// <returns>路径点列表（世界坐标）</returns>
-        public List<Point3D> FindPath(Point3D start, Point3D end, GridMap gridMap, ChannelCoverage channelCoverage = null, double vehicleHeight = 3.0)
+        public List<Point3D> FindPath(Point3D start, Point3D end, GridMap gridMap, ChannelCoverage channelCoverage, double vehicleHeight)
         {
             LogManager.Info($"开始A*路径查找: 起点({start?.X:F2}, {start?.Y:F2}, {start?.Z:F2}), 终点({end?.X:F2}, {end?.Y:F2}, {end?.Z:F2})");
             
@@ -48,9 +48,7 @@ namespace NavisworksTransport.PathPlanning
                 return FindPath2_5D(start, end, gridMap, channelCoverage, vehicleHeight);
             }
             
-            // 传统模式路径规划
-            LogManager.Info("[传统路径规划] 使用边界框模式");
-            return FindPathCore(start, end, gridMap);
+            return null;
         }
 
         /// <summary>
@@ -68,6 +66,10 @@ namespace NavisworksTransport.PathPlanning
             {
                 LogManager.Info("[2.5D路径规划] 开始执行");
                 
+                // 保存原始起点和终点，用于最终路径修正
+                var originalStart = start;
+                var originalEnd = end;
+                
                 // 1. 检查起点和终点的高度约束
                 if (!IsPointPassableAtHeight(start, gridMap, vehicleHeight))
                 {
@@ -83,142 +85,39 @@ namespace NavisworksTransport.PathPlanning
                 var astarGrid = ConvertToAStarGridWith2_5D(gridMap, channelCoverage, vehicleHeight);
                 
                 // 3. 执行A*算法
-                var path = ExecuteAStarAlgorithm(start, end, gridMap, astarGrid);
+                var rawPath = ExecuteAStarAlgorithm(start, end, gridMap, astarGrid);
                 
-                // 4. 对路径进行高度插值，利用2.5D高度区间信息
-                if (path != null && path.Any())
+                if (rawPath == null || !rawPath.Any())
                 {
-                    var enhancedPath = InterpolatePathHeights2_5D(path, gridMap, vehicleHeight);
-                    LogManager.Info($"[2.5D路径规划] 路径生成完成，包含 {enhancedPath.Count} 个路径点");
-                    return enhancedPath;
+                    LogManager.Warning("[2.5D路径规划] 未找到有效路径");
+                    return new List<Point3D>();
                 }
                 
-                LogManager.Warning("[2.5D路径规划] 未找到有效路径");
-                return new List<Point3D>();
+                LogManager.Info($"[2.5D路径规划] A*算法找到原始路径，包含 {rawPath.Count} 个点");
+                
+                // 4. 路径优化（添加缺失的步骤）
+                var optimizedPath = OptimizePath(rawPath, gridMap);
+                LogManager.Info($"[2.5D路径规划] 路径优化完成，从 {rawPath.Count} 个点优化到 {optimizedPath.Count} 个点");
+                
+                // 5. 对路径进行高度插值，利用2.5D高度区间信息
+                var enhancedPath = InterpolatePathHeights2_5D(optimizedPath, gridMap, vehicleHeight);
+                
+                // 6. 精确高度修正
+                var heightCorrectedPath = ApplyPreciseHeightCorrection(enhancedPath, gridMap);
+                
+                // 7. 替换起点和终点为原始用户指定的坐标（与FindPathCore保持一致）
+                var finalPath = CorrectStartEndPoints(heightCorrectedPath, originalStart, originalEnd);
+                
+                LogManager.Info($"[2.5D路径规划] 路径生成完成，最终包含 {finalPath.Count} 个路径点");
+                LogManager.Info($"[2.5D路径规划] 起点坐标修正: 网格转换({heightCorrectedPath[0].X:F2}, {heightCorrectedPath[0].Y:F2}) -> 原始坐标({originalStart.X:F2}, {originalStart.Y:F2})");
+                LogManager.Info($"[2.5D路径规划] 终点坐标修正: 网格转换({heightCorrectedPath[heightCorrectedPath.Count-1].X:F2}, {heightCorrectedPath[heightCorrectedPath.Count-1].Y:F2}) -> 原始坐标({originalEnd.X:F2}, {originalEnd.Y:F2})");
+                
+                return finalPath;
             }
             catch (Exception ex)
             {
                 LogManager.Error($"[2.5D路径规划] 路径查找失败: {ex.Message}");
-                LogManager.Info("[2.5D路径规划] 回退到传统模式");
-                return FindPathCore(start, end, gridMap);
-            }
-        }
-        
-        /// <summary>
-        /// 查找路径（传统方法，保持向后兼容）
-        /// </summary>
-        /// <param name="start">起点（世界坐标）</param>
-        /// <param name="end">终点（世界坐标）</param>
-        /// <param name="gridMap">网格地图</param>
-        /// <returns>路径点列表（世界坐标）</returns>
-        private List<Point3D> FindPathCore(Point3D start, Point3D end, GridMap gridMap)
-        {
-            try
-            {
-                // 验证输入参数
-                ValidateInputs(start, end, gridMap);
-
-                // 保存原始起点和终点，用于最终路径修正
-                var originalStart = start;
-                var originalEnd = end;
-
-                // 设置路径规划的起点和终点，用于Z坐标插值
-                gridMap.PlanningStartPoint = start;
-                gridMap.PlanningEndPoint = end;
-                gridMap.HasPlanningPoints = true;
-
-                // 转换起点和终点为网格坐标
-                var startGrid = gridMap.WorldToGrid(start);
-                var endGrid = gridMap.WorldToGrid(end);
-
-                // 验证起点和终点是否在有效范围内
-                if (!gridMap.IsValidGridPosition(startGrid))
-                {
-                    throw new AutoPathPlanningException($"起点({start.X:F2}, {start.Y:F2})超出网格范围");
-                }
-
-                if (!gridMap.IsValidGridPosition(endGrid))
-                {
-                    throw new AutoPathPlanningException($"终点({end.X:F2}, {end.Y:F2})超出网格范围");
-                }
-
-                // 🔥 移除权宜性措施：直接验证起点和终点是否可通行
-                if (!gridMap.IsWalkable(startGrid))
-                {
-                    throw new AutoPathPlanningException($"起点({start.X:F2}, {start.Y:F2})位于障碍物上，请检查起点位置或确认是否在通道内");
-                }
-                
-                if (!gridMap.IsWalkable(endGrid))
-                {
-                    throw new AutoPathPlanningException($"终点({end.X:F2}, {end.Y:F2})位于障碍物上，请检查起点位置或确认是否在通道内");
-                }
-
-                // 转换为RoyT.AStar网格格式并执行A*算法
-                LogManager.Info("执行A*算法查找路径...");
-                var astarGrid = ConvertToAStarGrid(gridMap);
-                var startPos = new GridPosition(startGrid.X, startGrid.Y);
-                var endPos = new GridPosition(endGrid.X, endGrid.Y);
-                
-                var pathFinder = new PathFinder();
-                var astarPath = pathFinder.FindPath(startPos, endPos, astarGrid);
-
-                if (astarPath == null || astarPath.Edges.Count == 0)
-                {
-                    throw new AutoPathPlanningException("未找到可行路径");
-                }
-
-                LogManager.Info($"A*算法找到路径，包含 {astarPath.Edges.Count + 1} 个网格点");
-
-                // 转换为世界坐标并优化
-                var worldPath = ConvertPathToWorldCoordinates(astarPath, gridMap);
-                var optimizedPath = OptimizePath(worldPath, gridMap);
-                var heightCorrectedPath = ApplyPreciseHeightCorrection(optimizedPath, gridMap);
-
-                // 🔥 关键修复：替换起点和终点为原始用户指定的坐标
-                var correctedPath = CorrectStartEndPoints(heightCorrectedPath, originalStart, originalEnd);
-
-                LogManager.Info($"路径查找完成，最终包含 {correctedPath.Count} 个点");
-                LogManager.Info($"起点坐标修正: 网格转换({heightCorrectedPath[0].X:F2}, {heightCorrectedPath[0].Y:F2}) -> 原始坐标({originalStart.X:F2}, {originalStart.Y:F2})");
-                LogManager.Info($"终点坐标修正: 网格转换({heightCorrectedPath[heightCorrectedPath.Count-1].X:F2}, {heightCorrectedPath[heightCorrectedPath.Count-1].Y:F2}) -> 原始坐标({originalEnd.X:F2}, {originalEnd.Y:F2})");
-                
-                return correctedPath;
-            }
-            catch (AutoPathPlanningException)
-            {
-                throw; // 重新抛出已知异常
-            }
-            catch (Exception ex)
-            {
-                LogManager.Error($"A*路径查找时发生错误: {ex.Message}");
-                throw new AutoPathPlanningException($"路径查找失败: {ex.Message}", ex);
-            }
-        }
-
-        /// <summary>
-        /// 验证输入参数
-        /// </summary>
-        /// <param name="start">起点</param>
-        /// <param name="end">终点</param>
-        /// <param name="gridMap">网格地图</param>
-        private void ValidateInputs(Point3D start, Point3D end, GridMap gridMap)
-        {
-            if (start == null)
-                throw new ArgumentNullException(nameof(start), "起点不能为空");
-
-            if (end == null)
-                throw new ArgumentNullException(nameof(end), "终点不能为空");
-
-            if (gridMap == null)
-                throw new ArgumentNullException(nameof(gridMap), "网格地图不能为空");
-
-            // 检查起点和终点是否相同
-            double distance = Math.Sqrt(
-                Math.Pow(end.X - start.X, 2) +
-                Math.Pow(end.Y - start.Y, 2));
-
-            if (distance < 0.1) // 10cm以内认为是同一点
-            {
-                throw new AutoPathPlanningException("起点和终点距离过近");
+                return null;
             }
         }
 
@@ -295,60 +194,6 @@ namespace NavisworksTransport.PathPlanning
             }
         }
 
-        /// <summary>
-        /// 将GridMap转换为RoyT.AStar的Grid格式（传统模式）
-        /// </summary>
-        /// <param name="gridMap">网格地图</param>
-        /// <returns>A*算法网格</returns>
-        private Grid ConvertToAStarGrid(GridMap gridMap)
-        {
-            try
-            {
-                LogManager.Info($"[A*转换] 开始转换网格格式: {gridMap.Width}x{gridMap.Height}");
-
-                // ⚠️ 关键修复：gridMap.CellSize 是模型单位，需要转换为米
-                // RoyT.AStar 的 cellSize 必须使用米为单位，因为它的 Position 就是米坐标
-                LogManager.Info($"[A*转换] 开始单位转换...");
-                double cellSizeInMeters = UnitsConverter.ConvertToMeters(gridMap.CellSize);
-                LogManager.Info($"[A*转换] 单位转换完成: {gridMap.CellSize:F2}模型单位 -> {cellSizeInMeters:F2}米");
-                
-                LogManager.Info($"[A*转换] 创建网格参数...");
-                var gridSize = new GridSize(gridMap.Width, gridMap.Height);
-                var cellSize = new Size(Distance.FromMeters((float)cellSizeInMeters), Distance.FromMeters((float)cellSizeInMeters));
-                var traversalVelocity = Velocity.FromKilometersPerHour(5); // 5 km/h 的遍历速度
-                
-                LogManager.Info($"[A*转换] A*网格参数: 尺寸={gridMap.Width}x{gridMap.Height}, 模型单位={gridMap.CellSize:F2}, 米单位={cellSizeInMeters:F2}, 速度=5km/h");
-
-                LogManager.Info($"[A*转换] 调用Grid.CreateGridWithLateralConnections...");
-                var grid = Grid.CreateGridWithLateralConnections(gridSize, cellSize, traversalVelocity);
-                LogManager.Info($"[A*转换] A*网格创建完成");
-
-                int blockedCells = 0;
-
-                // 标记不可通行的单元格
-                for (int x = 0; x < gridMap.Width; x++)
-                {
-                    for (int y = 0; y < gridMap.Height; y++)
-                    {
-                        if (!gridMap.Cells[x, y].IsWalkable)
-                        {
-                            var gridPos = new GridPosition(x, y);
-                            grid.DisconnectNode(gridPos);
-                            blockedCells++;
-                        }
-                    }
-                }
-
-                LogManager.Info($"网格转换完成，阻塞单元格: {blockedCells}个");
-                return grid;
-            }
-            catch (Exception ex)
-            {
-                LogManager.Error($"转换网格格式时发生错误: {ex.Message}");
-                throw new AutoPathPlanningException($"网格转换失败: {ex.Message}", ex);
-            }
-        }
-
         /// <summary>
         /// 将A*路径转换为世界坐标
         /// </summary>
@@ -524,138 +369,6 @@ namespace NavisworksTransport.PathPlanning
             return true;
         }
 
-        /// <summary>
-        /// 查找多个备选路径（支持精确高度计算）
-        /// </summary>
-        /// <param name="start">起点</param>
-        /// <param name="end">终点</param>
-        /// <param name="gridMap">网格地图</param>
-        /// <param name="maxAlternatives">最大备选路径数</param>
-        /// <param name="channelItems">通道模型项集合（可选，用于精确高度计算）</param>
-        /// <returns>备选路径列表</returns>
-        public List<List<Point3D>> FindAlternativePaths(Point3D start, Point3D end, GridMap gridMap, int maxAlternatives = 3, IEnumerable<ModelItem> channelItems = null)
-        {
-            // 如果提供了通道数据，设置到网格地图中启用精确高度计算
-            if (channelItems != null)
-            {
-                gridMap.SetChannelItems(channelItems);
-                LogManager.Info($"[备选路径] 已设置通道数据，启用精确高度计算: {channelItems.Count()} 个通道");
-            }
-
-            return FindAlternativePathsCore(start, end, gridMap, maxAlternatives);
-        }
-
-        /// <summary>
-        /// 查找多个备选路径（核心实现）
-        /// </summary>
-        /// <param name="start">起点</param>
-        /// <param name="end">终点</param>
-        /// <param name="gridMap">网格地图</param>
-        /// <param name="maxAlternatives">最大备选路径数</param>
-        /// <returns>备选路径列表</returns>
-        private List<List<Point3D>> FindAlternativePathsCore(Point3D start, Point3D end, GridMap gridMap, int maxAlternatives = 3)
-        {
-            var alternatives = new List<List<Point3D>>();
-
-            try
-            {
-                // 第一条路径：标准最短路径
-                var primaryPath = FindPathCore(start, end, gridMap);
-                alternatives.Add(primaryPath);
-
-                // 为了找到替代路径，我们可以临时阻塞主路径的部分节点
-                // 这是一个简化的实现，实际应用中可能需要更复杂的算法
-                LogManager.Info($"找到主路径，尝试寻找 {maxAlternatives - 1} 条备选路径");
-
-                for (int i = 1; i < maxAlternatives; i++)
-                {
-                    try
-                    {
-                        var alternativePath = FindAlternativePathByBlocking(start, end, gridMap, primaryPath, i);
-                        if (alternativePath != null && alternativePath.Count > 0)
-                        {
-                            alternatives.Add(alternativePath);
-                        }
-                    }
-                    catch
-                    {
-                        break; // 无法找到更多备选路径
-                    }
-                }
-
-                LogManager.Info($"共找到 {alternatives.Count} 条路径");
-            }
-            catch (Exception ex)
-            {
-                LogManager.Error($"查找备选路径时发生错误: {ex.Message}");
-            }
-
-            return alternatives;
-        }
-
-        /// <summary>
-        /// 通过阻塞主路径的部分节点来寻找备选路径
-        /// </summary>
-        /// <param name="start">起点</param>
-        /// <param name="end">终点</param>
-        /// <param name="gridMap">网格地图</param>
-        /// <param name="primaryPath">主路径</param>
-        /// <param name="blockingStrategy">阻塞策略</param>
-        /// <returns>备选路径</returns>
-        private List<Point3D> FindAlternativePathByBlocking(Point3D start, Point3D end,
-            GridMap gridMap, List<Point3D> primaryPath, int blockingStrategy)
-        {
-            if (primaryPath.Count < 3)
-                return null;
-
-            // 创建临时网格地图副本
-            var tempGridMap = CloneGridMap(gridMap);
-
-            // 根据策略阻塞主路径的部分节点
-            int blockStart = primaryPath.Count / (blockingStrategy + 1);
-            int blockEnd = Math.Min(blockStart + primaryPath.Count / 4, primaryPath.Count - 1);
-
-            for (int i = blockStart; i < blockEnd; i++)
-            {
-                var gridPos = tempGridMap.WorldToGrid(primaryPath[i]);
-                if (tempGridMap.IsValidGridPosition(gridPos))
-                {
-                    tempGridMap.SetCell(gridPos, false, double.MaxValue, ElementType.Obstacle);
-                }
-            }
-
-            // 在修改后的网格上寻找路径
-            return FindPathCore(start, end, tempGridMap);
-        }
-
-        /// <summary>
-        /// 克隆网格地图
-        /// </summary>
-        /// <param name="original">原始网格地图</param>
-        /// <returns>克隆的网格地图</returns>
-        private GridMap CloneGridMap(GridMap original)
-        {
-            var clone = new GridMap(original.Bounds, original.CellSize);
-
-            // 复制网格单元格数据
-            for (int x = 0; x < original.Width; x++)
-            {
-                for (int y = 0; y < original.Height; y++)
-                {
-                    clone.Cells[x, y] = original.Cells[x, y];
-                }
-            }
-
-            // 复制高度计算相关设置
-            clone.PlanningStartPoint = original.PlanningStartPoint;
-            clone.PlanningEndPoint = original.PlanningEndPoint;
-            clone.HasPlanningPoints = original.HasPlanningPoints;
-            clone.ChannelItems = original.ChannelItems;
-            clone.EnablePreciseHeightCalculation = original.EnablePreciseHeightCalculation;
-
-            return clone;
-        }
-
         /// <summary>
         /// 计算路径总长度
         /// </summary>