diff --git a/src/Core/PathPlanningManager.cs b/src/Core/PathPlanningManager.cs index 0140374..411dce4 100644 --- a/src/Core/PathPlanningManager.cs +++ b/src/Core/PathPlanningManager.cs @@ -949,7 +949,7 @@ namespace NavisworksTransport catch (Exception ex) { LogManager.Error($"网格地图生成失败: {ex.Message}"); - throw new Exception($"网格地图生成失败: {ex.Message}", ex); + throw; // 直接抛出原始异常，不包装 } // 根据用户设置决定是否进行网格可视化 @@ -1000,7 +1000,7 @@ namespace NavisworksTransport catch (Exception ex) { LogManager.Error($"路径查找失败: {ex.Message}"); - throw new Exception($"路径查找失败: {ex.Message}", ex); + throw; // 直接抛出原始异常，不包装 } // 1. 检查路径查找是否完全失败 @@ -1083,9 +1083,8 @@ namespace NavisworksTransport } catch (Exception ex) { - var errorMsg = $"自动路径规划失败: {ex.Message}"; - LogManager.Error(errorMsg, ex); - RaiseErrorOccurred(errorMsg, ex); + LogManager.Error($"自动路径规划失败: {ex.Message}", ex); + RaiseErrorOccurred(ex.Message, ex); // 使用原始异常消息，不再包装 return Task.FromResult(null); } } @@ -3391,7 +3390,7 @@ namespace NavisworksTransport // 清除之前的网格可视化 ClearGridVisualization(); - // 创建三个独立的网格可视化路径 + // 创建四种独立的网格可视化路径 var channelRoute = new PathRoute("GridVisualization_Channel") { Id = "grid_visualization_channel" @@ -3416,18 +3415,9 @@ namespace NavisworksTransport int channelCells = 0; int unknownCells = 0; int obstacleCells = 0; - int openSpaceCells = 0; int doorCells = 0; int totalVisualized = 0; int multiLayerCells = 0; - int totalLayersVisualized = 0; - int heightInsufficientLayers = 0; - - // 获取车辆高度（模型单位）用于判断层高是否足够 - double vehicleHeightInModelUnits = vehicleHeight * - UnitsConverter.GetMetersToUnitsConversionFactor(Application.ActiveDocument.Units); - - LogManager.Info($"[网格可视化] 车辆高度: {vehicleHeight}m = {vehicleHeightInModelUnits:F2}模型单位"); // 遍历所有网格单元格，根据类型分别收集 for (int x = 0; x < gridMap.Width; x++) @@ -3442,44 +3432,23 @@ namespace NavisworksTransport double centerX = gridCorner.X + gridMap.CellSize / 2; double centerY = gridCorner.Y + gridMap.CellSize / 2; - // 检查是否有多个高度层 + // 所有网格都基于高度层，统一处理 if (cell.HeightLayers != null && cell.HeightLayers.Count > 0) { - // 多层逻辑：遍历每一层，根据PassableHeight判断是否可通行 multiLayerCells++; foreach (var layer in cell.HeightLayers) { double layerZ = layer.Z; - double passableHeight = layer.PassableHeight.GetSpan(); - - // 判断该层是否可通行（考虑膨胀影响和高度限制） - bool isLayerWalkable = layer.IsWalkable; // 膨胀影响 - bool isHeightSufficient = passableHeight >= vehicleHeightInModelUnits; // 高度限制 - - // 调试：输出不可通行的层 - if ((!isLayerWalkable || !isHeightSufficient) && multiLayerCells < 5) // 只输出前5个网格的详情 - { - LogManager.Debug($"[层通行性] 网格({x},{y}) Layer Z={layerZ:F2}, IsWalkable={isLayerWalkable}, PassableH={passableHeight:F2}, 车辆H={vehicleHeightInModelUnits:F2}, 高度足够={isHeightSufficient}"); - } PathRoute targetRoute = null; string gridTypeName = ""; - // 根据层可通行性、高度是否足够和网格类型决定渲染方式 - if (isLayerWalkable && isHeightSufficient) + // 仅根据 layer.IsWalkable 决定渲染 + if (layer.IsWalkable) { - // 高度足够 - 按原类型渲染 - if (cell.CellType == CategoryAttributeManager.LogisticsElementType.通道) - { - if (_showWalkableGrid) - { - targetRoute = channelRoute; - gridTypeName = "通道"; - channelCells++; - } - } - else if (cell.CellType == CategoryAttributeManager.LogisticsElementType.门) + // 可通行层 - 门特殊处理，其他统一样式 + if (cell.CellType == CategoryAttributeManager.LogisticsElementType.门) { if (_showDoorGrid) { @@ -3488,38 +3457,25 @@ namespace NavisworksTransport doorCells++; } } - else if (cell.CellType == CategoryAttributeManager.LogisticsElementType.楼板) + else { + // 其他可通行类型统一为通道样式（绿色） if (_showWalkableGrid) { targetRoute = channelRoute; - gridTypeName = "开放"; - openSpaceCells++; + gridTypeName = "通道"; + channelCells++; } } } else { - // 层不可通行（被膨胀影响或高度不足）- 渲染为障碍物（灰色） + // 不可通行层 - 渲染为障碍物（灰色） if (_showObstacleGrid) { targetRoute = obstacleRoute; - - // 区分不可通行的原因 - if (!isLayerWalkable && !isHeightSufficient) - { - gridTypeName = "障碍_膨胀+高度不足"; - } - else if (!isLayerWalkable) - { - gridTypeName = "障碍_膨胀影响"; - } - else - { - gridTypeName = "障碍_高度不足"; - } - - heightInsufficientLayers++; + gridTypeName = "障碍"; + obstacleCells++; } } @@ -3534,83 +3490,30 @@ namespace NavisworksTransport Name = $"网格_{gridTypeName}({x},{y})_Z{layerZ:F2}", Type = PathPointType.WayPoint, Index = totalVisualized, - Notes = $"GridType:{cell.CellType}, LayerZ={layerZ:F2}, PassableH={passableHeight:F2}m" + Notes = $"GridType:{cell.CellType}, LayerZ={layerZ:F2}, IsWalkable={layer.IsWalkable}" }; targetRoute.Points.Add(gridPoint); totalVisualized++; - totalLayersVisualized++; } } } - else + else if (cell.CellType == CategoryAttributeManager.LogisticsElementType.Unknown) { - // 单层逻辑：保持原有逻辑不变 - var gridCenter = new Point3D(centerX, centerY, gridCorner.Z); - - PathRoute targetRoute = null; - string gridTypeName = ""; - - // 根据网格类型和用户设置确定目标路径和名称 - if (cell.IsWalkable && cell.CellType == CategoryAttributeManager.LogisticsElementType.通道) - { - if (_showWalkableGrid) - { - targetRoute = channelRoute; - gridTypeName = "通道"; - channelCells++; - } - } - else if (cell.IsWalkable && cell.CellType == CategoryAttributeManager.LogisticsElementType.门) - { - if (_showDoorGrid) - { - targetRoute = doorRoute; - gridTypeName = "门"; - doorCells++; - } - } - else if (cell.IsWalkable && cell.CellType == CategoryAttributeManager.LogisticsElementType.楼板) - { - if (_showWalkableGrid) - { - targetRoute = channelRoute; - gridTypeName = "开放"; - openSpaceCells++; - } - } - else if (cell.CellType == CategoryAttributeManager.LogisticsElementType.Unknown) - { - if (_showUnknownGrid) - { - targetRoute = unknownRoute; - gridTypeName = "空洞"; - unknownCells++; - } - } - else if (cell.CellType == CategoryAttributeManager.LogisticsElementType.障碍物) - { - if (_showObstacleGrid) - { - targetRoute = obstacleRoute; - gridTypeName = "障碍"; - obstacleCells++; - } - } - - // 如果有对应的路径，创建并添加网格点 - if (targetRoute != null) + // Unknown网格没有高度层，用于调试 + if (_showUnknownGrid) { + var gridCenter = new Point3D(centerX, centerY, gridCorner.Z); var gridPoint = new PathPoint { Position = gridCenter, - Name = $"网格_{gridTypeName}({x},{y})", + Name = $"网格_空洞({x},{y})", Type = PathPointType.WayPoint, Index = totalVisualized, - Notes = $"GridType:{cell.CellType}" + Notes = $"GridType:Unknown, NoLayers" }; - - targetRoute.Points.Add(gridPoint); + unknownRoute.Points.Add(gridPoint); + unknownCells++; totalVisualized++; } } @@ -3621,41 +3524,38 @@ namespace NavisworksTransport if (channelRoute.Points.Count > 0) { renderPlugin.RenderPointOnly(channelRoute); - LogManager.Info($"[网格可视化] 渲染通道/开放空间网格点: {channelRoute.Points.Count} 个"); + LogManager.Info($"[网格可视化] 渲染可通行网格层: {channelRoute.Points.Count} 个（绿色）"); } if (doorRoute.Points.Count > 0) { renderPlugin.RenderPointOnly(doorRoute); - LogManager.Info($"[网格可视化] 渲染门网格点: {doorRoute.Points.Count} 个（50%透明绿色）"); + LogManager.Info($"[网格可视化] 渲染门网格层: {doorRoute.Points.Count} 个（50%透明绿色）"); } if (unknownRoute.Points.Count > 0) { renderPlugin.RenderPointOnly(unknownRoute); - LogManager.Info($"[网格可视化] 渲染Unknown网格点: {unknownRoute.Points.Count} 个"); + LogManager.Info($"[网格可视化] 渲染Unknown网格: {unknownRoute.Points.Count} 个（红色，调试用）"); } if (obstacleRoute.Points.Count > 0) { renderPlugin.RenderPointOnly(obstacleRoute); - LogManager.Info($"[网格可视化] 渲染障碍物网格点: {obstacleRoute.Points.Count} 个"); + LogManager.Info($"[网格可视化] 渲染障碍物网格层: {obstacleRoute.Points.Count} 个（灰色）"); } // 输出统计信息 LogManager.Info($"[网格可视化] 可视化完成："); LogManager.Info($" - 总网格单元格数：{gridMap.Width * gridMap.Height}"); LogManager.Info($" - 多层网格数：{multiLayerCells}"); - LogManager.Info($" - 通道单元格数：{channelCells}"); - LogManager.Info($" - 门单元格数：{doorCells}"); - LogManager.Info($" - 开放空间单元格数：{openSpaceCells}"); + LogManager.Info($" - 可通行层数：{channelCells}"); + LogManager.Info($" - 门层数：{doorCells}"); LogManager.Info($" - Unknown单元格数：{unknownCells}"); - LogManager.Info($" - 障碍物单元格数：{obstacleCells}"); - LogManager.Info($" - 高度不足层数：{heightInsufficientLayers}"); - LogManager.Info($" - 已可视化单元格数：{totalVisualized}"); - LogManager.Info($" - 已可视化层数：{totalLayersVisualized}"); + LogManager.Info($" - 障碍物层数：{obstacleCells}"); + LogManager.Info($" - 已可视化点数：{totalVisualized}"); - RaiseStatusChanged($"网格可视化完成：显示了 {totalVisualized} 个网格单元 (多层网格:{multiLayerCells}, 通道:{channelCells}, 门:{doorCells}[50%透明], Unknown:{unknownCells}, 障碍:{obstacleCells}, 高度不足:{heightInsufficientLayers})", PathPlanningStatusType.Success); + RaiseStatusChanged($"网格可视化完成：显示了 {totalVisualized} 个网格层 (多层网格:{multiLayerCells}, 可通行:{channelCells}, 门:{doorCells}, Unknown:{unknownCells}, 障碍:{obstacleCells})", PathPlanningStatusType.Success); } catch (Exception ex) { @@ -3780,7 +3680,7 @@ namespace NavisworksTransport { _routes.Add(route); loadedCount++; - LogManager.Info($"加载历史路径: {route.Name}, ID={route.Id}, 长度={route.TotalLength:F2}米"); + //LogManager.Info($"加载历史路径: {route.Name}, ID={route.Id}, 长度={route.TotalLength:F2}米"); } } @@ -3870,4 +3770,4 @@ namespace NavisworksTransport #endregion } -} \ No newline at end of file +} diff --git a/src/PathPlanning/AutoPathFinder.cs b/src/PathPlanning/AutoPathFinder.cs index 50dcfe8..af5cab8 100644 --- a/src/PathPlanning/AutoPathFinder.cs +++ b/src/PathPlanning/AutoPathFinder.cs @@ -115,14 +115,7 @@ namespace NavisworksTransport.PathPlanning catch (Exception ex) { LogManager.Error($"[3D路径规划] 发生异常: {ex.Message}\n{ex.StackTrace}"); - return new PathFindingResult - { - PathPoints = new List { start, end }, - IsComplete = false, - OriginalEndPoint = end, - ActualEndPoint = start, - CompletionPercentage = 0 - }; + throw; // 直接抛出异常，不返回默认路径 } } @@ -160,7 +153,7 @@ namespace NavisworksTransport.PathPlanning for (int y = 0; y < gridMap.Height; y++) { var cell = gridMap.Cells[x, y]; - if (!cell.IsWalkable || cell.HeightLayers.Count == 0) + if (!cell.HasAnyWalkableLayer()) continue; // 为每个高度层创建独立节点 @@ -212,7 +205,7 @@ namespace NavisworksTransport.PathPlanning for (int y = 0; y < gridMap.Height; y++) { var cell = gridMap.Cells[x, y]; - if (!cell.IsWalkable || cell.HeightLayers.Count == 0) + if (!cell.HasAnyWalkableLayer()) continue; for (int li = 0; li < cell.HeightLayers.Count; li++) @@ -235,7 +228,7 @@ namespace NavisworksTransport.PathPlanning continue; var neighborCell = gridMap.Cells[nx, ny]; - if (!neighborCell.IsWalkable) + if (!neighborCell.HasAnyWalkableLayer()) continue; // 查找邻居中所有满足高度约束的层 @@ -452,17 +445,58 @@ namespace NavisworksTransport.PathPlanning var startNode = FindClosest3DNode(graph3D, startGridPos, start.Z); var endNode = FindClosest3DNode(graph3D, endGridPos, end.Z); - if (startNode == null || endNode == null) + // 起点和终点必须检查可达性 + bool startOnObstacle = (startNode == null); + bool endOnObstacle = (endNode == null); + + // 如果起点在障碍物上，直接报错 + if (startOnObstacle && endOnObstacle) { - LogManager.Error($"[A*执行-3D] 无法找到起点或终点节点"); - return new PathFindingResult + LogManager.Error($"[A*执行-3D] 起点网格({startGridPos.X},{startGridPos.Y})和终点网格({endGridPos.X},{endGridPos.Y})都在不可通行区域"); + throw new AutoPathPlanningException("起点和终点都在不可通行区域，无法生成路径"); + } + else if (startOnObstacle) + { + LogManager.Error($"[A*执行-3D] 起点网格({startGridPos.X},{startGridPos.Y})在不可通行区域"); + throw new AutoPathPlanningException("起点在不可通行区域，无法生成路径"); + } + + // 如果终点不可达，寻找最接近终点的可达节点（类似2.5D的ClosestApproach逻辑） + if (endOnObstacle) + { + LogManager.Warning($"[A*执行-3D] 终点网格({endGridPos.X},{endGridPos.Y})不可达，寻找最接近的可达节点"); + + // 从所有节点中找到最接近终点的节点 + double minDistance = double.MaxValue; + foreach (var node in graph3D.AllNodes) { - PathPoints = new List { start, end }, - IsComplete = false, - OriginalEndPoint = end, - ActualEndPoint = start, - CompletionPercentage = 0 - }; + var info = graph3D.NodeInfo[node]; + var nodeWorldPos = gridMap.GridToWorld3D(new GridPoint2D(info.X, info.Y)); + nodeWorldPos = new Point3D(nodeWorldPos.X, nodeWorldPos.Y, info.Z); + + double distance = Math.Sqrt( + Math.Pow(nodeWorldPos.X - end.X, 2) + + Math.Pow(nodeWorldPos.Y - end.Y, 2) + + Math.Pow(nodeWorldPos.Z - end.Z, 2) + ); + + if (distance < minDistance) + { + minDistance = distance; + endNode = node; + } + } + + if (endNode != null) + { + var closestInfo = graph3D.NodeInfo[endNode]; + LogManager.Info($"[A*执行-3D] 找到最接近终点的可达节点: ({closestInfo.X},{closestInfo.Y},层{closestInfo.LayerIndex},Z={closestInfo.Z:F2})，距离原终点 {minDistance:F2}模型单位"); + } + else + { + LogManager.Error($"[A*执行-3D] 无法找到任何可达节点（图中没有任何节点）"); + throw new AutoPathPlanningException("无法找到任何可达节点，网格地图可能完全被障碍物覆盖"); + } } var startInfo = graph3D.NodeInfo[startNode]; @@ -476,24 +510,88 @@ namespace NavisworksTransport.PathPlanning if (astarPath.Edges.Count == 0) { - LogManager.Warning($"[A*执行-3D] 未找到路径"); - return new PathFindingResult - { - PathPoints = new List { start, end }, - IsComplete = false, - OriginalEndPoint = end, - ActualEndPoint = start, - CompletionPercentage = 0 - }; + LogManager.Error($"[A*执行-3D] 未找到路径，起点({startInfo.X},{startInfo.Y})到终点({endInfo.X},{endInfo.Y})之间被障碍物完全阻断"); + throw new AutoPathPlanningException("起点和终点之间被障碍物完全阻断，无法找到可行路径"); } // 转换为世界坐标路径 var path3D = ConvertGraphPathToWorld(astarPath, graph3D, gridMap, start, end); + // 计算路径完成度，检查是否到达终点 double completionPercentage = 100.0; bool isComplete = true; Point3D actualEnd = path3D.Count > 0 ? path3D[path3D.Count - 1] : start; + // 如果终点在障碍物上，路径肯定不完整 + if (endOnObstacle) + { + isComplete = false; + actualEnd = path3D[path3D.Count - 1]; + + // 计算实际距离和总距离 + double totalDistance = Math.Sqrt( + Math.Pow(end.X - start.X, 2) + + Math.Pow(end.Y - start.Y, 2) + + Math.Pow(end.Z - start.Z, 2) + ); + double actualDistance = Math.Sqrt( + Math.Pow(actualEnd.X - start.X, 2) + + Math.Pow(actualEnd.Y - start.Y, 2) + + Math.Pow(actualEnd.Z - start.Z, 2) + ); + + completionPercentage = totalDistance > 0 ? (actualDistance / totalDistance) * 100 : 0; + + LogManager.Warning($"[3D路径规划] 终点在障碍物上，使用最接近的可达节点，路径不完整：完成度 {completionPercentage:F1}%"); + } + else + { + // 检查A*路径最后一个节点是否到达终点网格 + if (astarPath.Edges.Count > 0) + { + var lastEdge = astarPath.Edges[astarPath.Edges.Count - 1]; + var lastNode = lastEdge.End as Roy_T.AStar.Graphs.Node; + + if (lastNode != null && graph3D.NodeInfo.ContainsKey(lastNode)) + { + var lastInfo = graph3D.NodeInfo[lastNode]; + + // 检查最后节点是否到达终点网格（使用前面已定义的endGridPos） + if (lastInfo.X == endGridPos.X && lastInfo.Y == endGridPos.Y) + { + // 到达终点网格，路径完整 + isComplete = true; + actualEnd = path3D[path3D.Count - 1]; + completionPercentage = 100.0; + LogManager.Info($"[3D路径规划] 找到完整路径，到达终点网格({endGridPos.X},{endGridPos.Y})"); + } + else + { + // 未到达终点，计算完成度 + isComplete = false; + actualEnd = path3D[path3D.Count - 1]; + + // 计算实际距离和总距离 + double totalDistance = Math.Sqrt( + Math.Pow(end.X - start.X, 2) + + Math.Pow(end.Y - start.Y, 2) + + Math.Pow(end.Z - start.Z, 2) + ); + double actualDistance = Math.Sqrt( + Math.Pow(actualEnd.X - start.X, 2) + + Math.Pow(actualEnd.Y - start.Y, 2) + + Math.Pow(actualEnd.Z - start.Z, 2) + ); + + completionPercentage = totalDistance > 0 ? (actualDistance / totalDistance) * 100 : 0; + + LogManager.Warning($"[3D路径规划] 部分路径：到达网格({lastInfo.X},{lastInfo.Y})，距离终点网格({endGridPos.X},{endGridPos.Y})还有距离"); + LogManager.Info($"[3D路径规划] 完成度: {completionPercentage:F1}%，实际终点: ({actualEnd.X:F2}, {actualEnd.Y:F2}, Z={actualEnd.Z:F2})"); + } + } + } + } + LogManager.Info($"[3D路径规划] 路径生成完成，包含 {path3D.Count} 个路径点，完成度: {completionPercentage:F1}%"); return new PathFindingResult @@ -579,8 +677,7 @@ namespace NavisworksTransport.PathPlanning pointIndex++; } - path3D.Add(originalEnd); - LogManager.Info($"[路径点{pointIndex}] 原始终点: ({originalEnd.X:F2}, {originalEnd.Y:F2}, Z={originalEnd.Z:F2})"); + // 🔥 修复：不再强制添加原始终点，由ExecuteAStarOnGraph根据完成度决定 LogManager.Info($"[路径转换完成] 总共 {path3D.Count} 个路径点"); return path3D; @@ -621,14 +718,8 @@ namespace NavisworksTransport.PathPlanning if (pathResult == null || !pathResult.PathPoints.Any()) { - LogManager.Warning("[2.5D路径规划] 未找到有效路径"); - return new PathFindingResult - { - PathPoints = new List(), - IsComplete = false, - OriginalEndPoint = end, - CompletionPercentage = 0.0 - }; + LogManager.Error("[2.5D路径规划] 未找到有效路径"); + throw new AutoPathPlanningException("未找到有效路径，起点和终点之间可能被障碍物阻断"); } LogManager.Info($"[2.5D路径规划] A*算法找到原始路径，包含 {pathResult.PathPoints.Count} 个点"); @@ -658,13 +749,7 @@ namespace NavisworksTransport.PathPlanning catch (Exception ex) { LogManager.Error($"[2.5D路径规划] 路径查找失败: {ex.Message}"); - return new PathFindingResult - { - PathPoints = new List(), - IsComplete = false, - OriginalEndPoint = end, - CompletionPercentage = 0.0 - }; + throw; // 直接抛出异常，不返回默认路径 } } @@ -741,7 +826,7 @@ namespace NavisworksTransport.PathPlanning var cell = gridMap.Cells[x, y]; // 统计基本可通行性 - if (cell.IsWalkable) + if (cell.HasAnyWalkableLayer()) { totalWalkableCells++; } @@ -753,7 +838,7 @@ namespace NavisworksTransport.PathPlanning // 检查是否有满足车辆高度的层 if (!HasCompatibleLayer(cell, vehicleHeight)) { - if (cell.IsWalkable) + if (cell.HasAnyWalkableLayer()) { heightConstrainedCells++; if (heightConstrainedCells <= 10) @@ -978,7 +1063,7 @@ namespace NavisworksTransport.PathPlanning // 检查是否有兼容的高度层 if (!HasCompatibleLayer(cell, vehicleHeight)) { - if (cell.IsWalkable) + if (cell.HasAnyWalkableLayer()) { heightConstrainedCells++; } @@ -1200,7 +1285,7 @@ namespace NavisworksTransport.PathPlanning /// 是否可通行 private bool IsPassableWithHeight(GridCell cell, double vehicleHeight) { - if (!cell.IsWalkable) + if (!cell.HasAnyWalkableLayer()) return false; // 检查是否有任何高度层满足车辆高度要求 @@ -1225,7 +1310,7 @@ namespace NavisworksTransport.PathPlanning /// 是否有兼容的高度层 private bool HasCompatibleLayer(GridCell cell, double vehicleHeight) { - if (!cell.IsWalkable) + if (!cell.HasAnyWalkableLayer()) return false; if (cell.HeightLayers == null || cell.HeightLayers.Count == 0) @@ -1583,29 +1668,32 @@ namespace NavisworksTransport.PathPlanning start.Z + t * (end.Z - start.Z) ); - // 基础检查：采样点是否在可通行网格内 + // 🔥 修复：检查采样点的具体Z高度是否在可通行层范围内 + // 使用IsPassableAt3DPoint而不是IsWalkable，确保不穿过障碍物层 var gridPos = gridMap.WorldToGrid(samplePoint); - if (!gridMap.IsValidGridPosition(gridPos) || !gridMap.IsWalkable(gridPos)) + double tolerance = 0.1; // Z坐标容差（模型单位） + + if (!gridMap.IsValidGridPosition(gridPos) || !gridMap.IsPassableAt3DPoint(gridPos, samplePoint.Z, tolerance)) { // 🔥 修复：使用Origin计算网格左下角，而不是Bounds.Min var gridMinX = gridMap.Origin.X + gridPos.X * gridMap.CellSize; var gridMinY = gridMap.Origin.Y + gridPos.Y * gridMap.CellSize; - + // 计算网格中心点世界坐标（基于左下角） var gridCenterX = gridMinX + 0.5 * gridMap.CellSize; var gridCenterY = gridMinY + 0.5 * gridMap.CellSize; var gridCenterZ = samplePoint.Z; // Z保持不变 - + // 计算偏差 var deltaX = samplePoint.X - gridCenterX; var deltaY = samplePoint.Y - gridCenterY; - + // LogManager.Debug($"[斜线检查] 采样点{i}/{samples}失败：" + // $"采样点({samplePoint.X:F3},{samplePoint.Y:F3},{samplePoint.Z:F3})，" + // $"网格({gridPos.X},{gridPos.Y})左下角({gridMinX:F3},{gridMinY:F3})，" + // $"网格中心({gridCenterX:F3},{gridCenterY:F3},{gridCenterZ:F3})，" + // $"偏差(ΔX={deltaX:F3}, ΔY={deltaY:F3})，" + - // $"原因：{(!gridMap.IsValidGridPosition(gridPos) ? "网格无效" : "不可通行")}"); + // $"原因：{(!gridMap.IsValidGridPosition(gridPos) ? "网格无效" : "Z坐标不在可通行层范围")}"); return false; } @@ -1905,9 +1993,9 @@ namespace NavisworksTransport.PathPlanning var cell = gridMap.Cells[gridX, gridY]; // 检查基本可行走性 - if (!cell.IsWalkable) + if (!cell.HasAnyWalkableLayer()) { - LogManager.Info($"[高度检查] 单元格不可通行：({gridX}, {gridY}) - IsWalkable={cell.IsWalkable}, CellType={cell.CellType}, SpeedLimit={cell.SpeedLimit:F2}m/s, 原因：网格标记为不可通行"); + LogManager.Info($"[高度检查] 单元格不可通行：({gridX}, {gridY}) - HasWalkableLayer={cell.HasAnyWalkableLayer()}, CellType={cell.CellType}, SpeedLimit={cell.SpeedLimit:F2}m/s, 原因：网格标记为不可通行"); return false; } @@ -2275,14 +2363,8 @@ namespace NavisworksTransport.PathPlanning } else { - LogManager.Warning("[A*执行] 未找到路径"); - return new PathFindingResult - { - PathPoints = new List(), - IsComplete = false, - OriginalEndPoint = end, - CompletionPercentage = 0.0 - }; + LogManager.Error("[A*执行] 未找到路径"); + throw new AutoPathPlanningException("A*算法未找到路径，起点和终点之间被障碍物阻断"); } } catch (Exception ex) @@ -2508,7 +2590,7 @@ namespace NavisworksTransport.PathPlanning // 检查是否有兼容的高度层 if (!HasCompatibleLayer(cell, vehicleHeight)) { - if (cell.IsWalkable) + if (cell.HasAnyWalkableLayer()) { heightConstrainedCells++; } @@ -2743,7 +2825,7 @@ namespace NavisworksTransport.PathPlanning { var cell = gridMap.Cells[x, y]; // 🔥 核心修改：直接使用IsWalkable判断，所有不可通行的网格都视为障碍物 - bool isUnsafeArea = !cell.IsWalkable; + bool isUnsafeArea = !cell.HasAnyWalkableLayer(); distanceMap[x, y] = isUnsafeArea ? 0 : int.MaxValue; @@ -2752,7 +2834,7 @@ namespace NavisworksTransport.PathPlanning obstacleCount++; else if (cell.CellType == CategoryAttributeManager.LogisticsElementType.Unknown) unknownCount++; - else if (cell.IsWalkable) + else if (cell.HasAnyWalkableLayer()) walkableCount++; } } @@ -2942,4 +3024,4 @@ namespace NavisworksTransport.PathPlanning } } } -} \ No newline at end of file +} diff --git a/src/PathPlanning/ChannelBasedGridBuilder.cs b/src/PathPlanning/ChannelBasedGridBuilder.cs index f77ceee..8c5d876 100644 --- a/src/PathPlanning/ChannelBasedGridBuilder.cs +++ b/src/PathPlanning/ChannelBasedGridBuilder.cs @@ -145,7 +145,7 @@ namespace NavisworksTransport.PathPlanning var cell = gridMap.Cells[x, y]; // 只计算可通行的网格 - if (cell.IsWalkable) + if (cell.HasAnyWalkableLayer()) { walkableCount++; double cellZ = cell.HeightLayers != null && cell.HeightLayers.Count > 0 @@ -338,4 +338,4 @@ namespace NavisworksTransport.PathPlanning } -} \ No newline at end of file +} diff --git a/src/PathPlanning/GridCellBuilder.cs b/src/PathPlanning/GridCellBuilder.cs index 284e964..3112734 100644 --- a/src/PathPlanning/GridCellBuilder.cs +++ b/src/PathPlanning/GridCellBuilder.cs @@ -21,7 +21,6 @@ namespace NavisworksTransport.PathPlanning { return new GridCell { - IsWalkable = true, CellType = CategoryAttributeManager.LogisticsElementType.通道, IsInChannel = true, ChannelType = ChannelType.Corridor, @@ -44,7 +43,6 @@ namespace NavisworksTransport.PathPlanning { return new GridCell { - IsWalkable = true, CellType = CategoryAttributeManager.LogisticsElementType.门, IsInChannel = false, ChannelType = ChannelType.Other, @@ -65,7 +63,6 @@ namespace NavisworksTransport.PathPlanning { return new GridCell { - IsWalkable = false, // 障碍物永远不可通行 CellType = CategoryAttributeManager.LogisticsElementType.障碍物, IsInChannel = false, ChannelType = ChannelType.Other, @@ -88,7 +85,6 @@ namespace NavisworksTransport.PathPlanning { return new GridCell { - IsWalkable = true, CellType = CategoryAttributeManager.LogisticsElementType.电梯, IsInChannel = false, ChannelType = ChannelType.Other, @@ -111,7 +107,6 @@ namespace NavisworksTransport.PathPlanning { return new GridCell { - IsWalkable = true, CellType = CategoryAttributeManager.LogisticsElementType.楼梯, IsInChannel = false, ChannelType = ChannelType.Other, @@ -131,7 +126,6 @@ namespace NavisworksTransport.PathPlanning { return new GridCell { - IsWalkable = false, // Unknown类型默认不可通行 CellType = CategoryAttributeManager.LogisticsElementType.Unknown, IsInChannel = false, ChannelType = ChannelType.Other, diff --git a/src/PathPlanning/GridMap.cs b/src/PathPlanning/GridMap.cs index eaab45a..056f596 100644 --- a/src/PathPlanning/GridMap.cs +++ b/src/PathPlanning/GridMap.cs @@ -160,7 +160,7 @@ namespace NavisworksTransport.PathPlanning var cell = new GridCell { - IsWalkable = false, +// IsWalkable字段已删除，通行性由HeightLayers决定 CellType = CategoryAttributeManager.LogisticsElementType.Unknown, // 修改：默认为未知/空洞类型 IsInChannel = false, HeightLayers = new List(), @@ -240,8 +240,8 @@ namespace NavisworksTransport.PathPlanning { if (!IsValidGridPosition(gridPosition)) return false; - - return Cells[gridPosition.X, gridPosition.Y].IsWalkable; + + return Cells[gridPosition.X, gridPosition.Y].HasAnyWalkableLayer(); } /// @@ -260,7 +260,7 @@ namespace NavisworksTransport.PathPlanning var worldPos = GridToWorld3D(gridPosition); var cell = new GridCell { - IsWalkable = isWalkable, +// IsWalkable字段已删除，通行性由HeightLayers决定 CellType = cellType, IsInChannel = cellType == CategoryAttributeManager.LogisticsElementType.通道, HeightLayers = new List(), @@ -315,7 +315,7 @@ namespace NavisworksTransport.PathPlanning // 更新单元格的基本属性（使用第一个层的属性） if (cell.HeightLayers.Count == 1) { - cell.IsWalkable = true; + // IsWalkable由层决定，不再在网格级别设置 cell.CellType = layer.Type; cell.IsInChannel = true; cell.RelatedModelItem = layer.SourceItem; @@ -385,7 +385,8 @@ namespace NavisworksTransport.PathPlanning if (cell.CellType == CategoryAttributeManager.LogisticsElementType.Unknown || cell.CellType == CategoryAttributeManager.LogisticsElementType.障碍物) { - cell.IsWalkable = false; + // IsWalkable由层决定，不再在网格级别设置 + // Unknown和障碍物没有可通行层，HasAnyWalkableLayer()自动返回false cell.Cost = double.MaxValue; } else @@ -407,7 +408,7 @@ namespace NavisworksTransport.PathPlanning return false; var cell = Cells[gridPosition.X, gridPosition.Y]; - if (!cell.IsWalkable) + if (!cell.HasAnyWalkableLayer()) return false; // 检查是否有任何高度层满足高度要求 @@ -422,6 +423,46 @@ namespace NavisworksTransport.PathPlanning return false; } + /// + /// 检查指定3D点（包含Z坐标）是否在可通行的高度层范围内 + /// 用于路径优化中的斜线连接检查，确保路径不穿过障碍物层 + /// + /// 网格坐标 + /// Z坐标（模型单位） + /// Z坐标容差（模型单位），默认0.1 + /// 是否可通行 + public bool IsPassableAt3DPoint(GridPoint2D gridPosition, double zCoord, double tolerance = 0.1) + { + if (!IsValidGridPosition(gridPosition)) + return false; + + var cell = Cells[gridPosition.X, gridPosition.Y]; + if (!cell.HasAnyWalkableLayer()) + return false; + + // 检查是否有任何可通行层包含这个Z坐标 + if (cell.HeightLayers != null && cell.HeightLayers.Count > 0) + { + foreach (var layer in cell.HeightLayers) + { + // 跳过不可通行的层 + if (!layer.IsWalkable) + continue; + + // 检查Z坐标是否在这一层的范围内 + double layerMinZ = layer.Z; + double layerMaxZ = layer.Z + layer.PassableHeight.GetSpan(); + + if (zCoord >= layerMinZ - tolerance && zCoord <= layerMaxZ + tolerance) + { + return true; + } + } + } + + return false; + } + /// /// 获取所有通道类型的网格单元 /// @@ -575,7 +616,7 @@ namespace NavisworksTransport.PathPlanning { var cell = Cells[x, y]; - if (cell.IsWalkable) + if (cell.HasAnyWalkableLayer()) { totalWalkable++; switch (cell.CellType) @@ -663,11 +704,6 @@ namespace NavisworksTransport.PathPlanning /// public struct GridCell { - /// - /// 是否可通行 - /// - public bool IsWalkable { get; set; } - /// /// 遍历成本（1.0为标准成本） /// @@ -703,15 +739,24 @@ namespace NavisworksTransport.PathPlanning /// public double SpeedLimit { get; set; } + /// + /// 检查是否至少有一个可通行的高度层 + /// + /// 如果至少有一层可通行返回true，否则返回false + public bool HasAnyWalkableLayer() + { + return HeightLayers != null && HeightLayers.Any(layer => layer.IsWalkable); + } + /// /// 根据物流类型获取通行成本 /// /// 通行成本 public double GetCost() { - if (!IsWalkable) + if (!HasAnyWalkableLayer()) return double.MaxValue; - + switch (CellType) { case CategoryAttributeManager.LogisticsElementType.通道: @@ -749,10 +794,10 @@ namespace NavisworksTransport.PathPlanning { var cell = new GridCell { - IsWalkable = false, + // IsWalkable由层决定，障碍物没有层或所有层IsWalkable=false CellType = CategoryAttributeManager.LogisticsElementType.障碍物, IsInChannel = false, - HeightLayers = new List(), + HeightLayers = new List(), // 空列表，HasAnyWalkableLayer()返回false ChannelType = ChannelType.Other, SpeedLimit = 0 }; @@ -768,7 +813,7 @@ namespace NavisworksTransport.PathPlanning { var cell = new GridCell { - IsWalkable = true, +// IsWalkable字段已删除，通行性由HeightLayers决定 CellType = CategoryAttributeManager.LogisticsElementType.楼板, IsInChannel = false, HeightLayers = new List(), @@ -788,7 +833,7 @@ namespace NavisworksTransport.PathPlanning { var cell = new GridCell { - IsWalkable = isOpen, +// IsWalkable字段已删除，通行性由HeightLayers决定 CellType = CategoryAttributeManager.LogisticsElementType.门, IsInChannel = isOpen, HeightLayers = new List(), @@ -808,7 +853,7 @@ namespace NavisworksTransport.PathPlanning { var cell = new GridCell { - IsWalkable = true, +// IsWalkable字段已删除，通行性由HeightLayers决定 CellType = CategoryAttributeManager.LogisticsElementType.通道, IsInChannel = true, HeightLayers = new List(), @@ -1029,4 +1074,4 @@ namespace NavisworksTransport.PathPlanning public AutoPathPlanningException(string message) : base(message) { } public AutoPathPlanningException(string message, Exception innerException) : base(message, innerException) { } } -} \ No newline at end of file +} diff --git a/src/PathPlanning/GridMapGenerator.cs b/src/PathPlanning/GridMapGenerator.cs index fe951e5..29c6133 100644 --- a/src/PathPlanning/GridMapGenerator.cs +++ b/src/PathPlanning/GridMapGenerator.cs @@ -136,16 +136,16 @@ namespace NavisworksTransport.PathPlanning LogManager.Info($"【生成网格地图】智能收集到 {irrelevantRelatedItems.Count} 个无关项相关节点（将被排除）"); var irrelevantItemsSet = irrelevantRelatedItems; // 使用收集后的完整集合 - // 2. 直接遍历处理障碍物（包围盒检测） - 使用高性能优化版本 - LogManager.Info("【生成网格地图】步骤2: 高性能包围盒遍历处理障碍物"); - ProcessObstaclesWithBoundingBox(channelCoverage.GridMap, traversableRelatedItems, scanHeightInModelUnits, channelCoverage, irrelevantItemsSet); - - LogManager.Info($"【阶段2完成】障碍物处理后网格统计: {channelCoverage.GridMap.GetStatistics()}"); - - // 2.5. 为所有可通行网格设置PassableHeights确保高度约束检查 - LogManager.Info("【生成网格地图】步骤2.5: 为可通行网格设置高度约束"); + // 2. 先设置PassableHeights，确保障碍物高度检查时有正确的高度范围 + LogManager.Info("【生成网格地图】步骤2: 为可通行网格设置高度约束"); SetChannelPassableHeights(channelCoverage.GridMap, scanHeightInModelUnits); - LogManager.Info($"【阶段2.5完成】高度约束设置后网格统计: {channelCoverage.GridMap.GetStatistics()}"); + LogManager.Info($"【阶段2完成】高度约束设置后网格统计: {channelCoverage.GridMap.GetStatistics()}"); + + // 2.5. 处理障碍物（包围盒检测） - 使用高性能优化版本 + LogManager.Info("【生成网格地图】步骤2.5: 高性能包围盒遍历处理障碍物"); + ProcessObstaclesWithBoundingBox(channelCoverage.GridMap, traversableRelatedItems, scanHeightInModelUnits, channelCoverage, irrelevantItemsSet); + + LogManager.Info($"【阶段2.5完成】障碍物处理后网格统计: {channelCoverage.GridMap.GetStatistics()}"); // 2.6. 单独处理门元素，设置为可通行，并设置通行高度 LogManager.Info("【生成网格地图】步骤2.6: 处理门元素"); @@ -367,7 +367,7 @@ namespace NavisworksTransport.PathPlanning for (int y = 0; y < gridMap.Height; y++) { var cell = gridMap.Cells[x, y]; - if (cell.IsWalkable && cell.HeightLayers != null && cell.HeightLayers.Count > 0) + if (cell.HasAnyWalkableLayer()) { // 检查是否是楼梯或电梯网格（有多层且包含楼梯/电梯层） bool hasStairsOrElevator = cell.HeightLayers.Any(layer => @@ -449,7 +449,7 @@ namespace NavisworksTransport.PathPlanning var cell = gridMap.Cells[x, y]; // 只检查可通行且有高度层的网格 - if (!cell.IsWalkable || cell.HeightLayers == null || cell.HeightLayers.Count == 0) + if (!cell.HasAnyWalkableLayer()) continue; totalCheckedCells++; @@ -461,14 +461,44 @@ namespace NavisworksTransport.PathPlanning int nx = x + dx[dir]; int ny = y + dy[dir]; - // 边界检查 + // 情况1：网格地图边界 - 标记当前网格所有层为边界 if (nx < 0 || nx >= gridMap.Width || ny < 0 || ny >= gridMap.Height) + { + for (int li = 0; li < currentLayerCount; li++) + { + var layer = cell.HeightLayers[li]; + if (!layer.IsBoundary) + { + layer.IsBoundary = true; + cell.HeightLayers[li] = layer; + totalBoundaryLayers++; + //LogManager.Debug($"【边界检测】网格({x},{y}) Layer{li} 标记为边界（地图边界），方向={dirNames[dir]}"); + } + } continue; + } var neighbor = gridMap.Cells[nx, ny]; int neighborLayerCount = (neighbor.HeightLayers != null) ? neighbor.HeightLayers.Count : 0; - // 层数梯度检测：当前层数 > 邻居层数 + // 情况2：邻居是Unknown - 标记当前网格所有层为边界 + if (neighbor.CellType == CategoryAttributeManager.LogisticsElementType.Unknown) + { + for (int li = 0; li < currentLayerCount; li++) + { + var layer = cell.HeightLayers[li]; + if (!layer.IsBoundary) + { + layer.IsBoundary = true; + cell.HeightLayers[li] = layer; + totalBoundaryLayers++; + //LogManager.Debug($"【边界检测】网格({x},{y}) Layer{li} 标记为边界（邻居Unknown），方向={dirNames[dir]}"); + } + } + continue; + } + + // 情况3：层数梯度检测：当前层数 > 邻居层数 if (currentLayerCount <= neighborLayerCount) continue; @@ -514,9 +544,9 @@ namespace NavisworksTransport.PathPlanning cell.HeightLayers[currentLayerIndex] = currentLayer; totalBoundaryLayers++; - LogManager.Debug($"【边界检测】网格({x},{y}) Layer{currentLayerIndex} 标记为边界，" + - $"方向={dirNames[dir]}, 当前Z={currentZ:F2}, 邻居Z={neighborZ:F2}, " + - $"高度差={heightDiff:F2}, 梯度={gradient}"); + // LogManager.Debug($"【边界检测】网格({x},{y}) Layer{currentLayerIndex} 标记为边界，" + + // $"方向={dirNames[dir]}, 当前Z={currentZ:F2}, 邻居Z={neighborZ:F2}, " + + // $"高度差={heightDiff:F2}, 梯度={gradient}"); } } } @@ -673,7 +703,6 @@ namespace NavisworksTransport.PathPlanning // 创建更新后的通道GridCell，保持原有属性但更新位置和高度 var updatedCell = new GridCell { - IsWalkable = true, CellType = CategoryAttributeManager.LogisticsElementType.通道, IsInChannel = true, ChannelType = ChannelType.Corridor, @@ -694,7 +723,6 @@ namespace NavisworksTransport.PathPlanning // 创建高度不足的通道GridCell，保持原有属性但设为不可通行 var updatedCell = new GridCell { - IsWalkable = false, CellType = CategoryAttributeManager.LogisticsElementType.通道, IsInChannel = true, ChannelType = ChannelType.Corridor, @@ -748,7 +776,7 @@ namespace NavisworksTransport.PathPlanning { var cell = gridMap.Cells[x, y]; - if (cell.IsWalkable) + if (cell.HasAnyWalkableLayer()) { walkableCount++; switch (cell.CellType) @@ -866,30 +894,37 @@ namespace NavisworksTransport.PathPlanning // 默认为无穷大 distanceMap[x, y] = double.MaxValue; - // 1. 整个网格是障碍物 → 距离为0 - if (!cell.IsWalkable) - { - distanceMap[x, y] = 0.0; - continue; - } - - // 2. Unknown网格 → 距离为0 + // 1. Unknown网格 → 距离为0（整个网格无法通行） if (cell.CellType == CategoryAttributeManager.LogisticsElementType.Unknown) { distanceMap[x, y] = 0.0; continue; } - // 3. 该层索引存在且是边界 → 距离为0 + // 2. 检查该层索引是否存在 if (cell.HeightLayers != null && layerIndex < cell.HeightLayers.Count) { var layer = cell.HeightLayers[layerIndex]; + + // 2a. 该层不可通行 → 距离为0（障碍物层） + if (!layer.IsWalkable) + { + distanceMap[x, y] = 0.0; + continue; + } + + // 2b. 该层是边界 → 距离为0 if (layer.IsBoundary) { distanceMap[x, y] = 0.0; boundaryCount++; } } + else + { + // 3. 该层索引不存在（网格没有这一层）→ 距离为0（相当于障碍物） + distanceMap[x, y] = 0.0; + } } } @@ -964,14 +999,12 @@ namespace NavisworksTransport.PathPlanning if (layer.Type == CategoryAttributeManager.LogisticsElementType.门) continue; - // 膨胀条件：距离 < 膨胀半径 - // 边界网格距离=0，算在膨胀半径内 - // 例如：半径=1 → 只膨胀边界本身（距离0） - // 半径=2 → 膨胀边界+向内1格（距离0,1） - // 半径=3 → 膨胀边界+向内2格（距离0,1,2） + // 膨胀条件：距离>0（排除源点）且距离<=半径 + // 例如：半径=2 → 膨胀距离1和距离2的网格 + // 半径=3 → 膨胀距离1、2、3的网格 bool shouldInflate = false; - if (distanceMap[x, y] < inflationRadius) + if (distanceMap[x, y] > 0.0 && distanceMap[x, y] <= inflationRadius) { shouldInflate = true; } @@ -993,33 +1026,11 @@ namespace NavisworksTransport.PathPlanning LogManager.Info($"【多层膨胀】Layer{layerIndex} 完成，边界: {boundaryCount}, 膨胀: {inflatedCount}"); } - // 第三步：更新GridCell.IsWalkable（至少有一层可通行） - int cellsBecameUnwalkable = 0; - for (int x = 0; x < gridMap.Width; x++) - { - for (int y = 0; y < gridMap.Height; y++) - { - var cell = gridMap.Cells[x, y]; - bool wasWalkable = cell.IsWalkable; - - // 检查是否至少有一层可通行 - bool hasWalkableLayer = false; - if (cell.HeightLayers != null && cell.HeightLayers.Count > 0) - { - hasWalkableLayer = cell.HeightLayers.Any(layer => layer.IsWalkable); - } - - cell.IsWalkable = hasWalkableLayer; - gridMap.Cells[x, y] = cell; - - if (wasWalkable && !hasWalkableLayer) - cellsBecameUnwalkable++; - } - } + // GridCell.IsWalkable已移除，通行性由HeightLayer.IsWalkable决定 + // 不再需要更新网格级别的IsWalkable stopwatch.Stop(); LogManager.Info($"【多层膨胀】完成，耗时: {stopwatch.ElapsedMilliseconds}ms"); - LogManager.Info($"【多层膨胀】网格变为不可通行: {cellsBecameUnwalkable}个"); foreach (var kvp in layerStats.OrderBy(k => k.Key)) { @@ -1052,7 +1063,7 @@ namespace NavisworksTransport.PathPlanning var cell = gridMap.Cells[x, y]; // 膨胀源点：1.障碍物 2.Unknown网格 3.边界层网格 - if (!cell.IsWalkable) + if (!cell.HasAnyWalkableLayer()) { distanceMap[x, y] = 0.0; obstacleCount++; @@ -1180,10 +1191,23 @@ namespace NavisworksTransport.PathPlanning continue; } - // 创建障碍物GridCell - var worldPos = gridMap.GridToWorld3D(gridPos); - var obstacleCell = GridCellBuilder.Obstacle(worldPos, null); - gridMap.PlaceCell(gridPos, obstacleCell); + // 膨胀处理：保留层结构，只将所有层标记为不可通行 + if (cell.HeightLayers != null && cell.HeightLayers.Count > 0) + { + // 有高度层：将所有层标记为不可通行 + for (int i = 0; i < cell.HeightLayers.Count; i++) + { + var layer = cell.HeightLayers[i]; + layer.IsWalkable = false; + cell.HeightLayers[i] = layer; + } + } + + // 更新网格类型和属性 + cell.CellType = CategoryAttributeManager.LogisticsElementType.障碍物; + cell.Cost = double.MaxValue; + cell.IsInChannel = false; + gridMap.Cells[x, y] = cell; inflatedCells++; } else if (distanceMap[x, y] == double.MaxValue || distanceMap[x, y] > inflationRadius) @@ -1673,7 +1697,7 @@ namespace NavisworksTransport.PathPlanning var updatedCells = 0; var obstacleItems = 0; - var layersRemoved = 0; + var layersBlocked = 0; var cellsFullyBlocked = 0; var cellsWithLayers = 0; var totalLayersChecked = 0; @@ -1694,11 +1718,13 @@ namespace NavisworksTransport.PathPlanning totalLayersChecked += cell.HeightLayers.Count; } - // 3D高度检查：只移除与障碍物高度重叠的层 - var remainingLayers = new List(); + // 3D高度检查：标记与障碍物重叠的层为不可通行（保留层结构） + bool anyLayerModified = false; - foreach (var layer in cell.HeightLayers) + for (int i = 0; i < cell.HeightLayers.Count; i++) { + var layer = cell.HeightLayers[i]; + // 计算该层的高度范围 double layerMinZ = layer.Z; double layerMaxZ = layer.Z + layer.PassableHeight.GetSpan(); @@ -1710,34 +1736,25 @@ namespace NavisworksTransport.PathPlanning // 检查是否重叠 bool overlaps = !(obstacleMaxZ <= layerMinZ || obstacleMinZ > layerMaxZ); - if (!overlaps) + if (overlaps && layer.IsWalkable) { - // 不重叠，保留该层 - remainingLayers.Add(layer); - } - else - { - // 重叠，移除该层 - layersRemoved++; - LogManager.Debug($"[3D高度检查] 网格({update.X},{update.Y}) 层Z={layer.Z:F2}, 高度范围=[{layerMinZ:F2},{layerMaxZ:F2}], 障碍物范围=[{obstacleMinZ:F2},{obstacleMaxZ:F2}], 重叠=移除"); + // 重叠：标记为不可通行，但保留层结构 + layer.IsWalkable = false; + cell.HeightLayers[i] = layer; + layersBlocked++; + anyLayerModified = true; } } - // 更新网格的高度层列表 - cell.HeightLayers = remainingLayers; - - // 如果所有层都被移除，标记整个网格为障碍物 - if (remainingLayers.Count == 0) + // 如果所有层都被标记为不可通行，标记整个网格为障碍物 + if (anyLayerModified && !cell.HeightLayers.Any(l => l.IsWalkable)) { - cell.IsWalkable = false; cell.CellType = CategoryAttributeManager.LogisticsElementType.障碍物; cell.Cost = double.MaxValue; cell.IsInChannel = false; cell.RelatedModelItem = update.Item; cellsFullyBlocked++; } - // 如果还有层保留，网格仍然可通行（只是少了某些层） - // 保持 IsWalkable = true，路径规划时会基于剩余层进行 gridMap.Cells[update.X, update.Y] = cell; updatedCells++; @@ -1747,7 +1764,7 @@ namespace NavisworksTransport.PathPlanning obstacleItems = gridUpdates.GroupBy(u => u.Item).Count(); var updateElapsed = (DateTime.Now - updateStart).TotalMilliseconds; LogManager.Info($"[高性能障碍物处理] 阶段5完成: 更新 {updatedCells} 个网格单元，涉及 {obstacleItems} 个障碍物项，耗时: {updateElapsed:F1}ms"); - LogManager.Info($"[3D高度检查] 有层的网格: {cellsWithLayers}, 检查的总层数: {totalLayersChecked}, 移除了 {layersRemoved} 个高度层"); + LogManager.Info($"[3D高度检查] 有层的网格: {cellsWithLayers}, 检查的总层数: {totalLayersChecked}, 阻挡了 {layersBlocked} 个高度层"); LogManager.Info($"[3D高度检查] {cellsFullyBlocked} 个网格被完全阻挡，{updatedCells - cellsFullyBlocked} 个网格保留了部分层"); // 输出总体统计 @@ -1987,4 +2004,4 @@ namespace NavisworksTransport.PathPlanning } #endregion -} \ No newline at end of file +}