From 59ecebebc4d51f804e71e6da25121c1efe3ae71b Mon Sep 17 00:00:00 2001
From: tian <11429339@qq.com>
Date: Fri, 10 Oct 2025 11:03:38 +0800
Subject: [PATCH] =?UTF-8?q?=E6=94=AF=E6=8C=81=E4=BA=863D=E9=9A=9C=E7=A2=8D?=
 =?UTF-8?q?=E7=89=A9=E6=8A=95=E5=BD=B1?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/PathPlanning/GridMapGenerator.cs | 81 ++++++++++++++++++++++------
 1 file changed, 65 insertions(+), 16 deletions(-)

diff --git a/src/PathPlanning/GridMapGenerator.cs b/src/PathPlanning/GridMapGenerator.cs
index 825023b..3e50ca2 100644
--- a/src/PathPlanning/GridMapGenerator.cs
+++ b/src/PathPlanning/GridMapGenerator.cs
@@ -1294,31 +1294,78 @@ namespace NavisworksTransport.PathPlanning
                 LogManager.Info($"[高性能障碍物处理] 阶段4完成: 生成 {gridUpdates.Count} 个网格更新操作，耗时: {gridCalcElapsed:F1}ms");
                 LogManager.Info($"[精确高度检查] 检查了 {totalCheckedItems} 个几何体，过滤掉 {filteredItems} 个高度范围外的几何体，保留 {heightCheckedItems.Count} 个");
                 
-                // 阶段5：网格状态更新（单线程写入）
-                LogManager.Info("[高性能障碍物处理] 阶段5: 单线程网格状态更新");
+                // 阶段5：网格状态更新（单线程写入，3D高度检查）
+                LogManager.Info("[高性能障碍物处理] 阶段5: 单线程网格状态更新（3D高度检查）");
                 var updateStart = DateTime.Now;
-                
+
                 var updatedCells = 0;
                 var obstacleItems = 0;
-                
+                var layersRemoved = 0;
+                var cellsFullyBlocked = 0;
+                var cellsWithLayers = 0;
+                var totalLayersChecked = 0;
+
                 foreach (var update in gridUpdates)
                 {
                     var cell = gridMap.Cells[update.X, update.Y];
-                    
+
                     // 只更新通道网格，跳过已是障碍物的网格
                     if (cell.CellType == CategoryAttributeManager.LogisticsElementType.通道 && cell.IsInChannel)
                     {
-                        // 标记为障碍物
-                        cell.IsWalkable = false;
-                        cell.CellType = CategoryAttributeManager.LogisticsElementType.障碍物;
-                        cell.Cost = double.MaxValue;
-                        cell.IsInChannel = false;
-                        cell.RelatedModelItem = update.Item;
-                        
-                        // 多层架构：障碍物不需要PassableHeight
-                        // var groundHeight = cell.WorldPosition.Z;
-                        // cell.PassableHeight = new HeightInterval(...)
-                        
+                        var bbox = update.BoundingBox;
+
+                        // 统计有多少网格有高度层
+                        if (cell.HeightLayers != null && cell.HeightLayers.Count > 0)
+                        {
+                            cellsWithLayers++;
+                            totalLayersChecked += cell.HeightLayers.Count;
+                        }
+
+                        // 3D高度检查：只移除与障碍物高度重叠的层
+                        var remainingLayers = new List<HeightLayer>();
+
+                        foreach (var layer in cell.HeightLayers)
+                        {
+                            // 计算该层的高度范围
+                            double layerMinZ = layer.Z;
+                            double layerMaxZ = layer.Z + layer.PassableHeight.GetSpan();
+
+                            // 障碍物的高度范围
+                            double obstacleMinZ = bbox.Min.Z;
+                            double obstacleMaxZ = bbox.Max.Z;
+
+                            // 检查是否重叠
+                            bool overlaps = !(obstacleMaxZ <= layerMinZ || obstacleMinZ > layerMaxZ);
+
+                            if (!overlaps)
+                            {
+                                // 不重叠，保留该层
+                                remainingLayers.Add(layer);
+                            }
+                            else
+                            {
+                                // 重叠，移除该层
+                                layersRemoved++;
+                                LogManager.Debug($"[3D高度检查] 网格({update.X},{update.Y}) 层Z={layer.Z:F2}, 高度范围=[{layerMinZ:F2},{layerMaxZ:F2}], 障碍物范围=[{obstacleMinZ:F2},{obstacleMaxZ:F2}], 重叠=移除");
+                            }
+                        }
+
+                        // 更新网格的高度层列表
+                        cell.HeightLayers = remainingLayers;
+
+                        // 如果所有层都被移除，标记整个网格为障碍物
+                        if (remainingLayers.Count == 0)
+                        {
+                            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++;
                     }
@@ -1327,6 +1374,8 @@ 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高度检查] {cellsFullyBlocked} 个网格被完全阻挡，{updatedCells - cellsFullyBlocked} 个网格保留了部分层");
                 
                 // 输出总体统计
                 var totalElapsed = (DateTime.Now - startTime).TotalMilliseconds;