From 504a2c9862fd0282f1d696263fa83a8661a5f1c5 Mon Sep 17 00:00:00 2001 From: tian <11429339@qq.com> Date: Wed, 24 Sep 2025 02:25:25 +0800 Subject: [PATCH] =?UTF-8?q?=E6=89=AB=E6=8F=8F=E9=9A=9C=E7=A2=8D=E7=89=A9?= =?UTF-8?q?=E6=97=B6=EF=BC=8C=E4=BD=BF=E7=94=A8=E5=8C=85=E5=9B=B4=E7=9B=92?= =?UTF-8?q?=E4=B8=AD=E5=BF=83=E4=B8=8B=E9=9D=A2=E7=9A=84=E9=80=9A=E9=81=93?= =?UTF-8?q?=E7=BD=91=E6=A0=BCz=E9=AB=98=E5=BA=A6=EF=BC=8C=E8=BF=9B?= =?UTF-8?q?=E8=A1=8C=E9=AB=98=E5=BA=A6=E8=8C=83=E5=9B=B4=E7=AD=9B=E9=80=89?= =?UTF-8?q?=E3=80=82=20=E5=87=A0=E4=BD=95=E4=BD=93=E9=99=A4=E4=BA=86?= =?UTF-8?q?=E4=B8=89=E8=A7=92=E5=BD=A2=E5=A4=96=EF=BC=8C=E8=BF=98=E6=9C=89?= =?UTF-8?q?=E7=BA=BF=E5=BD=A2=EF=BC=88=E4=B9=9F=E8=AE=B8=E6=9C=89=E7=82=B9?= =?UTF-8?q?=E3=80=81SnapPoints=E3=80=81=E6=96=87=E5=AD=97=E7=AD=89?= =?UTF-8?q?=EF=BC=89=EF=BC=8C=E8=A6=81=E8=BF=87=E6=BB=A4=E6=8E=89=EF=BC=8C?= =?UTF-8?q?=E5=90=A6=E5=88=99=E4=B9=9F=E8=A2=AB=E5=BD=93=E6=88=90=E9=9A=9C?= =?UTF-8?q?=E7=A2=8D=E7=89=A9=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CLAUDE.md | 3 +- src/Core/PathPlanningManager.cs | 10 +- src/PathPlanning/ChannelBasedGridBuilder.cs | 25 +- src/PathPlanning/GridMap.cs | 7 +- src/PathPlanning/GridMapGenerator.cs | 410 +++++++++++--------- 5 files changed, 234 insertions(+), 221 deletions(-) diff --git a/CLAUDE.md b/CLAUDE.md index d2bdcf4..5fcc41b 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -204,4 +204,5 @@ public class PathClickToolPlugin : ToolPlugin { } - 在编码中，不要用回退或向后兼容的思路和步骤 - 程序的日志在：C:\ProgramData\Autodesk\Navisworks Manage 2026\NavisworksTransport\logs\debug.log - 不要搞向后兼容 -- 使用agent完成任务前，一定要先用Plan模式设计好方案和任务清单，并征得我同意。 \ No newline at end of file +- 使用agent完成任务前，一定要先用Plan模式设计好方案和任务清单，并征得我同意。 +- 网格坐标代表的是网格单元的左下角，而不是中心点！ \ No newline at end of file diff --git a/src/Core/PathPlanningManager.cs b/src/Core/PathPlanningManager.cs index 3953453..68a5101 100644 --- a/src/Core/PathPlanningManager.cs +++ b/src/Core/PathPlanningManager.cs @@ -3291,8 +3291,12 @@ namespace NavisworksTransport // 计算网格单元格的世界中心位置 var gridPos = new NavisworksTransport.PathPlanning.GridPoint2D(x, y); - var worldCenter = gridMap.GridToWorld3D(gridPos, cell.WorldPosition.Z); - var visualizationPoint = worldCenter; + var gridCorner = gridMap.GridToWorld3D(gridPos, cell.WorldPosition.Z); + var gridCenter = new Point3D( + gridCorner.X + gridMap.CellSize / 2, + gridCorner.Y + gridMap.CellSize / 2, + gridCorner.Z + ); PathRoute targetRoute = null; string gridTypeName = ""; @@ -3349,7 +3353,7 @@ namespace NavisworksTransport { var gridPoint = new PathPoint { - Position = visualizationPoint, + Position = gridCenter, Name = $"网格_{gridTypeName}({x},{y})", Type = PathPointType.WayPoint, Index = totalVisualized, diff --git a/src/PathPlanning/ChannelBasedGridBuilder.cs b/src/PathPlanning/ChannelBasedGridBuilder.cs index c1d9112..ae48cd8 100644 --- a/src/PathPlanning/ChannelBasedGridBuilder.cs +++ b/src/PathPlanning/ChannelBasedGridBuilder.cs @@ -82,16 +82,13 @@ namespace NavisworksTransport.PathPlanning // 3. 创建网格地图 var gridMap = new GridMap(totalBounds, gridSize); - // 新增：跟踪最小通道顶面高度 - double minChannelTopZ = double.MaxValue; - // 4. 为每个通道生成精确投影 var processedChannels = new List(); foreach (var channel in channelItems) { try { - ProjectChannelToGrid(channel, gridMap, ref minChannelTopZ); + ProjectChannelToGrid(channel, gridMap); processedChannels.Add(channel); } catch (Exception ex) @@ -101,14 +98,12 @@ namespace NavisworksTransport.PathPlanning } LogManager.Info($"[通道网格构建器] 成功投影 {processedChannels.Count}/{channelItems.Count} 个通道"); - LogManager.Info($"[通道网格构建器] 最低通道顶面高度: {minChannelTopZ:F2}"); return new ChannelCoverage { GridMap = gridMap, ChannelItems = processedChannels, - TotalBounds = totalBounds, - MinChannelTopZ = minChannelTopZ + TotalBounds = totalBounds }; } @@ -119,8 +114,7 @@ namespace NavisworksTransport.PathPlanning /// /// 通道物品 /// 目标网格地图 - /// 最小通道顶面高度的引用，用于跟踪所有通道中的最低顶面 - private void ProjectChannelToGrid(ModelItem channel, GridMap gridMap, ref double minChannelTopZ) + private void ProjectChannelToGrid(ModelItem channel, GridMap gridMap) { LogManager.Info($"[通道网格构建器] 投影通道: {channel.DisplayName}"); @@ -162,13 +156,14 @@ namespace NavisworksTransport.PathPlanning // 根据法向量决定处理方式 if (normal.Z > 0.7) // 朝上的水平面 { - RasterizeTriangleToGrid(gridMap, triangle, ref minChannelTopZ); + RasterizeTriangleToGrid(gridMap, triangle); processedTriangles++; } else if (normal.Z > 0.2) // 可行走的斜面（楼梯、坡道） { // 对于斜面，也进行投影，适用于楼梯等倾斜通道 - RasterizeTriangleToGrid(gridMap, triangle, ref minChannelTopZ); + // [TODO] 斜面应该用不同的计算顶面高度方法 + RasterizeTriangleToGrid(gridMap, triangle); processedTriangles++; } // 垂直面和朝下的面不投影 @@ -216,8 +211,7 @@ namespace NavisworksTransport.PathPlanning /// /// 网格地图 /// 三角形 - /// 最小通道顶面高度的引用，用于跟踪所有通道中的最低顶面 - private void RasterizeTriangleToGrid(GridMap gridMap, Triangle3D triangle, ref double minChannelTopZ) + private void RasterizeTriangleToGrid(GridMap gridMap, Triangle3D triangle) { // 获取三角形的2D投影边界框 var minX = Math.Min(triangle.Point1.X, Math.Min(triangle.Point2.X, triangle.Point3.X)); @@ -231,9 +225,6 @@ namespace NavisworksTransport.PathPlanning // 计算三角形的平均高度作为通道顶面高度 double triangleHeight = (triangle.Point1.Z + triangle.Point2.Z + triangle.Point3.Z) / 3.0; - // 更新最小通道顶面高度 - minChannelTopZ = Math.Min(minChannelTopZ, triangleHeight); - // 遍历边界框内的所有网格点 for (int x = minGrid.X; x <= maxGrid.X; x++) { @@ -335,7 +326,7 @@ namespace NavisworksTransport.PathPlanning /// 格式化字符串 private string FormatBounds(BoundingBox3D bounds) { - return $"[{bounds.Min.X:F1},{bounds.Min.Y:F1},{bounds.Min.Z:F1}] - [{bounds.Max.X:F1},{bounds.Max.Y:F1},{bounds.Max.Z:F1}]"; + return $"[{bounds.Min.X:F2},{bounds.Min.Y:F2},{bounds.Min.Z:F2}] - [{bounds.Max.X:F2},{bounds.Max.Y:F2},{bounds.Max.Z:F2}]"; } } diff --git a/src/PathPlanning/GridMap.cs b/src/PathPlanning/GridMap.cs index 4b4d583..66f1fb2 100644 --- a/src/PathPlanning/GridMap.cs +++ b/src/PathPlanning/GridMap.cs @@ -182,7 +182,7 @@ namespace NavisworksTransport.PathPlanning { double gridX = (worldPosition.X - Origin.X) / CellSize; double gridY = (worldPosition.Y - Origin.Y) / CellSize; - + return new GridPoint2D((int)Math.Floor(gridX), (int)Math.Floor(gridY)); } @@ -843,11 +843,6 @@ namespace NavisworksTransport.PathPlanning /// public BoundingBox3D TotalBounds { get; set; } - /// - /// 最低通道顶面高度，用作障碍物扫描的基准Z值 - /// - public double MinChannelTopZ { get; set; } = double.MaxValue; - /// /// 获取统计信息 /// diff --git a/src/PathPlanning/GridMapGenerator.cs b/src/PathPlanning/GridMapGenerator.cs index 7880466..7284067 100644 --- a/src/PathPlanning/GridMapGenerator.cs +++ b/src/PathPlanning/GridMapGenerator.cs @@ -100,8 +100,7 @@ namespace NavisworksTransport.PathPlanning // 2. 直接遍历处理障碍物（包围盒检测） - 使用高性能优化版本 LogManager.Info("【生成网格地图】步骤2: 高性能包围盒遍历处理障碍物"); - double minChannelTopZ = channelCoverage.MinChannelTopZ; // 提取最小通道顶面高度（模型单位） - ProcessObstaclesWithBoundingBoxOptimized(document, channelCoverage.GridMap, channelRelatedItems, scanHeightInModelUnits, minChannelTopZ); + ProcessObstaclesWithBoundingBoxOptimized(document, channelCoverage.GridMap, channelRelatedItems, scanHeightInModelUnits); LogManager.Info($"【阶段2完成】障碍物处理后网格统计: {channelCoverage.GridMap.GetStatistics()}"); @@ -590,137 +589,105 @@ namespace NavisworksTransport.PathPlanning } /// - /// 优化的车辆膨胀算法 - 使用距离变换方法 + /// 优化的车辆膨胀算法 - 使用正确的8连通距离变换方法 /// 时间复杂度: O(n*m) 而不是 O(n*m*r^2) /// private void ApplyVehicleInflationOptimized(GridMap gridMap, int inflationRadius) { var stopwatch = System.Diagnostics.Stopwatch.StartNew(); - - LogManager.Info($"【高效膨胀】使用距离变换算法，网格大小: {gridMap.Width}x{gridMap.Height}"); - - // 创建距离矩阵 - var distanceMap = new int[gridMap.Width, gridMap.Height]; - - // 初始化距离矩阵 - 膨胀源点：障碍物、Unknown网格 + + LogManager.Info($"【8连通膨胀】使用正确的8连通距离变换算法，网格大小: {gridMap.Width}x{gridMap.Height}"); + + // 使用浮点数距离矩阵支持√2对角线距离 + var distanceMap = new double[gridMap.Width, gridMap.Height]; + const double SQRT2 = 1.414213562373095; // √2 + + // 初始化距离矩阵 int obstacleCount = 0; int unknownCount = 0; - + for (int x = 0; x < gridMap.Width; x++) { for (int y = 0; y < gridMap.Height; y++) { var cell = gridMap.Cells[x, y]; - - // 1. 障碍物作为膨胀源点 + + // 障碍物和Unknown网格作为膨胀源点 if (!cell.IsWalkable) { - distanceMap[x, y] = 0; + distanceMap[x, y] = 0.0; obstacleCount++; } - // 2. Unknown网格作为膨胀源点 else if (cell.CellType == CategoryAttributeManager.LogisticsElementType.Unknown) { - distanceMap[x, y] = 0; + distanceMap[x, y] = 0.0; unknownCount++; } - // 3. 其他可通行区域（包括边界通道）距离为无穷大 else { - distanceMap[x, y] = int.MaxValue; + distanceMap[x, y] = double.MaxValue; } } } - - LogManager.Info($"【统一膨胀】膨胀源点统计 - 障碍物: {obstacleCount}, Unknown: {unknownCount}"); - - LogManager.Info($"【高效膨胀】距离矩阵初始化完成，耗时: {stopwatch.ElapsedMilliseconds}ms"); - - // 正向扫描（从左上到右下） + + LogManager.Info($"【8连通膨胀】膨胀源点统计 - 障碍物: {obstacleCount}, Unknown: {unknownCount}"); + + // 使用标准的距离变换算法 - 正向扫描 for (int x = 0; x < gridMap.Width; x++) { for (int y = 0; y < gridMap.Height; y++) { - if (distanceMap[x, y] != 0 && distanceMap[x, y] != int.MaxValue) + if (distanceMap[x, y] == double.MaxValue) { - // 检查左侧和上方的邻居 - int minDist = distanceMap[x, y]; - - if (x > 0 && distanceMap[x - 1, y] != int.MaxValue) - minDist = Math.Min(minDist, distanceMap[x - 1, y] + 1); - if (y > 0 && distanceMap[x, y - 1] != int.MaxValue) - minDist = Math.Min(minDist, distanceMap[x, y - 1] + 1); - if (x > 0 && y > 0 && distanceMap[x - 1, y - 1] != int.MaxValue) - minDist = Math.Min(minDist, distanceMap[x - 1, y - 1] + 1); - if (x > 0 && y < gridMap.Height - 1 && distanceMap[x - 1, y + 1] != int.MaxValue) - minDist = Math.Min(minDist, distanceMap[x - 1, y + 1] + 1); - - distanceMap[x, y] = minDist; - } - else if (distanceMap[x, y] == int.MaxValue) - { - // 对于可通行区域，计算到最近障碍物的距离 - int minDist = int.MaxValue; - - if (x > 0 && distanceMap[x - 1, y] != int.MaxValue) - minDist = Math.Min(minDist, distanceMap[x - 1, y] + 1); - if (y > 0 && distanceMap[x, y - 1] != int.MaxValue) - minDist = Math.Min(minDist, distanceMap[x, y - 1] + 1); - if (x > 0 && y > 0 && distanceMap[x - 1, y - 1] != int.MaxValue) - minDist = Math.Min(minDist, distanceMap[x - 1, y - 1] + 1); - if (x > 0 && y < gridMap.Height - 1 && distanceMap[x - 1, y + 1] != int.MaxValue) - minDist = Math.Min(minDist, distanceMap[x - 1, y + 1] + 1); - - if (minDist != int.MaxValue) + double minDist = double.MaxValue; + + // 检查左侧和上方的邻居（4连通） + if (x > 0) + minDist = Math.Min(minDist, distanceMap[x - 1, y] + 1.0); + if (y > 0) + minDist = Math.Min(minDist, distanceMap[x, y - 1] + 1.0); + + // 检查对角线邻居（8连通扩展） + if (x > 0 && y > 0) + minDist = Math.Min(minDist, distanceMap[x - 1, y - 1] + SQRT2); + if (x > 0 && y < gridMap.Height - 1) + minDist = Math.Min(minDist, distanceMap[x - 1, y + 1] + SQRT2); + + if (minDist != double.MaxValue) distanceMap[x, y] = minDist; } } } - - LogManager.Info($"【高效膨胀】正向扫描完成，耗时: {stopwatch.ElapsedMilliseconds}ms"); - - // 反向扫描（从右下到左上） + + LogManager.Info($"【8连通膨胀】正向扫描完成，耗时: {stopwatch.ElapsedMilliseconds}ms"); + + // 反向扫描 for (int x = gridMap.Width - 1; x >= 0; x--) { for (int y = gridMap.Height - 1; y >= 0; y--) { - if (distanceMap[x, y] != 0 && distanceMap[x, y] != int.MaxValue) + if (distanceMap[x, y] != 0.0) { - // 检查右侧和下方的邻居 - int minDist = distanceMap[x, y]; - - if (x < gridMap.Width - 1 && distanceMap[x + 1, y] != int.MaxValue) - minDist = Math.Min(minDist, distanceMap[x + 1, y] + 1); - if (y < gridMap.Height - 1 && distanceMap[x, y + 1] != int.MaxValue) - minDist = Math.Min(minDist, distanceMap[x, y + 1] + 1); - if (x < gridMap.Width - 1 && y < gridMap.Height - 1 && distanceMap[x + 1, y + 1] != int.MaxValue) - minDist = Math.Min(minDist, distanceMap[x + 1, y + 1] + 1); - if (x < gridMap.Width - 1 && y > 0 && distanceMap[x + 1, y - 1] != int.MaxValue) - minDist = Math.Min(minDist, distanceMap[x + 1, y - 1] + 1); - + double minDist = distanceMap[x, y]; + + // 检查右侧和下方的邻居（4连通） + if (x < gridMap.Width - 1) + minDist = Math.Min(minDist, distanceMap[x + 1, y] + 1.0); + if (y < gridMap.Height - 1) + minDist = Math.Min(minDist, distanceMap[x, y + 1] + 1.0); + + // 检查对角线邻居（8连通扩展） + if (x < gridMap.Width - 1 && y < gridMap.Height - 1) + minDist = Math.Min(minDist, distanceMap[x + 1, y + 1] + SQRT2); + if (x < gridMap.Width - 1 && y > 0) + minDist = Math.Min(minDist, distanceMap[x + 1, y - 1] + SQRT2); + distanceMap[x, y] = minDist; } - else if (distanceMap[x, y] == int.MaxValue) - { - // 对于可通行区域，计算到最近障碍物的距离 - int minDist = int.MaxValue; - - if (x < gridMap.Width - 1 && distanceMap[x + 1, y] != int.MaxValue) - minDist = Math.Min(minDist, distanceMap[x + 1, y] + 1); - if (y < gridMap.Height - 1 && distanceMap[x, y + 1] != int.MaxValue) - minDist = Math.Min(minDist, distanceMap[x, y + 1] + 1); - if (x < gridMap.Width - 1 && y < gridMap.Height - 1 && distanceMap[x + 1, y + 1] != int.MaxValue) - minDist = Math.Min(minDist, distanceMap[x + 1, y + 1] + 1); - if (x < gridMap.Width - 1 && y > 0 && distanceMap[x + 1, y - 1] != int.MaxValue) - minDist = Math.Min(minDist, distanceMap[x + 1, y - 1] + 1); - - if (minDist != int.MaxValue) - distanceMap[x, y] = minDist; - } } } - - LogManager.Info($"【高效膨胀】反向扫描完成，耗时: {stopwatch.ElapsedMilliseconds}ms"); + + LogManager.Info($"【8连通膨胀】反向扫描完成，耗时: {stopwatch.ElapsedMilliseconds}ms"); // 应用膨胀结果 int inflatedCells = 0; @@ -728,9 +695,9 @@ namespace NavisworksTransport.PathPlanning int totalCells = gridMap.Width * gridMap.Height; int skippedCells = 0; int protectedDoorCells = 0; - - LogManager.Info($"【高效膨胀】开始应用膨胀结果，总单元格数: {totalCells}"); - + + LogManager.Info($"【8连通膨胀】开始应用膨胀结果，总单元格数: {totalCells}"); + try { for (int x = 0; x < gridMap.Width; x++) @@ -738,26 +705,26 @@ namespace NavisworksTransport.PathPlanning for (int y = 0; y < gridMap.Height; y++) { processedCells++; - + // 每处理50万个单元格输出一次进度 if (processedCells % 500000 == 0) { double progress = (double)processedCells / totalCells * 100; - LogManager.Info($"【高效膨胀】应用进度: {progress:F1}% ({processedCells}/{totalCells})"); + LogManager.Info($"【8连通膨胀】应用进度: {progress:F1}% ({processedCells}/{totalCells})"); } - + var gridPos = new GridPoint2D(x, y); var cell = gridMap.Cells[x, y]; - + // 只有满足以下所有条件才进行膨胀： // 1. 当前位置原本是可通行的 // 2. 距离值不是无穷大（有有效的距离计算） - // 3. 距离小于等于膨胀半径 + // 3. 距离小于等于膨胀半径（使用浮点数比较） // 4. 距离值大于0（不是原始障碍物） // 5. 不是门类型（门保护） - if (gridMap.IsWalkable(gridPos) && - distanceMap[x, y] != int.MaxValue && - distanceMap[x, y] > 0 && + if (gridMap.IsWalkable(gridPos) && + distanceMap[x, y] != double.MaxValue && + distanceMap[x, y] > 0.0 && distanceMap[x, y] <= inflationRadius) { // 门元素保护：跳过门类型的网格，不将其膨胀为障碍物 @@ -767,11 +734,11 @@ namespace NavisworksTransport.PathPlanning // 跳过门网格，保持其可通行状态 continue; } - + gridMap.SetCell(gridPos, false, double.MaxValue, CategoryAttributeManager.LogisticsElementType.障碍物); inflatedCells++; } - else if (distanceMap[x, y] == int.MaxValue || distanceMap[x, y] > inflationRadius) + else if (distanceMap[x, y] == double.MaxValue || distanceMap[x, y] > inflationRadius) { skippedCells++; } @@ -780,15 +747,15 @@ namespace NavisworksTransport.PathPlanning } catch (Exception ex) { - LogManager.Error($"【高效膨胀】应用膨胀结果时发生错误: {ex.Message}，已处理{processedCells}/{totalCells}个单元格"); + LogManager.Error($"【8连通膨胀】应用膨胀结果时发生错误: {ex.Message}，已处理{processedCells}/{totalCells}个单元格"); throw; } - + // 单独处理包围盒边界膨胀 int boundaryInflatedCells = ApplyBoundaryInflationPost(gridMap, inflationRadius); - + stopwatch.Stop(); - LogManager.Info($"【高效膨胀】车辆膨胀完成: 膨胀半径={inflationRadius}格, 新增障碍物={inflatedCells}个, 边界膨胀={boundaryInflatedCells}个, 跳过={skippedCells}个, 保护门网格={protectedDoorCells}个, 总耗时={stopwatch.ElapsedMilliseconds}ms"); + LogManager.Info($"【8连通膨胀】车辆膨胀完成: 膨胀半径={inflationRadius}格, 新增障碍物={inflatedCells}个, 边界膨胀={boundaryInflatedCells}个, 跳过={skippedCells}个, 保护门网格={protectedDoorCells}个, 总耗时={stopwatch.ElapsedMilliseconds}ms"); } /// @@ -809,7 +776,7 @@ namespace NavisworksTransport.PathPlanning { int layerInflatedCount = 0; - // 处理上边界 (y = layer) + // 处理下边界 (y = layer) if (layer < gridMap.Height) { for (int x = 0; x < gridMap.Width; x++) @@ -823,7 +790,7 @@ namespace NavisworksTransport.PathPlanning } } - // 处理下边界 (y = maxY - layer) + // 处理上边界 (y = maxY - layer) if (gridMap.Height - 1 - layer >= 0 && gridMap.Height - 1 - layer != layer) // 避免重复处理 { for (int x = 0; x < gridMap.Width; x++) @@ -953,9 +920,72 @@ namespace NavisworksTransport.PathPlanning LogManager.Info($"[Search API优化] Search API阶段完成: 获取 {geometryItems.Count()} 个几何体项目，耗时: {searchElapsed:F1}ms"); LogManager.Info($"[Search API优化] 几何体筛选效果: 过滤掉 {filteredByGeometry} 个无几何体项目，筛选率: {geometryFilterRatio:P1}"); - // 🔥 关键优化：直接返回Search API结果，移除所有昂贵的LINQ统计调用 + // 🔥 关键优化：使用PrimitiveTypes精确过滤线性几何体 // 基于多模型测试验证，LINQ筛选条件(IsCollection, IsComposite, Children.Any)都返回0项 - var filteredItems = geometryItems.ToList(); + LogManager.Info("[Search API优化] 开始使用PrimitiveTypes过滤线性几何体"); + var primitiveFilterStart = DateTime.Now; + + int triangleCount = 0; + int lineOnlyCount = 0; + int pointOnlyCount = 0; + int textOnlyCount = 0; + int mixedCount = 0; + int noGeometryCount = 0; + + var filteredItems = geometryItems.Where(item => + { + try + { + var geometry = item.Geometry; + if (geometry == null) + { + noGeometryCount++; + return false; + } + + var primitiveTypes = geometry.PrimitiveTypes; + + // 统计各种图元类型 + if (primitiveTypes.HasFlag(PrimitiveTypes.Triangles)) + { + if (primitiveTypes == PrimitiveTypes.Triangles) + triangleCount++; + else + mixedCount++; + return true; // 包含三角形的几何体都保留 + } + else if (primitiveTypes == PrimitiveTypes.Lines) + { + lineOnlyCount++; + return false; // 纯线条几何体过滤掉 + } + else if (primitiveTypes == PrimitiveTypes.Points) + { + pointOnlyCount++; + return false; // 纯点几何体过滤掉 + } + else if (primitiveTypes == PrimitiveTypes.Text) + { + textOnlyCount++; + return false; // 纯文本几何体过滤掉 + } + else + { + mixedCount++; + return true; // 其他混合类型保留 + } + } + catch + { + return false; // 出错时跳过该项目 + } + }).ToList(); + + var primitiveFilterElapsed = (DateTime.Now - primitiveFilterStart).TotalMilliseconds; + var filteredByPrimitives = geometryItems.Count() - filteredItems.Count; + + LogManager.Info($"[Search API优化] PrimitiveTypes过滤完成: 过滤掉 {filteredByPrimitives} 个线性几何体，耗时: {primitiveFilterElapsed:F1}ms"); + LogManager.Info($"[Search API优化] 图元类型统计 - 纯三角形: {triangleCount}, 纯线条: {lineOnlyCount}, 纯点: {pointOnlyCount}, 纯文本: {textOnlyCount}, 混合类型: {mixedCount}, 无几何体: {noGeometryCount}"); var totalElapsed = (DateTime.Now - startTime).TotalMilliseconds; var totalFilteredCount = totalModelItems - filteredItems.Count; @@ -982,8 +1012,7 @@ namespace NavisworksTransport.PathPlanning List geometryItems, HashSet channelItemsSet, GridMap gridMap, - double scanHeightInModelUnits, - double minChannelTopZ) + double scanHeightInModelUnits) { LogManager.Info("[轻量级后处理] 开始处理Search API筛选结果"); var startTime = DateTime.Now; @@ -993,7 +1022,6 @@ namespace NavisworksTransport.PathPlanning var apiCalls = 0; var skippedByChannel = 0; var skippedByBoundingBox = 0; - var skippedByHeight = 0; foreach (var item in geometryItems) { @@ -1019,16 +1047,6 @@ namespace NavisworksTransport.PathPlanning continue; // 跳过通道子项目 } - // 移除容器检查：基于4个模型测试验证，所有几何体项目的容器检查结果都是0 - // 注释：原有的容器检查代码 - // API调用1: Children.Any() - 检查是否为容器 - // properties.IsContainer = item.Children?.Any() ?? false; - // apiCalls++; - // if (properties.IsContainer) { - // skippedByContainer++; - // continue; // 跳过容器项目 - // } - // 设为已知值（避免后续筛选中的判断错误） properties.IsContainer = false; // 基于测试验证，几何体项目都不是容器 @@ -1041,14 +1059,9 @@ namespace NavisworksTransport.PathPlanning skippedByBoundingBox++; continue; // 跳过无边界框项目 } - - // 快速计算：高度范围检查（纯数值计算，无API调用） - properties.IsInScanHeightRange = IsInScanHeightRange(properties.BoundingBox, minChannelTopZ, scanHeightInModelUnits); - if (!properties.IsInScanHeightRange) - { - skippedByHeight++; - continue; // 跳过高度范围外项目 - } + + // 设置为默认值，实际的高度检查在后续阶段进行 + properties.IsInScanHeightRange = true; // 只有通过所有筛选的项目才加入结果 itemCache[item] = properties; @@ -1061,7 +1074,8 @@ namespace NavisworksTransport.PathPlanning var elapsed = (DateTime.Now - startTime).TotalMilliseconds; LogManager.Info($"[轻量级后处理] 完成，输入 {totalItems} 项，有效结果 {itemCache.Count} 项，API调用 {apiCalls} 次，耗时: {elapsed:F1}ms"); - LogManager.Info($"[轻量级后处理] 筛除统计 - 通道相关: {skippedByChannel}, 无边界框: {skippedByBoundingBox}, 高度范围外: {skippedByHeight}"); + LogManager.Info($"[轻量级后处理] 筛除统计 - 通道相关: {skippedByChannel}, 无边界框: {skippedByBoundingBox}"); + LogManager.Info("[轻量级后处理] 已移除全局高度预过滤，将在障碍物遍历阶段进行精确高度检查"); return itemCache; } @@ -1074,8 +1088,7 @@ namespace NavisworksTransport.PathPlanning /// 网格地图 /// 通道模型项列表 /// 扫描高度范围（模型单位） - /// 最小通道顶面高度，用作扫描基准 - private void ProcessObstaclesWithBoundingBoxOptimized(Document document, GridMap gridMap, HashSet channelItems, double scanHeightInModelUnits, double minChannelTopZ) + private void ProcessObstaclesWithBoundingBoxOptimized(Document document, GridMap gridMap, HashSet channelItems, double scanHeightInModelUnits) { try { @@ -1090,7 +1103,7 @@ namespace NavisworksTransport.PathPlanning LogManager.Info($"[高性能障碍物处理] 输入统计 - 几何体项目: {geometryItems.Count}, 通道元素: {channelItemsSet.Count}"); // 阶段2：轻量级后处理（只对预筛选结果进行必要的API调用） - var itemCache = PostProcessGeometryItems(geometryItems, channelItemsSet, gridMap, scanHeightInModelUnits, minChannelTopZ); + var itemCache = PostProcessGeometryItems(geometryItems, channelItemsSet, gridMap, scanHeightInModelUnits); // 阶段3：条件过滤（并行数值计算）- 使用50%CPU核心优化 LogManager.Info("[高性能障碍物处理] 阶段3: 并行条件过滤"); @@ -1113,11 +1126,38 @@ namespace NavisworksTransport.PathPlanning var filterElapsed = (DateTime.Now - filterStart).TotalMilliseconds; LogManager.Info($"[高性能障碍物处理] 阶段3完成: 从缓存中筛选出 {validItems.Count} 个有效项，耗时: {filterElapsed:F1}ms"); - // 阶段4：网格覆盖计算（并行几何计算）- 使用50%CPU核心优化 - LogManager.Info("[高性能障碍物处理] 阶段4: 并行网格覆盖计算"); + // 阶段4：网格覆盖计算（并行几何计算）- 使用50%CPU核心优化 + 精确高度检查 + LogManager.Info("[高性能障碍物处理] 阶段4: 并行网格覆盖计算 + 精确高度检查"); var gridCalcStart = DateTime.Now; - - var gridUpdates = validItems.AsParallel() + + var heightCheckedItems = validItems.AsParallel() + .WithDegreeOfParallelism(optimalParallelism) + .Where(kvp => + { + // 精确高度检查：使用几何体中心点对应的网格高度 + var bbox = kvp.Value.BoundingBox; + var centerX = (bbox.Min.X + bbox.Max.X) / 2; + var centerY = (bbox.Min.Y + bbox.Max.Y) / 2; + + var centerGridPos = gridMap.WorldToGrid(new Point3D(centerX, centerY, 0)); + if (!gridMap.IsValidGridPosition(centerGridPos)) + { + return false; + } + + var centerCell = gridMap.Cells[centerGridPos.X, centerGridPos.Y]; + var referenceZ = centerCell.WorldPosition.Z; + var scanMin = referenceZ; + var scanMax = referenceZ + scanHeightInModelUnits; + + // 只有与该网格高度范围重叠的几何体才进入处理 + bool isInRange = !(bbox.Max.Z < scanMin || bbox.Min.Z > scanMax); + + return isInRange; + }) + .ToList(); + + var gridUpdates = heightCheckedItems.AsParallel() .WithDegreeOfParallelism(optimalParallelism) .SelectMany(kvp => { @@ -1131,9 +1171,13 @@ namespace NavisworksTransport.PathPlanning }); }) .ToList(); - + var gridCalcElapsed = (DateTime.Now - gridCalcStart).TotalMilliseconds; + var totalCheckedItems = validItems.Count; + var filteredItems = totalCheckedItems - heightCheckedItems.Count; + LogManager.Info($"[高性能障碍物处理] 阶段4完成: 生成 {gridUpdates.Count} 个网格更新操作，耗时: {gridCalcElapsed:F1}ms"); + LogManager.Info($"[精确高度检查] 检查了 {totalCheckedItems} 个几何体，过滤掉 {filteredItems} 个高度范围外的几何体，保留 {heightCheckedItems.Count} 个"); // 阶段5：网格状态更新（单线程写入） LogManager.Info("[高性能障碍物处理] 阶段5: 单线程网格状态更新"); @@ -1184,6 +1228,7 @@ namespace NavisworksTransport.PathPlanning // 性能对比统计 LogManager.Info($"[性能优化统计] 处理几何体项目: {geometryItems.Count}, 最终有效项目: {validItems.Count}, 优化比率: {(1.0 - (double)validItems.Count / geometryItems.Count):P1}"); + LogManager.Info($"[精确高度过滤统计] 高度检查项目: {totalCheckedItems}, 高度过滤项目: {filteredItems}, 精确过滤率: {(double)filteredItems / Math.Max(1, totalCheckedItems):P1}"); LogManager.Info($"[并行优化统计] 使用 {optimalParallelism}/{Environment.ProcessorCount} CPU核心，每核心处理 {Math.Ceiling((double)validItems.Count / optimalParallelism)} 个项目"); } catch (Exception ex) @@ -1196,6 +1241,7 @@ namespace NavisworksTransport.PathPlanning /// /// 计算包围盒在网格上覆盖的单元坐标 + /// 严格按照"网格坐标代表左下角"的设计原则进行边界处理 /// /// 包围盒 /// 网格地图 @@ -1203,19 +1249,27 @@ namespace NavisworksTransport.PathPlanning private List<(int x, int y)> CalculateBoundingBoxGridCoverage(BoundingBox3D bbox, GridMap gridMap) { var coveredCells = new List<(int x, int y)>(); - + try { - // 将包围盒的世界坐标转换为网格坐标 + // 网格坐标(x,y)代表左下角，覆盖世界坐标范围： + // X: [Origin.X + x*CellSize, Origin.X + (x+1)*CellSize) + // Y: [Origin.Y + y*CellSize, Origin.Y + (y+1)*CellSize) + + // 直接使用WorldToGrid进行坐标转换 var minGridPos = gridMap.WorldToGrid(new Point3D(bbox.Min.X, bbox.Min.Y, 0)); var maxGridPos = gridMap.WorldToGrid(new Point3D(bbox.Max.X, bbox.Max.Y, 0)); - + // 确保坐标在有效范围内 - int minX = Math.Max(0, Math.Min(minGridPos.X, maxGridPos.X)); - int minY = Math.Max(0, Math.Min(minGridPos.Y, maxGridPos.Y)); - int maxX = Math.Min(gridMap.Width - 1, Math.Max(minGridPos.X, maxGridPos.X)); - int maxY = Math.Min(gridMap.Height - 1, Math.Max(minGridPos.Y, maxGridPos.Y)); - + int minX = Math.Max(0, minGridPos.X); + int minY = Math.Max(0, minGridPos.Y); + int maxX = Math.Min(gridMap.Width - 1, maxGridPos.X); + int maxY = Math.Min(gridMap.Height - 1, maxGridPos.Y); + + // 确保至少覆盖一个网格（防止空包围盒） + if (maxX < minX) maxX = minX; + if (maxY < minY) maxY = minY; + // 遍历覆盖的矩形区域 for (int x = minX; x <= maxX; x++) { @@ -1224,12 +1278,14 @@ namespace NavisworksTransport.PathPlanning coveredCells.Add((x, y)); } } + + LogManager.Debug($"[包围盒覆盖计算] 包围盒[{bbox.Min.X:F2},{bbox.Min.Y:F2}]-[{bbox.Max.X:F2},{bbox.Max.Y:F2}] → 网格[{minX},{minY}]-[{maxX},{maxY}]，覆盖{coveredCells.Count}个网格"); } catch (Exception ex) { LogManager.Debug($"[包围盒覆盖计算] 计算失败: {ex.Message}"); } - + return coveredCells; } @@ -1310,29 +1366,21 @@ namespace NavisworksTransport.PathPlanning LogManager.Debug($"【门开口计算】{doorItem.DisplayName} 开口区域坐标: [{minX:F2},{minY:F2}]-[{maxX:F2},{maxY:F2}]"); - // 7. 将开口区域坐标直接转换为网格坐标 + // 7. 将开口区域坐标转换为网格坐标（严格按照左下角原则） + var minGridPos = gridMap.WorldToGrid(new Point3D(minX, minY, 0)); + var maxGridPos = gridMap.WorldToGrid(new Point3D(maxX, maxY, 0)); - + // 确保坐标在有效范围内 - int gridMinX = Math.Max(0, Math.Min(minGridPos.X, maxGridPos.X)); - int gridMinY = Math.Max(0, Math.Min(minGridPos.Y, maxGridPos.Y)); - int gridMaxX = Math.Min(gridMap.Width - 1, Math.Max(minGridPos.X, maxGridPos.X)); - int gridMaxY = Math.Min(gridMap.Height - 1, Math.Max(minGridPos.Y, maxGridPos.Y)); + int gridMinX = Math.Max(0, minGridPos.X); + int gridMinY = Math.Max(0, minGridPos.Y); + int gridMaxX = Math.Min(gridMap.Width - 1, maxGridPos.X); + int gridMaxY = Math.Min(gridMap.Height - 1, maxGridPos.Y); // 确保至少有1个网格可通行 - if (gridMinX >= gridMaxX) - { - int centerX = (gridMinX + gridMaxX) / 2; - gridMinX = Math.Max(0, centerX); - gridMaxX = Math.Min(gridMap.Width - 1, centerX); - } - if (gridMinY >= gridMaxY) - { - int centerY = (gridMinY + gridMaxY) / 2; - gridMinY = Math.Max(0, centerY); - gridMaxY = Math.Min(gridMap.Height - 1, centerY); - } + if (gridMaxX < gridMinX) gridMaxX = gridMinX; + if (gridMaxY < gridMinY) gridMaxY = gridMinY; // 8. 生成覆盖的网格单元列表 for (int x = gridMinX; x <= gridMaxX; x++) @@ -1354,32 +1402,6 @@ namespace NavisworksTransport.PathPlanning return coveredCells; } - - /// - /// 检查包围盒是否在扫描高度范围内 - /// - /// 包围盒 - /// 最小通道顶面高度，用作扫描基准（模型单位） - /// 扫描高度范围（模型单位） - /// 是否在高度范围内 - private bool IsInScanHeightRange(BoundingBox3D bbox, double minChannelTopZ, double scanHeightInModelUnits) - { - try - { - // 使用最低通道顶面高度作为扫描基准，避免门高度干扰 - var scanMinZ = minChannelTopZ; // 最低通道顶面（模型单位） - var scanMaxZ = minChannelTopZ + scanHeightInModelUnits; // 从最低通道顶面向上扫描（模型单位） - - // 检查包围盒的Z范围是否与扫描范围有重叠 - return !(bbox.Max.Z < scanMinZ || bbox.Min.Z > scanMaxZ); - } - catch (Exception ex) - { - LogManager.Debug($"[高度范围检查] 检查失败: {ex.Message}"); - return true; // 出错时保守处理，包含该项 - } - } - /// /// 检查模型项是否是通道项的子项 ///