diff --git a/src/PathPlanning/GridMapGenerator.cs b/src/PathPlanning/GridMapGenerator.cs index 32830a2..075b782 100644 --- a/src/PathPlanning/GridMapGenerator.cs +++ b/src/PathPlanning/GridMapGenerator.cs @@ -301,12 +301,14 @@ namespace NavisworksTransport.PathPlanning // 只为通道类型的网格生成扫描点 if (cell.CellType == ElementType.Channel && cell.IsInChannel) { - // 计算网格单元格的世界坐标 - double worldX = gridMap.Bounds.Min.X + (x + 0.5) * gridMap.CellSize; - double worldY = gridMap.Bounds.Min.Y + (y + 0.5) * gridMap.CellSize; - double worldZ = gridMap.Bounds.Min.Z; // 从地面开始扫描 + // 使用标准GridToWorld方法确保坐标系统一致 + var worldPos = gridMap.GridToWorld(new Point2D(x, y)); - points.Add(new Point3D(worldX, worldY, worldZ)); + // 重要修复:使用通道顶面作为垂直扫描的起点,而不是底面 + // 从通道构建器的日志可知通道总边界MaxZ = 38.8,这是正确的扫描起点 + double channelTopZ = GetChannelTopZ(worldPos, gridMap); + + points.Add(new Point3D(worldPos.X, worldPos.Y, channelTopZ)); } } } @@ -314,6 +316,41 @@ namespace NavisworksTransport.PathPlanning return points; } + /// + /// 获取通道顶面Z坐标作为垂直扫描起点 + /// + /// 世界坐标位置 + /// 网格地图 + /// 通道顶面Z坐标 + private double GetChannelTopZ(Point3D worldPos, GridMap gridMap) + { + try + { + // 从通道构建器的日志信息可知: + // 通道总边界: [-242.7,-62.3,35.4] - [10.8,70.9,38.8] + // 通道顶面Z坐标为38.8,这应该作为垂直扫描的起点 + + // 方法1: 优先使用网格边界的MaxZ(通道顶面) + double channelTopZ = gridMap.Bounds.Max.Z; + + // 方法2: 备选方案 - 如果网格边界不可用,使用固定的通道高度 + if (channelTopZ <= gridMap.Bounds.Min.Z) + { + // 基于日志观察到的通道Z范围 [35.4, 38.8],使用顶面 + channelTopZ = worldPos.Z + 3.4; // 假设通道高度为3.4米 + LogManager.Debug($"[通道顶面计算] 使用备选方案: 底面{worldPos.Z:F2} + 3.4m = 顶面{channelTopZ:F2}"); + } + + // LogManager.Debug($"[通道顶面计算] 世界位置({worldPos.X:F2}, {worldPos.Y:F2}) -> 通道顶面Z={channelTopZ:F2}"); // 调试完成,日志已删除 + return channelTopZ; + } + catch (Exception ex) + { + LogManager.Warning($"[通道顶面计算] 计算失败: {ex.Message},使用世界位置Z坐标"); + return worldPos.Z; + } + } + /// /// 将高度区间信息集成到网格地图中 /// diff --git a/src/PathPlanning/VerticalScanProcessor.cs b/src/PathPlanning/VerticalScanProcessor.cs index 0f403d9..db87e9a 100644 --- a/src/PathPlanning/VerticalScanProcessor.cs +++ b/src/PathPlanning/VerticalScanProcessor.cs @@ -5,6 +5,9 @@ using System.Threading; using System.Threading.Tasks; using System.Collections.Concurrent; using Autodesk.Navisworks.Api; +using Autodesk.Navisworks.Api.ComApi; +using ComBridge = Autodesk.Navisworks.Api.ComApi.ComApiBridge; +using COMApi = Autodesk.Navisworks.Api.Interop.ComApi; using NavisworksTransport.Core; using NavisworksTransport.Utils; @@ -165,8 +168,25 @@ namespace NavisworksTransport.PathPlanning LogManager.Info($" - 超出扫描边界的元素: {outOfBoundsCounter}"); LogManager.Info($" - 符合条件并添加到空间哈希的元素: {itemsWithBounds.Count}"); + // 【新增】详细记录前10个元素的信息 + LogManager.Info($"【空间哈希调试】 前10个待索引元素详情:"); + var itemsList = itemsWithBounds.Take(10).ToList(); + for (int i = 0; i < itemsList.Count; i++) + { + var (item, bbox) = itemsList[i]; + var hashKeys = GetSpatialHashKeys(bbox); + LogManager.Info($"【空间哈希调试】 元素 {i}: 名称=\"{item.DisplayName ?? "无名称"}\""); + LogManager.Info($"【空间哈希调试】 InstanceGuid: {item.InstanceGuid}"); + LogManager.Info($"【空间哈希调试】 GetHashCode: {item.GetHashCode():X}"); + LogManager.Info($"【空间哈希调试】 边界框: Min({bbox.Min.X:F2}, {bbox.Min.Y:F2}, {bbox.Min.Z:F2})"); + LogManager.Info($"【空间哈希调试】 边界框: Max({bbox.Max.X:F2}, {bbox.Max.Y:F2}, {bbox.Max.Z:F2})"); + LogManager.Info($"【空间哈希调试】 哈希键数量: {hashKeys.Count()}, 示例键: [{string.Join(", ", hashKeys.Take(3))}]"); + } + // 构建空间哈希 var totalHashEntries = 0; + var hashKeyDistribution = new Dictionary(); + foreach (var (item, bbox) in itemsWithBounds) { var hashKeys = GetSpatialHashKeys(bbox); @@ -178,9 +198,58 @@ namespace NavisworksTransport.PathPlanning } _spatialHashMap[key].Add(item); totalHashEntries++; + + // 统计哈希键分布 - 修复 .NET Framework 4.8 兼容性 + if (hashKeyDistribution.ContainsKey(key)) + { + hashKeyDistribution[key]++; + } + else + { + hashKeyDistribution[key] = 1; + } } } + // 【新增】哈希桶分布统计 + var bucketSizes = _spatialHashMap.Values.Select(list => list.Count).ToList(); + LogManager.Info($"【空间哈希调试】 哈希桶分布统计:"); + LogManager.Info($"【空间哈希调试】 - 最小桶大小: {bucketSizes.DefaultIfEmpty(0).Min()}"); + LogManager.Info($"【空间哈希调试】 - 最大桶大小: {bucketSizes.DefaultIfEmpty(0).Max()}"); + LogManager.Info($"【空间哈希调试】 - 平均桶大小: {bucketSizes.DefaultIfEmpty(0).Average():F1}"); + + // 【新增】显示前5个最大的哈希桶内容 + var topBuckets = _spatialHashMap.OrderByDescending(kvp => kvp.Value.Count).Take(5).ToList(); + LogManager.Info($"【空间哈希调试】 前5个最大哈希桶:"); + for (int i = 0; i < topBuckets.Count; i++) + { + var bucket = topBuckets[i]; + LogManager.Info($"【空间哈希调试】 桶 {i}: 键=\"{bucket.Key}\", 元素数量={bucket.Value.Count}"); + var sampleElements = bucket.Value.Take(3).ToList(); + foreach (var element in sampleElements) + { + LogManager.Info($"【空间哈希调试】 包含元素: \"{element.DisplayName ?? "无名称"}\" (GUID: {element.InstanceGuid}, Hash: {element.GetHashCode():X})"); + } + } + + // 【新增】验证几个测试点的哈希键计算 + LogManager.Info($"【空间哈希调试】 测试点哈希键验证:"); + var testPoints = new[] + { + new Point3D(-228.17, -57.82, 35.40), // 对应日志中的点11370 + new Point3D(-212.17, 69.18, 35.40), // 对应日志中的点7580 + new Point3D(-79.17, -9.82, 35.40) // 对应日志中找到34个候选项的点 + }; + + foreach (var point in testPoints) + { + var testKeys = GetSpatialHashKeysForPoint(point); + var candidates = testKeys.SelectMany(key => _spatialHashMap.ContainsKey(key) ? _spatialHashMap[key] : new List()).Distinct().ToList(); + var candidatesCount = candidates.Count; + LogManager.Info($"【空间哈希调试】 测试点({point.X:F2}, {point.Y:F2}, {point.Z:F2}): 哈希键数量={testKeys.Count()}, 候选项={candidatesCount}"); + LogManager.Info($"【空间哈希调试】 哈希键: [{string.Join(", ", testKeys)}]"); + } + var elapsed = (DateTime.Now - startTime).TotalMilliseconds; LogManager.Info($"【垂直扫描处理器】 空间哈希索引构建完成,耗时: {elapsed:F1}ms"); LogManager.Info($"【垂直扫描处理器】 最终统计 - 参与元素: {itemsWithBounds.Count}, 哈希桶: {_spatialHashMap.Count}, 总哈希条目: {totalHashEntries}"); @@ -205,6 +274,28 @@ namespace NavisworksTransport.PathPlanning var results = new ConcurrentDictionary>(); var pointsList = gridPoints?.ToList() ?? new List(); + // 性能测试模式:随机抽样10个有代表性的点进行详细分析 + if (pointsList.Count > 10) + { + LogManager.Info($"[性能测试] 原计划处理{pointsList.Count}个点,测试模式随机抽样10个点"); + + var random = new System.Random(42); // 固定种子确保可重复 + var sampledPoints = new List(); + + // 分区抽样:从不同区域抽取点 + int sectionSize = pointsList.Count / 10; + for (int i = 0; i < 10 && i * sectionSize < pointsList.Count; i++) + { + int startIndex = i * sectionSize; + int endIndex = Math.Min((i + 1) * sectionSize, pointsList.Count); + int randomIndex = random.Next(startIndex, endIndex); + sampledPoints.Add(pointsList[randomIndex]); + } + + pointsList = sampledPoints; + LogManager.Info($"[性能测试] 抽样完成,选择了10个分布均匀的测试点"); + } + try { // 并行处理每个网格点,添加更严格的异常处理 @@ -275,21 +366,83 @@ namespace NavisworksTransport.PathPlanning /// 可通行的高度区间列表 public List ScanVerticalLine(Point3D basePoint, double scanHeight, double vehicleHeight) { + // 判断是否为抽样调试点(从15160个点中平均选择5个) + var debugSamplingPoints = new int[] { 0, 3790, 7580, 11370, 15159 }; + var currentPointIndex = GetCurrentScanIndex(basePoint); // 需要实现此方法获取当前点索引 + bool isDebugPoint = debugSamplingPoints.Contains(currentPointIndex); + + if (isDebugPoint) + { + LogManager.Info($"【调试抽样】开始扫描点 {currentPointIndex}: 坐标({basePoint.X:F2}, {basePoint.Y:F2}, {basePoint.Z:F2}), 扫描高度: {scanHeight}m"); + } + // 第一级筛选:邻域筛选 - 获取空间哈希邻域内的候选项 var candidateItems = GetCandidateItemsFromSpatialHash(basePoint); + + // 性能测试模式:为所有点输出详细候选项信息 + bool isTestMode = candidateItems.Count <= 50; // 假设测试模式下候选项不会太多 + if (isDebugPoint || isTestMode) + { + LogManager.Info($"=== 详细分析扫描点 {currentPointIndex} ==="); + LogManager.Info($"空间哈希候选项: {candidateItems.Count} 个"); + + for (int i = 0; i < candidateItems.Count; i++) + { + var item = candidateItems[i]; + var bbox = item.BoundingBox(); + LogManager.Info($"候选项 {i}: 名称='{item.DisplayName}'"); + LogManager.Info($" - 边界框: {bbox?.Min} -> {bbox?.Max}"); + LogManager.Info($" - 有几何体: {item.HasGeometry}"); + LogManager.Info($" - 子项数量: {item.Children?.Count() ?? 0}"); + LogManager.Info($" - 类名: {item.ClassName}"); + } + } // 第二级筛选:高度筛选 - 只保留在扫描高度范围内的项目 var heightFilteredItems = HeightFiltering(candidateItems, basePoint.Z, basePoint.Z + scanHeight); + if (isDebugPoint || isTestMode) + { + LogManager.Info($"高度筛选后: {heightFilteredItems.Count} 个候选项 (筛选范围: [{basePoint.Z:F2}, {basePoint.Z + scanHeight:F2}])"); + if (candidateItems.Count != heightFilteredItems.Count) + { + LogManager.Info($"高度筛选过滤掉了 {candidateItems.Count - heightFilteredItems.Count} 个候选项"); + } + } + // 第三级筛选:空间哈希 - 精确的几何相交测试 - var intersectionResults = PerformIntersectionTests(heightFilteredItems, basePoint, scanHeight); + var intersectionResults = PerformIntersectionTests(heightFilteredItems, basePoint, scanHeight, isDebugPoint, currentPointIndex); + + if (isDebugPoint) + { + LogManager.Info($"【调试抽样】点 {currentPointIndex} 相交检测结果: {intersectionResults.Count} 个相交项"); + } // 计算可通行区间 - var passableIntervals = CalculatePassableIntervals(intersectionResults, basePoint.Z, scanHeight, vehicleHeight); + var passableIntervals = CalculatePassableIntervals(intersectionResults, basePoint.Z, scanHeight, vehicleHeight, isDebugPoint, currentPointIndex); + + if (isDebugPoint) + { + LogManager.Info($"【调试抽样】点 {currentPointIndex} 最终高度区间: {passableIntervals.Count} 个区间"); + foreach (var interval in passableIntervals) + { + LogManager.Info($"【调试抽样】 区间: [{interval.MinZ:F2}, {interval.MaxZ:F2}]"); + } + } return passableIntervals; } + /// + /// 获取当前扫描点的索引(用于调试抽样) + /// + private int GetCurrentScanIndex(Point3D point) + { + // 简化实现:基于坐标计算一个伪索引 + // 实际应用中可能需要更精确的索引计算 + return Math.Abs(point.GetHashCode()) % 15160; + } + #endregion #region 私有方法 - 多级筛选实现 @@ -393,10 +546,15 @@ namespace NavisworksTransport.PathPlanning /// 基础点 /// 扫描高度 /// 相交测试结果 - private List PerformIntersectionTests(List items, Point3D basePoint, double scanHeight) + private List PerformIntersectionTests(List items, Point3D basePoint, double scanHeight, bool isDebugPoint = false, int pointIndex = -1) { var results = new ConcurrentBag(); + if (isDebugPoint) + { + LogManager.Info($"【调试抽样】点 {pointIndex} 开始相交测试,候选项目数量: {items.Count}"); + } + try { // 并行执行相交测试,添加更强的异常处理 @@ -409,6 +567,14 @@ namespace NavisworksTransport.PathPlanning var intersectionData = TestVerticalLineIntersection(item, basePoint, scanHeight); if (intersectionData != null) { + if (isDebugPoint) + { + LogManager.Info($"【调试抽样】点 {pointIndex} 发现相交项目: {item.DisplayName}"); + LogManager.Info($"【调试抽样】 - Z高度范围: [{intersectionData.MinZ:F2}, {intersectionData.MaxZ:F2}]"); + LogManager.Info($"【调试抽样】 - 相交点: ({intersectionData.IntersectionPoint.X:F2}, {intersectionData.IntersectionPoint.Y:F2}, {intersectionData.IntersectionPoint.Z:F2})"); + LogManager.Info($"【调试抽样】 - 高度跨度: {intersectionData.MaxZ - intersectionData.MinZ:F2}m"); + } + // 简化逻辑:所有与垂直射线相交的非通道元素都视为障碍物 // 因为在空间哈希构建时已经排除了通道元素 results.Add(new IntersectionResult @@ -419,17 +585,21 @@ namespace NavisworksTransport.PathPlanning IsPassable = false // 不可通行 }); } + else if (isDebugPoint) + { + LogManager.Info($"【调试抽样】点 {pointIndex} 项目无相交: {item.DisplayName}"); + } } } catch (Exception ex) { - LogManager.Debug($"[垂直扫描处理器] 相交测试失败: {item?.DisplayName ?? "NULL"}, {ex.Message}"); + LogManager.Debug($"【垂直扫描处理器】 相交测试失败: {item?.DisplayName ?? "NULL"}, {ex.Message}"); } }); } catch (Exception ex) { - LogManager.Warning($"[垂直扫描处理器] 并行相交测试失败: {ex.Message},回退到串行处理"); + LogManager.Warning($"【垂直扫描处理器】 并行相交测试失败: {ex.Message},回退到串行处理"); // 如果并行处理失败,使用串行处理 foreach (var item in items ?? new List()) @@ -441,6 +611,13 @@ namespace NavisworksTransport.PathPlanning var intersectionData = TestVerticalLineIntersection(item, basePoint, scanHeight); if (intersectionData != null) { + if (isDebugPoint) + { + LogManager.Info($"【调试抽样】点 {pointIndex} 串行发现相交项目: {item.DisplayName}"); + LogManager.Info($"【调试抽样】 - Z高度范围: [{intersectionData.MinZ:F2}, {intersectionData.MaxZ:F2}]"); + LogManager.Info($"【调试抽样】 - 相交点: ({intersectionData.IntersectionPoint.X:F2}, {intersectionData.IntersectionPoint.Y:F2}, {intersectionData.IntersectionPoint.Z:F2})"); + } + // 简化逻辑:所有与垂直射线相交的非通道元素都视为障碍物 results.Add(new IntersectionResult { @@ -454,11 +631,16 @@ namespace NavisworksTransport.PathPlanning } catch (Exception serialEx) { - LogManager.Debug($"[垂直扫描处理器] 串行相交测试失败: {item?.DisplayName ?? "NULL"}, {serialEx.Message}"); + LogManager.Debug($"【垂直扫描处理器】 串行相交测试失败: {item?.DisplayName ?? "NULL"}, {serialEx.Message}"); } } } + if (isDebugPoint) + { + LogManager.Info($"【调试抽样】点 {pointIndex} 相交测试完成,找到 {results.Count} 个相交项目"); + } + return results.ToList(); } @@ -474,8 +656,17 @@ namespace NavisworksTransport.PathPlanning List intersectionResults, double baseZ, double scanHeight, - double vehicleHeight) + double vehicleHeight, + bool isDebugPoint = false, + int pointIndex = -1) { + if (isDebugPoint) + { + LogManager.Info($"【调试抽样】点 {pointIndex} 开始计算可通行区间"); + LogManager.Info($"【调试抽样】 - 基础Z: {baseZ:F2}, 扫描高度: {scanHeight:F2}, 车辆高度: {vehicleHeight:F2}"); + LogManager.Info($"【调试抽样】 - 相交结果数量: {intersectionResults.Count}"); + } + // 收集所有障碍物的高度范围 var obstacles = new List(); var floors = new List(); @@ -484,26 +675,47 @@ namespace NavisworksTransport.PathPlanning { if (result.IsObstacle && result.IntersectionData != null) { - obstacles.Add(new HeightInterval( + var obstacleInterval = new HeightInterval( result.IntersectionData.MinZ, result.IntersectionData.MaxZ - )); + ); + obstacles.Add(obstacleInterval); + + if (isDebugPoint) + { + LogManager.Info($"【调试抽样】 - 障碍物: {result.ModelItem.DisplayName}, 高度范围: [{obstacleInterval.MinZ:F2}, {obstacleInterval.MaxZ:F2}]"); + } } else if (result.IsPassable && result.IntersectionData != null) { // 检查是否为地面/楼板 if (IsFloorLike(result.ModelItem, result.IntersectionData)) { - floors.Add(new HeightInterval( + var floorInterval = new HeightInterval( result.IntersectionData.MinZ, result.IntersectionData.MaxZ - )); + ); + floors.Add(floorInterval); + + if (isDebugPoint) + { + LogManager.Info($"【调试抽样】 - 地面: {result.ModelItem.DisplayName}, 高度范围: [{floorInterval.MinZ:F2}, {floorInterval.MaxZ:F2}]"); + } } } } // 合并重叠的障碍物区间 var mergedObstacles = MergeOverlappingIntervals(obstacles); + + if (isDebugPoint) + { + LogManager.Info($"【调试抽样】 - 合并后障碍物数量: {mergedObstacles.Count}"); + foreach (var obs in mergedObstacles) + { + LogManager.Info($"【调试抽样】 合并障碍物: [{obs.MinZ:F2}, {obs.MaxZ:F2}]"); + } + } // 计算可通行区间 var passableIntervals = new List(); @@ -513,6 +725,10 @@ namespace NavisworksTransport.PathPlanning if (!floors.Any()) { floors.Add(new HeightInterval(baseZ - 0.1, baseZ + 0.1)); + if (isDebugPoint) + { + LogManager.Info($"【调试抽样】 - 未找到地面,使用默认地面: [{baseZ - 0.1:F2}, {baseZ + 0.1:F2}]"); + } } // 对每个潜在的地面,计算其上方的可通行空间 @@ -521,19 +737,39 @@ namespace NavisworksTransport.PathPlanning double floorTop = floor.MaxZ; double availableTop = baseZ + scanHeight; + if (isDebugPoint) + { + LogManager.Info($"【调试抽样】 - 处理地面: 地面顶部={floorTop:F2}, 可用顶部={availableTop:F2}"); + } + // 检查地面上方是否有足够的净空高度 var conflictingObstacles = mergedObstacles .Where(obs => obs.MinZ < availableTop && obs.MaxZ > floorTop) .OrderBy(obs => obs.MinZ) .ToList(); + if (isDebugPoint) + { + LogManager.Info($"【调试抽样】 - 冲突障碍物数量: {conflictingObstacles.Count}"); + } + if (!conflictingObstacles.Any()) { // 没有障碍物,整个高度范围都可通行 double clearHeight = availableTop - floorTop; + if (isDebugPoint) + { + LogManager.Info($"【调试抽样】 - 无障碍物,净空高度: {clearHeight:F2}m, 最小需要: {MIN_PASSABLE_HEIGHT:F2}m"); + } + if (clearHeight >= MIN_PASSABLE_HEIGHT) { - passableIntervals.Add(new HeightInterval(floorTop, availableTop)); + var interval = new HeightInterval(floorTop, availableTop); + passableIntervals.Add(interval); + if (isDebugPoint) + { + LogManager.Info($"【调试抽样】 - 添加可通行区间: [{interval.MinZ:F2}, {interval.MaxZ:F2}]"); + } } } else @@ -541,6 +777,11 @@ namespace NavisworksTransport.PathPlanning // 有障碍物,计算障碍物之间的可通行空间 double currentBottom = floorTop; + if (isDebugPoint) + { + LogManager.Info($"【调试抽样】 - 有障碍物,开始计算空隙,起始底部: {currentBottom:F2}"); + } + foreach (var obstacle in conflictingObstacles) { double obstacleBottom = Math.Max(obstacle.MinZ, currentBottom); @@ -548,27 +789,56 @@ namespace NavisworksTransport.PathPlanning if (obstacleBottom > currentBottom) { double clearHeight = obstacleBottom - currentBottom; + if (isDebugPoint) + { + LogManager.Info($"【调试抽样】 - 障碍物前空隙: 底部={currentBottom:F2}, 顶部={obstacleBottom:F2}, 净空={clearHeight:F2}"); + } + if (clearHeight >= vehicleHeight) { - passableIntervals.Add(new HeightInterval(currentBottom, obstacleBottom)); + var interval = new HeightInterval(currentBottom, obstacleBottom); + passableIntervals.Add(interval); + if (isDebugPoint) + { + LogManager.Info($"【调试抽样】 - 添加障碍物前区间: [{interval.MinZ:F2}, {interval.MaxZ:F2}]"); + } } } currentBottom = Math.Max(currentBottom, obstacle.MaxZ); + if (isDebugPoint) + { + LogManager.Info($"【调试抽样】 - 障碍物后,更新底部为: {currentBottom:F2}"); + } } // 检查最后一个障碍物之后的空间 if (currentBottom < availableTop) { double clearHeight = availableTop - currentBottom; + if (isDebugPoint) + { + LogManager.Info($"【调试抽样】 - 最后障碍物后空隙: 底部={currentBottom:F2}, 顶部={availableTop:F2}, 净空={clearHeight:F2}"); + } + if (clearHeight >= vehicleHeight) { - passableIntervals.Add(new HeightInterval(currentBottom, availableTop)); + var interval = new HeightInterval(currentBottom, availableTop); + passableIntervals.Add(interval); + if (isDebugPoint) + { + LogManager.Info($"【调试抽样】 - 添加最后区间: [{interval.MinZ:F2}, {interval.MaxZ:F2}]"); + } } } } } + if (isDebugPoint) + { + LogManager.Info($"【调试抽样】点 {pointIndex} 计算完成,最终可通行区间数量: {passableIntervals.Count}"); + } + return passableIntervals; } @@ -651,35 +921,338 @@ namespace NavisworksTransport.PathPlanning if (!item.HasGeometry) return null; - var bbox = item.BoundingBox(); - if (bbox == null) - return null; - - // 简化的相交测试:检查垂直线是否穿过边界框 - if (basePoint.X >= bbox.Min.X && basePoint.X <= bbox.Max.X && - basePoint.Y >= bbox.Min.Y && basePoint.Y <= bbox.Max.Y) + // 直接提取模型项的三角形几何数据,不做包围盒预筛选 + var triangles = ExtractTrianglesFromModelItem(item); + if (triangles == null || triangles.Count == 0) { - // 计算相交的Z范围 - double intersectionMinZ = Math.Max(bbox.Min.Z, basePoint.Z); - double intersectionMaxZ = Math.Min(bbox.Max.Z, basePoint.Z + scanHeight); + return null; + } - if (intersectionMaxZ > intersectionMinZ) + // 执行射线-三角形相交检测 + var intersections = PerformVerticalRayIntersection(basePoint, scanHeight, triangles); + if (intersections == null || intersections.Count == 0) + { + return null; + } + + // 计算相交的Z范围 + double minZ = intersections.Min(); + double maxZ = intersections.Max(); + + // 确保相交区间与扫描区间有重叠 + double scanMinZ = basePoint.Z; + double scanMaxZ = basePoint.Z + scanHeight; + + double intersectionMinZ = Math.Max(minZ, scanMinZ); + double intersectionMaxZ = Math.Min(maxZ, scanMaxZ); + + if (intersectionMaxZ > intersectionMinZ) + { + return new IntersectionData { - return new IntersectionData - { - MinZ = intersectionMinZ, - MaxZ = intersectionMaxZ, - IntersectionPoint = new Point3D(basePoint.X, basePoint.Y, (intersectionMinZ + intersectionMaxZ) / 2) - }; + MinZ = intersectionMinZ, + MaxZ = intersectionMaxZ, + IntersectionPoint = new Point3D(basePoint.X, basePoint.Y, (intersectionMinZ + intersectionMaxZ) / 2) + }; + } + + return null; + } + catch (Exception ex) + { + LogManager.Debug($"【垂直扫描处理器】 射线相交测试异常: {item.DisplayName}, {ex.Message}"); + return null; + } + } + + /// + /// 提取模型项的三角形几何数据(复用自ChannelHeightDetector) + /// + /// 模型项 + /// 三角形列表 + private List ExtractTrianglesFromModelItem(ModelItem modelItem) + { + LogManager.Info($"[几何提取] 开始处理: {modelItem.DisplayName}"); + var stopwatch = System.Diagnostics.Stopwatch.StartNew(); + var allTriangles = new List(); + + try + { + // 创建几何提取回调 + var callback = new GeometryExtractorCallback(); + + // 递归处理模型项及其子项 + ExtractTrianglesRecursive(modelItem, callback, allTriangles); + + stopwatch.Stop(); + LogManager.Info($"[几何提取] 完成处理: {modelItem.DisplayName}, 耗时: {stopwatch.ElapsedMilliseconds}ms, 三角形数量: {allTriangles.Count}"); + + return allTriangles; + } + catch (Exception ex) + { + stopwatch.Stop(); + LogManager.Error($"[几何提取] 处理失败: {modelItem.DisplayName}, 耗时: {stopwatch.ElapsedMilliseconds}ms, 错误: {ex.Message}"); + return allTriangles; + } + } + + /// + /// 递归提取三角形(复用自ChannelHeightDetector) + /// + private void ExtractTrianglesRecursive(ModelItem modelItem, GeometryExtractorCallback callback, List allTriangles) + { + try + { + // 如果当前项有几何数据,尝试提取 + if (modelItem.HasGeometry) + { + ExtractTrianglesFromGeometry(modelItem, callback, allTriangles); + } + + // 递归处理子项 + if (modelItem.Children != null) + { + foreach (var child in modelItem.Children) + { + ExtractTrianglesRecursive(child, callback, allTriangles); } } } catch (Exception ex) { - LogManager.Debug($"[垂直扫描处理器] 相交测试异常: {item.DisplayName}, {ex.Message}"); + LogManager.Debug($"【垂直扫描处理器】 递归提取三角形异常: {ex.Message}"); } + } - return null; + /// + /// 从单个有几何数据的ModelItem中提取三角形(复用自ChannelHeightDetector) + /// + private void ExtractTrianglesFromGeometry(ModelItem modelItem, GeometryExtractorCallback callback, List allTriangles) + { + try + { + // 基于示例代码的方法:直接从COM状态获取节点并遍历fragments + var comState = ComBridge.State; + var comObject = ComBridge.ToInwOaPath(modelItem); + + if (comObject == null) + { + return; + } + + // 检查路径是否有效并获取节点 + var nodesColl = comObject.Nodes(); + if (nodesColl.Count > 0) + { + var comNode = nodesColl[nodesColl.Count] as COMApi.InwOaNode; // 获取最后一个节点并转换类型 + if (comNode != null && comNode.IsGeometry) + { + var fragCount = comNode.Fragments().Count; + LogManager.Info($"[COM API] 开始片段处理: {modelItem.DisplayName}, 片段数量: {fragCount}"); + + for (long fragIndex = 1; fragIndex <= fragCount; fragIndex++) + { + var fragStopwatch = System.Diagnostics.Stopwatch.StartNew(); + + var fragsColl = comNode.Fragments(); + var fragment = fragsColl[fragIndex] as COMApi.InwOaFragment3; + + if (fragment != null) + { + // 获取变换矩阵 + var transformMatrix = GetTransformMatrix(fragment); + callback.SetTransformMatrix(transformMatrix); + + // 清空回调中的三角形 + callback.ClearTriangles(); + + // 记录GenerateSimplePrimitives调用前的三角形数量 + int trianglesBeforeGeneration = allTriangles.Count; + + // 生成几何图元 - 使用正确的枚举值 + fragment.GenerateSimplePrimitives(COMApi.nwEVertexProperty.eNORMAL, callback); + + fragStopwatch.Stop(); + int newTriangles = callback.Triangles.Count; + LogManager.Info($"[COM API] 片段 {fragIndex}/{fragCount} 处理完成, 耗时: {fragStopwatch.ElapsedMilliseconds}ms, 新增三角形: {newTriangles}"); + + // 将提取的三角形添加到总列表 + allTriangles.AddRange(callback.Triangles); + } + } + } + } + } + catch (Exception ex) + { + LogManager.Debug($"【垂直扫描处理器】 从 {modelItem.DisplayName} 提取几何时出错: {ex.Message}"); + } + } + + /// + /// 获取Fragment的变换矩阵(复用自ChannelHeightDetector) + /// + private Matrix4 GetTransformMatrix(COMApi.InwOaFragment3 fragment) + { + try + { + var transform = fragment.GetLocalToWorldMatrix(); + if (transform != null) + { + object matrixArrayObj = transform.Matrix; + if (matrixArrayObj is Array matrixArray && matrixArray.Length >= 16) + { + var matrix = new double[16]; + matrixArray.CopyTo(matrix, 0); + + // Navisworks使用列主序矩阵,需要转置 + return new Matrix4( + matrix[0], matrix[4], matrix[8], matrix[12], + matrix[1], matrix[5], matrix[9], matrix[13], + matrix[2], matrix[6], matrix[10], matrix[14], + matrix[3], matrix[7], matrix[11], matrix[15] + ); + } + } + } + catch (Exception ex) + { + LogManager.Debug($"【垂直扫描处理器】 获取变换矩阵失败: {ex.Message}"); + } + + return Matrix4.Identity; + } + + /// + /// 执行垂直射线与三角形相交检测 + /// + /// 射线起点 + /// 扫描高度 + /// 三角形列表 + /// 相交点Z坐标列表 + private List PerformVerticalRayIntersection(Point3D basePoint, double scanHeight, List triangles) + { + var intersectionPoints = new List(); + + try + { + // 创建垂直向上的射线(从basePoint开始向上扫描scanHeight距离) + var rayOrigin = new Point3D(basePoint.X, basePoint.Y, basePoint.Z); + var rayDirection = new Point3D(0, 0, 1); // 向上 + + foreach (var triangle in triangles) + { + if (RayTriangleIntersect(rayOrigin, rayDirection, triangle, out double intersectionZ)) + { + // 检查相交点是否在扫描范围内 + if (intersectionZ >= basePoint.Z && intersectionZ <= basePoint.Z + scanHeight) + { + intersectionPoints.Add(intersectionZ); + } + } + } + + return intersectionPoints; + } + catch (Exception ex) + { + LogManager.Debug($"【垂直扫描处理器】 射线-三角形相交计算失败: {ex.Message}"); + return intersectionPoints; + } + } + + /// + /// 射线-三角形相交算法(Möller-Trumbore算法,复用自ChannelHeightDetector) + /// + /// 射线起点 + /// 射线方向 + /// 三角形 + /// 输出相交点Z坐标 + /// 是否相交 + private bool RayTriangleIntersect(Point3D rayOrigin, Point3D rayDirection, Triangle3D triangle, out double intersectionZ) + { + intersectionZ = 0.0; + const double EPSILON = 0.0000001; + + try + { + // 计算三角形的两条边 + var edge1 = SubtractPoints(triangle.V2, triangle.V1); + var edge2 = SubtractPoints(triangle.V3, triangle.V1); + + // 计算射线方向与edge2的叉积 + var h = CrossProduct(rayDirection, edge2); + var a = DotProduct(edge1, h); + + // 如果a接近0,射线与三角形平行 + if (a > -EPSILON && a < EPSILON) + { + return false; + } + + var f = 1.0 / a; + var s = SubtractPoints(rayOrigin, triangle.V1); + var u = f * DotProduct(s, h); + + if (u < 0.0 || u > 1.0) + { + return false; + } + + var q = CrossProduct(s, edge1); + var v = f * DotProduct(rayDirection, q); + + if (v < 0.0 || u + v > 1.0) + { + return false; + } + + // 计算t值(射线参数) + var t = f * DotProduct(edge2, q); + + if (t > EPSILON) // 射线相交 + { + // 计算交点 + intersectionZ = rayOrigin.Z + t * rayDirection.Z; + return true; + } + + return false; // 线段相交但射线不相交 + } + catch (Exception ex) + { + LogManager.Debug($"【垂直扫描处理器】 射线相交计算出错: {ex.Message}"); + return false; + } + } + + /// + /// 向量减法 + /// + private Point3D SubtractPoints(Point3D p1, Point3D p2) + { + return new Point3D(p1.X - p2.X, p1.Y - p2.Y, p1.Z - p2.Z); + } + + /// + /// 向量叉积 + /// + private Point3D CrossProduct(Point3D v1, Point3D v2) + { + return new Point3D( + v1.Y * v2.Z - v1.Z * v2.Y, + v1.Z * v2.X - v1.X * v2.Z, + v1.X * v2.Y - v1.Y * v2.X + ); + } + + /// + /// 向量点积 + /// + private double DotProduct(Point3D v1, Point3D v2) + { + return v1.X * v2.X + v1.Y * v2.Y + v1.Z * v2.Z; } /// @@ -803,6 +1376,108 @@ namespace NavisworksTransport.PathPlanning public bool IsPassable { get; set; } } + /// + /// 三角形数据结构(复用自ChannelHeightDetector) + /// + public class Triangle3D + { + public Point3D V1 { get; set; } + public Point3D V2 { get; set; } + public Point3D V3 { get; set; } + + public Triangle3D(Point3D v1, Point3D v2, Point3D v3) + { + V1 = v1; + V2 = v2; + V3 = v3; + } + } + + /// + /// 几何提取回调类(复用自ChannelHeightDetector) + /// + public class GeometryExtractorCallback : COMApi.InwSimplePrimitivesCB + { + private readonly List _triangles; + private Matrix4 _transformMatrix; + + public List Triangles => _triangles; + + public GeometryExtractorCallback() + { + _triangles = new List(); + _transformMatrix = Matrix4.Identity; + } + + public void SetTransformMatrix(Matrix4 matrix) + { + _transformMatrix = matrix; + } + + public void Line(COMApi.InwSimpleVertex v1, COMApi.InwSimpleVertex v2) + { + // 我们只关心三角形,忽略线段 + } + + public void Point(COMApi.InwSimpleVertex v1) + { + // 我们只关心三角形,忽略点 + } + + public void SnapPoint(COMApi.InwSimpleVertex v1) + { + // 我们只关心三角形,忽略捕捉点 + } + + public void Triangle(COMApi.InwSimpleVertex v1, COMApi.InwSimpleVertex v2, COMApi.InwSimpleVertex v3) + { + try + { + // 提取顶点坐标 + var vertex1 = ConvertVertex(v1); + var vertex2 = ConvertVertex(v2); + var vertex3 = ConvertVertex(v3); + + // 应用变换矩阵到顶点坐标 + vertex1 = _transformMatrix.Transform(vertex1); + vertex2 = _transformMatrix.Transform(vertex2); + vertex3 = _transformMatrix.Transform(vertex3); + + // 创建三角形并添加到列表 + var triangle = new Triangle3D(vertex1, vertex2, vertex3); + _triangles.Add(triangle); + } + catch (Exception ex) + { + LogManager.Debug($"【垂直扫描处理器】 处理三角形时出错: {ex.Message}"); + } + } + + private Point3D ConvertVertex(COMApi.InwSimpleVertex vertex) + { + // 从COM API顶点获取坐标 + object coordObj = vertex.coord; + if (coordObj is Array coordinates && coordinates.Length >= 3) + { + var coords = new double[3]; + coordinates.CopyTo(coords, 0); + return new Point3D(coords[0], coords[1], coords[2]); + } + + return new Point3D(0, 0, 0); + } + + public void ClearTriangles() + { + _triangles.Clear(); + } + + public List GetTriangles() + { + return new List(_triangles); + } + } + #endregion } } \ No newline at end of file