From 97b9c2beca6170a8994a78845b58e4705a8e0d5d Mon Sep 17 00:00:00 2001 From: tian <11429339@qq.com> Date: Tue, 16 Sep 2025 04:56:56 +0800 Subject: [PATCH] =?UTF-8?q?=E7=BB=99=E7=BD=91=E6=A0=BC=E5=85=A8=E9=83=A8?= =?UTF-8?q?=E5=8A=A0=E4=B8=8A=E9=AB=98=E5=BA=A6=EF=BC=8C=E8=A7=A3=E5=86=B3?= =?UTF-8?q?=E4=BA=86=E9=97=A8=E9=99=90=E9=AB=98=E7=9A=84=E9=97=AE=E9=A2=98?= =?UTF-8?q?=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/PathPlanning/AutoPathFinder.cs | 119 +++++++++++++++++++++------ src/PathPlanning/GridMapGenerator.cs | 64 ++++++++++++-- 2 files changed, 151 insertions(+), 32 deletions(-) diff --git a/src/PathPlanning/AutoPathFinder.cs b/src/PathPlanning/AutoPathFinder.cs index 330cc4e..e3e878b 100644 --- a/src/PathPlanning/AutoPathFinder.cs +++ b/src/PathPlanning/AutoPathFinder.cs @@ -165,6 +165,9 @@ namespace NavisworksTransport.PathPlanning // 7. 直接返回高度校正后的路径（不再修改XY坐标） var finalPath = heightCorrectedPath; + // 8. 验证路径中所有点的高度约束（特别是门网格） + ValidatePathHeightConstraints(finalPath, gridMap, vehicleHeight); + // 更新PathFindingResult pathResult.PathPoints = finalPath; pathResult.OriginalEndPoint = originalEnd; @@ -251,24 +254,34 @@ namespace NavisworksTransport.PathPlanning // 检查基本可通行性和高度约束 bool isPassable = cell.IsWalkable; - if (isPassable && cell.PassableHeights != null && cell.PassableHeights.Any()) + if (isPassable) { - bool heightOk = cell.PassableHeights.Any(interval => interval.GetSpan() >= vehicleHeight); - if (!heightOk) + if (cell.PassableHeights != null && cell.PassableHeights.Any()) { - heightConstrainedCells++; + // 将车辆高度从米转换为模型单位 + double vehicleHeightInModelUnits = vehicleHeight * UnitsConverter.GetMetersToUnitsConversionFactor(Application.ActiveDocument.Units); + bool heightOk = cell.PassableHeights.Any(interval => interval.GetSpan() >= vehicleHeightInModelUnits); + if (!heightOk) + { + heightConstrainedCells++; - // 详细日志：记录被高度约束排除的单元格（仅记录前10个避免日志过多） - if (heightConstrainedCells <= 10) - { - LogManager.Info($"[A*高度约束] 单元格({x},{y})被高度约束排除：车辆高度{vehicleHeight}m，可用区间：{string.Join(", ", cell.PassableHeights.Select(i => $"{i.MinZ:F2}-{i.MaxZ:F2}(跨度{i.GetSpan():F2})"))}"); - } - else if (heightConstrainedCells == 11) - { - LogManager.Info($"[A*高度约束] 还有更多单元格被高度约束排除，不再详细记录..."); + // 详细日志：记录被高度约束排除的单元格（仅记录前10个避免日志过多） + if (heightConstrainedCells <= 10) + { + LogManager.Info($"[A*高度约束] 单元格({x},{y})被高度约束排除：车辆高度{vehicleHeight}m({vehicleHeightInModelUnits:F2}模型单位)，可用区间：{string.Join(", ", cell.PassableHeights.Select(i => $"{i.MinZ:F2}-{i.MaxZ:F2}(跨度{i.GetSpan():F2})"))}"); + } + else if (heightConstrainedCells == 11) + { + LogManager.Info($"[A*高度约束] 还有更多单元格被高度约束排除，不再详细记录..."); + } } + isPassable = heightOk; + } + else + { + // 没有高度信息的网格直接标记为不可通行 + isPassable = false; } - isPassable = heightOk; } if (isPassable) @@ -284,7 +297,7 @@ namespace NavisworksTransport.PathPlanning bool rightPassable = rightCell.IsWalkable; if (rightPassable && rightCell.PassableHeights != null && rightCell.PassableHeights.Any()) { - rightPassable = rightCell.PassableHeights.Any(interval => interval.GetSpan() >= vehicleHeight); + rightPassable = rightCell.PassableHeights.Any(interval => interval.GetSpan() >= vehicleHeightInModelUnits); } if (rightPassable) { @@ -301,7 +314,7 @@ namespace NavisworksTransport.PathPlanning bool bottomPassable = bottomCell.IsWalkable; if (bottomPassable && bottomCell.PassableHeights != null && bottomCell.PassableHeights.Any()) { - bottomPassable = bottomCell.PassableHeights.Any(interval => interval.GetSpan() >= vehicleHeight); + bottomPassable = bottomCell.PassableHeights.Any(interval => interval.GetSpan() >= vehicleHeightInModelUnits); } if (bottomPassable) { @@ -550,7 +563,7 @@ namespace NavisworksTransport.PathPlanning return cell.PassableHeights.Any(interval => interval.GetSpan() >= vehicleHeight); } - return true; + return false; } /// @@ -1282,13 +1295,17 @@ namespace NavisworksTransport.PathPlanning // 检查高度约束 if (cell.PassableHeights != null && cell.PassableHeights.Any()) { - LogManager.Info($"[高度检查] 检查 {cell.PassableHeights.Count} 个高度区间，车辆高度：{vehicleHeight}m"); + // 将车辆高度从米转换为模型单位 + double vehicleHeightInModelUnits = vehicleHeight * UnitsConverter.GetMetersToUnitsConversionFactor(Application.ActiveDocument.Units); + LogManager.Info($"[高度检查] 检查 {cell.PassableHeights.Count} 个高度区间，车辆高度：{vehicleHeight}m ({vehicleHeightInModelUnits:F2}模型单位)"); foreach (var interval in cell.PassableHeights) { - bool spanOk = interval.GetSpan() >= vehicleHeight; - bool containsHeight = interval.Contains(point.Z); - LogManager.Info($"[高度检查] 区间 {interval}: 跨度={interval.GetSpan():F2}m (需要≥{vehicleHeight}m): {spanOk}, 包含Z={point.Z:F2}: {containsHeight}"); + bool spanOk = interval.GetSpan() >= vehicleHeightInModelUnits; + // 将绝对坐标转换为相对于网格底面的坐标 + double relativeZ = point.Z - cell.WorldPosition.Z; + bool containsHeight = interval.Contains(relativeZ); + LogManager.Info($"[高度检查] 区间 {interval}: 跨度={interval.GetSpan():F2}模型单位 (需要≥{vehicleHeightInModelUnits:F2}模型单位): {spanOk}, 包含相对Z={relativeZ:F2}: {containsHeight}"); if (spanOk && containsHeight) { @@ -1301,9 +1318,7 @@ namespace NavisworksTransport.PathPlanning return false; } - // 如果没有高度信息，认为可通行（向后兼容） - LogManager.Info($"[高度检查] 无高度信息，默认可通行（向后兼容模式）"); - return true; + return false; } catch (Exception ex) { @@ -1509,6 +1524,64 @@ namespace NavisworksTransport.PathPlanning } } + /// + /// 验证路径中所有点的高度约束（特别检查门网格） + /// + /// 路径点列表 + /// 网格地图 + /// 车辆高度 + private void ValidatePathHeightConstraints(List path, GridMap gridMap, double vehicleHeight) + { + LogManager.Info($"[路径高度验证] 开始验证 {path.Count} 个路径点的高度约束"); + + int checkedPoints = 0; + int passedPoints = 0; + int failedPoints = 0; + int doorPoints = 0; + + for (int i = 0; i < path.Count; i++) + { + var point = path[i]; + var gridPos = gridMap.WorldToGrid(point); + + if (gridMap.IsValidGridPosition(gridPos)) + { + var cell = gridMap.Cells[gridPos.X, gridPos.Y]; + checkedPoints++; + + // 特别标记门网格点 + if (cell.CellType == CategoryAttributeManager.LogisticsElementType.门) + { + doorPoints++; + LogManager.Info($"[路径高度验证] 检查路径点{i}（门网格）: ({point.X:F2}, {point.Y:F2}, {point.Z:F2}) -> 网格({gridPos.X},{gridPos.Y})"); + } + else + { + LogManager.Info($"[路径高度验证] 检查路径点{i}（{cell.CellType}）: ({point.X:F2}, {point.Y:F2}, {point.Z:F2}) -> 网格({gridPos.X},{gridPos.Y})"); + } + + // 进行高度约束检查 + bool isPassable = IsPointPassableAtHeight(point, gridMap, vehicleHeight); + if (isPassable) + { + passedPoints++; + } + else + { + failedPoints++; + LogManager.Warning($"[路径高度验证] ❌ 路径点{i}高度约束检查失败！"); + } + } + } + + LogManager.Info($"[路径高度验证] 完成 - 检查点数: {checkedPoints}, 通过: {passedPoints}, 失败: {failedPoints}, 门网格点: {doorPoints}"); + + if (failedPoints > 0) + { + LogManager.Warning($"[路径高度验证] 警告：发现 {failedPoints} 个点不满足高度约束！"); + } + } + /// /// 计算两点间的3D距离 /// diff --git a/src/PathPlanning/GridMapGenerator.cs b/src/PathPlanning/GridMapGenerator.cs index 2ea4860..a15bc7e 100644 --- a/src/PathPlanning/GridMapGenerator.cs +++ b/src/PathPlanning/GridMapGenerator.cs @@ -243,7 +243,12 @@ namespace NavisworksTransport.PathPlanning LogManager.Info("【包围盒2.5D模式】步骤2.5: 处理门元素"); ProcessDoorElements(channelCoverage.GridMap, allChannelItems); LogManager.Info($"【阶段2.5完成】门元素处理后网格统计: {channelCoverage.GridMap.GetStatistics()}"); - + + // 2.6. 为通道网格设置PassableHeights确保高度约束检查 + LogManager.Info("【包围盒2.5D模式】步骤2.6: 为通道网格设置高度约束"); + SetChannelPassableHeights(channelCoverage.GridMap, scanHeight); + LogManager.Info($"【阶段2.6完成】通道高度约束设置后网格统计: {channelCoverage.GridMap.GetStatistics()}"); + // 3. 应用车辆尺寸膨胀（如果需要） if (vehicleRadius > 0 || safetyMargin > 0) { @@ -335,20 +340,27 @@ namespace NavisworksTransport.PathPlanning // 获取门的限高属性 string heightLimitStr = CategoryAttributeManager.GetLogisticsPropertyValue(doorItem, CategoryAttributeManager.LogisticsProperties.HEIGHT_LIMIT); - double doorHeight = CategoryAttributeManager.ParseLogisticsLimitValue(heightLimitStr, "限高"); - - // 如果没有限高属性或解析失败，使用门包围盒的高度 - if (doorHeight <= 0) + double configuredHeight = CategoryAttributeManager.ParseLogisticsLimitValue(heightLimitStr, "限高"); + + // 计算门的实际物理高度 + double actualDoorHeight = bbox.Max.Z - bbox.Min.Z; + + double doorHeight; + if (configuredHeight <= 0) { - doorHeight = bbox.Max.Z - bbox.Min.Z; + // 没有限高配置，使用实际门高 + doorHeight = actualDoorHeight; LogManager.Debug($"【门元素处理】门元素 {doorItem.DisplayName} 未设置限高，使用包围盒高度: {doorHeight:F2}"); } else { // 转换限高到模型单位 double metersToModelUnits = UnitsConverter.GetMetersToUnitsConversionFactor(Application.ActiveDocument.Units); - doorHeight = doorHeight * metersToModelUnits; - LogManager.Debug($"【门元素处理】门元素 {doorItem.DisplayName} 限高: {doorHeight:F2} 模型单位"); + double configuredHeightInModelUnits = configuredHeight * metersToModelUnits; + + // 取实际门高和配置限高的最小值 + doorHeight = Math.Min(actualDoorHeight, configuredHeightInModelUnits); + LogManager.Debug($"【门元素处理】门元素 {doorItem.DisplayName} 实际高度: {actualDoorHeight:F2}, 限高:{configuredHeightInModelUnits:F2}, 有效高度: {doorHeight:F2}"); } // 获取门底部的Z坐标 @@ -368,7 +380,7 @@ namespace NavisworksTransport.PathPlanning var worldPos = gridMap.GridToWorld3D(gridPos, doorBottomZ); cell.WorldPosition = worldPos; - // 将门的高度范围存储在PassableHeights中供后续使用 + // 将门的高度范围存储在PassableHeights中供后续使用（使用相对坐标） if (doorHeight > 0) { cell.PassableHeights = new List @@ -399,6 +411,40 @@ namespace NavisworksTransport.PathPlanning } } + /// + /// 为通道网格设置PassableHeights确保高度约束检查 + /// + /// 网格地图 + /// 扫描高度（车辆高度+安全间隙） + private void SetChannelPassableHeights(GridMap gridMap, double scanHeight) + { + // scanHeight是米，需要转换为模型单位 + double metersToModelUnits = UnitsConverter.GetMetersToUnitsConversionFactor(Application.ActiveDocument.Units); + double scanHeightInModelUnits = scanHeight * metersToModelUnits; + + LogManager.Info($"【通道高度设置】开始为通道网格设置高度约束，扫描高度: {scanHeight:F2}米 ({scanHeightInModelUnits:F2}模型单位)"); + + int processedCount = 0; + for (int x = 0; x < gridMap.Width; x++) + { + for (int y = 0; y < gridMap.Height; y++) + { + var cell = gridMap.Cells[x, y]; + if (cell.CellType == CategoryAttributeManager.LogisticsElementType.通道 && cell.IsWalkable) + { + cell.PassableHeights = new List + { + new HeightInterval(0, scanHeightInModelUnits) + }; + gridMap.Cells[x, y] = cell; + processedCount++; + } + } + } + + LogManager.Info($"【通道高度设置】完成，已处理 {processedCount} 个通道网格"); + } + /// /// 从物流元素集合中过滤出门类型的元素 ///