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