1055 lines
43 KiB
C#
1055 lines
43 KiB
C#
using System;
|
||
using System.Collections.Generic;
|
||
using System.Linq;
|
||
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;
|
||
|
||
namespace NavisworksTransport.PathPlanning
|
||
{
|
||
/// <summary>
|
||
/// 垂直扫描处理器
|
||
/// 实现多级筛选优化的障碍物检测和2.5D高度区间计算
|
||
/// 预期实现10,000-40,000倍性能提升
|
||
/// </summary>
|
||
public class VerticalScanProcessor
|
||
{
|
||
#region 私有字段
|
||
|
||
/// <summary>
|
||
/// 空间哈希表，用于快速查找邻域内的模型项
|
||
/// </summary>
|
||
private readonly Dictionary<string, List<ModelItem>> _spatialHashMap;
|
||
|
||
/// <summary>
|
||
/// 空间哈希的网格大小（米）
|
||
/// </summary>
|
||
private readonly double _spatialHashSize;
|
||
|
||
/// <summary>
|
||
/// 并行处理的任务数量
|
||
/// </summary>
|
||
private readonly int _parallelDegree;
|
||
|
||
/// <summary>
|
||
/// 候选项缓存，基于空间哈希桶坐标（线程安全）
|
||
/// </summary>
|
||
private readonly ConcurrentDictionary<string, HashSet<ModelItem>> _candidateCache;
|
||
|
||
/// <summary>
|
||
/// 缓存大小限制
|
||
/// </summary>
|
||
private int _maxCacheSize;
|
||
|
||
/// <summary>
|
||
/// 缓存命中统计
|
||
/// </summary>
|
||
private int _cacheHits;
|
||
|
||
/// <summary>
|
||
/// 缓存未命中统计
|
||
/// </summary>
|
||
private int _cacheMisses;
|
||
|
||
/// <summary>
|
||
/// 高度筛选的容差值（米）
|
||
/// </summary>
|
||
private const double HEIGHT_TOLERANCE = 0.1;
|
||
|
||
/// <summary>
|
||
/// 默认人行高度（米）- 用于未检测到通道时的默认高度
|
||
/// </summary>
|
||
private const double DEFAULT_WALKING_HEIGHT = 2.5;
|
||
|
||
/// <summary>
|
||
/// 最小通行高度（米）
|
||
/// </summary>
|
||
private const double MIN_PASSABLE_HEIGHT = 1.8;
|
||
|
||
#endregion
|
||
|
||
#region 构造函数
|
||
|
||
/// <summary>
|
||
/// 构造函数
|
||
/// </summary>
|
||
/// <param name="spatialHashSize">空间哈希网格大小（米）或实际网格大小。</param>
|
||
/// <param name="parallelDegree">并行度，默认为CPU核心数的一半，避免过度并行导致崩溃</param>
|
||
public VerticalScanProcessor(double spatialHashSize, int parallelDegree = 0)
|
||
{
|
||
_spatialHashSize = spatialHashSize;
|
||
|
||
// 限制并行度，避免过度并行导致崩溃，最大为CPU核心数的一半
|
||
int maxParallelism = Math.Max(1, Environment.ProcessorCount / 2);
|
||
_parallelDegree = parallelDegree > 0 ? Math.Min(parallelDegree, maxParallelism) : maxParallelism;
|
||
_spatialHashMap = new Dictionary<string, List<ModelItem>>();
|
||
|
||
// 初始化线程安全的缓存
|
||
_candidateCache = new ConcurrentDictionary<string, HashSet<ModelItem>>();
|
||
_maxCacheSize = 1000; // 默认值，将在BuildSpatialHashIndex中动态调整
|
||
_cacheHits = 0;
|
||
_cacheMisses = 0;
|
||
|
||
// 确定查询策略
|
||
string queryStrategy;
|
||
int bucketCount;
|
||
// 只用单桶模式测试
|
||
queryStrategy = "单桶模式";
|
||
bucketCount = 1;
|
||
// if (_spatialHashSize < 1.5)
|
||
// {
|
||
// queryStrategy = "单桶模式";
|
||
// bucketCount = 1;
|
||
// }
|
||
// else if (_spatialHashSize < 3.0)
|
||
// {
|
||
// queryStrategy = "十字形模式";
|
||
// bucketCount = 5;
|
||
// }
|
||
// else
|
||
// {
|
||
// queryStrategy = "3x3模式";
|
||
// bucketCount = 9;
|
||
// }
|
||
|
||
LogManager.Info($"[垂直扫描处理器] 初始化完成，空间哈希大小: {_spatialHashSize}m, 查询策略: {queryStrategy}({bucketCount}桶), 并行度: {_parallelDegree}");
|
||
}
|
||
|
||
#endregion
|
||
|
||
#region 公共方法
|
||
|
||
/// <summary>
|
||
/// 构建空间哈希索引
|
||
/// 第一级筛选：邻域筛选
|
||
/// </summary>
|
||
/// <param name="modelItems">所有模型项</param>
|
||
/// <param name="bounds">扫描边界</param>
|
||
/// <param name="channelItems">已确定的通道元素列表（将被排除）</param>
|
||
public void BuildSpatialHashIndex(IEnumerable<ModelItem> modelItems, BoundingBox3D bounds, IEnumerable<ModelItem> channelItems = null)
|
||
{
|
||
LogManager.Info("【垂直扫描处理器】 开始构建空间哈希索引");
|
||
var startTime = DateTime.Now;
|
||
|
||
_spatialHashMap.Clear();
|
||
|
||
// 创建通道元素的HashSet以便快速查找
|
||
var channelItemsSet = new HashSet<ModelItem>(channelItems ?? new List<ModelItem>());
|
||
|
||
// 统计变量
|
||
var totalItems = modelItems?.Count() ?? 0;
|
||
|
||
LogManager.Info($"【垂直扫描处理器】 输入统计 - 总模型项: {totalItems}, 将排除通道元素: {channelItemsSet.Count}");
|
||
LogManager.Info($"【垂直扫描处理器】 扫描边界: [{bounds.Min.X:F1},{bounds.Min.Y:F1},{bounds.Min.Z:F1}] - [{bounds.Max.X:F1},{bounds.Max.Y:F1},{bounds.Max.Z:F1}]");
|
||
|
||
var itemsWithBounds = new ConcurrentBag<(ModelItem item, BoundingBox3D bbox)>();
|
||
|
||
// 统计计数器（线程安全）
|
||
var geometryCounter = 0;
|
||
var channelExcludedCounter = 0;
|
||
var noBoundsCounter = 0;
|
||
var outOfBoundsCounter = 0;
|
||
|
||
try
|
||
{
|
||
// 并行计算所有模型项的边界框，使用ConcurrentBag避免锁竞争
|
||
Parallel.ForEach(modelItems, new ParallelOptions { MaxDegreeOfParallelism = _parallelDegree }, item =>
|
||
{
|
||
try
|
||
{
|
||
// 🔥 关键修复：检查是否需要排除通道元素（包括容器节点和子节点）
|
||
if (channelItemsSet.Contains(item) || IsChildOfChannelItems(item, channelItemsSet))
|
||
{
|
||
Interlocked.Increment(ref channelExcludedCounter);
|
||
return; // 跳过通道元素及其子节点
|
||
}
|
||
|
||
// 然后检查是否有几何体，只对有几何体的元素进行空间哈希处理
|
||
if (item?.HasGeometry == true)
|
||
{
|
||
Interlocked.Increment(ref geometryCounter);
|
||
|
||
var bbox = item.BoundingBox();
|
||
if (bbox == null)
|
||
{
|
||
Interlocked.Increment(ref noBoundsCounter);
|
||
return;
|
||
}
|
||
|
||
if (IsWithinBounds(bbox, bounds))
|
||
{
|
||
itemsWithBounds.Add((item, bbox));
|
||
}
|
||
else
|
||
{
|
||
Interlocked.Increment(ref outOfBoundsCounter);
|
||
}
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Debug($"【垂直扫描处理器】 获取边界框失败: {item?.DisplayName ?? "NULL"}, {ex.Message}");
|
||
}
|
||
});
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"【垂直扫描处理器】 并行计算边界框时发生严重错误: {ex.Message}");
|
||
return; // 提前退出，避免进一步崩溃
|
||
}
|
||
|
||
// 输出详细统计信息
|
||
LogManager.Info($"【垂直扫描处理器】 元素筛选统计:");
|
||
LogManager.Info($" - 有几何体的元素: {geometryCounter}");
|
||
LogManager.Info($" - 被排除的通道元素: {channelExcludedCounter}");
|
||
LogManager.Info($" - 无边界框的元素: {noBoundsCounter}");
|
||
LogManager.Info($" - 超出扫描边界的元素: {outOfBoundsCounter}");
|
||
LogManager.Info($" - 符合条件并添加到空间哈希的元素: {itemsWithBounds.Count}");
|
||
|
||
// 记录待索引元素数量
|
||
LogManager.Info($"【垂直扫描处理器】 待索引元素数量: {itemsWithBounds.Count()}");
|
||
|
||
// 构建空间哈希
|
||
var totalHashEntries = 0;
|
||
var hashKeyDistribution = new Dictionary<string, int>();
|
||
|
||
foreach (var (item, bbox) in itemsWithBounds)
|
||
{
|
||
var hashKeys = GetSpatialHashKeys(bbox);
|
||
foreach (var key in hashKeys)
|
||
{
|
||
if (!_spatialHashMap.ContainsKey(key))
|
||
{
|
||
_spatialHashMap[key] = new List<ModelItem>();
|
||
}
|
||
_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($"【垂直扫描处理器】 空间哈希构建完成 - 哈希桶数量: {_spatialHashMap.Count}, 平均桶大小: {bucketSizes.DefaultIfEmpty(0).Average():F1}");
|
||
|
||
// 空间哈希索引构建完成
|
||
|
||
// 🔥 关键新增：动态计算缓存容量
|
||
double rangeX = bounds.Max.X - bounds.Min.X;
|
||
double rangeY = bounds.Max.Y - bounds.Min.Y;
|
||
int bucketsX = (int)(rangeX / _spatialHashSize + 1);
|
||
int bucketsY = (int)(rangeY / _spatialHashSize + 1);
|
||
int estimatedBuckets = bucketsX * bucketsY;
|
||
|
||
if (estimatedBuckets > 50000) // 大量哈希桶，需要更大缓存
|
||
{
|
||
_maxCacheSize = Math.Min(200000, estimatedBuckets); // 直接匹配桶数量
|
||
}
|
||
else if (estimatedBuckets > 20000)
|
||
{
|
||
_maxCacheSize = Math.Min(100000, estimatedBuckets);
|
||
}
|
||
else
|
||
{
|
||
_maxCacheSize = Math.Min(20000, estimatedBuckets * 2);
|
||
}
|
||
LogManager.Info($"【垂直扫描处理器】 空间哈希大小: {_spatialHashSize}m");
|
||
LogManager.Info($"【垂直扫描处理器】 范围: X={rangeX:F1}m, Y={rangeY:F1}m");
|
||
LogManager.Info($"【垂直扫描处理器】 桶数: X={bucketsX}, Y={bucketsY}, 总计={estimatedBuckets}");
|
||
LogManager.Info($"【垂直扫描处理器】 动态设置缓存容量: {_maxCacheSize} (预计哈希桶: {estimatedBuckets}, 实际哈希桶: {_spatialHashMap.Count})");
|
||
|
||
var elapsed = (DateTime.Now - startTime).TotalMilliseconds;
|
||
LogManager.Info($"【垂直扫描处理器】 空间哈希索引构建完成，耗时: {elapsed:F1}ms");
|
||
LogManager.Info($"【垂直扫描处理器】 最终统计 - 参与元素: {itemsWithBounds.Count}, 哈希桶: {_spatialHashMap.Count}, 总哈希条目: {totalHashEntries}");
|
||
}
|
||
|
||
|
||
/// <summary>
|
||
/// 并行扫描网格点的2.5D高度区间
|
||
/// </summary>
|
||
/// <param name="gridPoints">网格点列表</param>
|
||
/// <param name="scanHeight">扫描高度（从地面向上的距离）</param>
|
||
/// <param name="vehicleHeight">车辆高度（用于通行检查）</param>
|
||
/// <returns>每个网格点的高度区间列表</returns>
|
||
public Dictionary<Point3D, List<HeightInterval>> ParallelScanHeightIntervals(
|
||
IEnumerable<Point3D> gridPoints,
|
||
double scanHeight = 20.0,
|
||
double vehicleHeight = 3.0)
|
||
{
|
||
LogManager.Info($"[垂直扫描处理器] 开始并行扫描高度区间，扫描高度: {scanHeight}m, 车辆高度: {vehicleHeight}m");
|
||
var startTime = DateTime.Now;
|
||
|
||
var results = new ConcurrentDictionary<Point3D, List<HeightInterval>>();
|
||
var pointsList = gridPoints?.ToList() ?? new List<Point3D>();
|
||
|
||
LogManager.Info($"[垂直扫描处理器] 准备处理{pointsList.Count}个点");
|
||
|
||
try
|
||
{
|
||
// 并行处理每个网格点，添加更严格的异常处理
|
||
Parallel.ForEach(pointsList, new ParallelOptions
|
||
{
|
||
MaxDegreeOfParallelism = _parallelDegree,
|
||
CancellationToken = System.Threading.CancellationToken.None
|
||
}, gridPoint =>
|
||
{
|
||
try
|
||
{
|
||
if (gridPoint != null)
|
||
{
|
||
var intervals = ScanVerticalLine(gridPoint, scanHeight, vehicleHeight);
|
||
results[gridPoint] = intervals ?? new List<HeightInterval>();
|
||
}
|
||
}
|
||
catch (OutOfMemoryException)
|
||
{
|
||
LogManager.Error($"[垂直扫描处理器] 内存不足，跳过点 {gridPoint}");
|
||
if (gridPoint != null)
|
||
{
|
||
results[gridPoint] = new List<HeightInterval>();
|
||
}
|
||
throw; // 内存不足需要重新抛出
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"[垂直扫描处理器] 扫描点 {gridPoint} 失败: {ex.Message}");
|
||
if (gridPoint != null)
|
||
{
|
||
results[gridPoint] = new List<HeightInterval>();
|
||
}
|
||
}
|
||
});
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"[垂直扫描处理器] 并行扫描过程中发生严重错误: {ex.Message}");
|
||
// 如果并行处理失败，尝试串行处理作为备选方案
|
||
LogManager.Info("[垂直扫描处理器] 尝试串行处理作为备选方案");
|
||
|
||
foreach (var gridPoint in pointsList)
|
||
{
|
||
try
|
||
{
|
||
if (gridPoint != null && !results.ContainsKey(gridPoint))
|
||
{
|
||
var intervals = ScanVerticalLine(gridPoint, scanHeight, vehicleHeight);
|
||
results[gridPoint] = intervals ?? new List<HeightInterval>();
|
||
}
|
||
}
|
||
catch (Exception serialEx)
|
||
{
|
||
LogManager.Error($"[垂直扫描处理器] 串行扫描点 {gridPoint} 也失败: {serialEx.Message}");
|
||
if (gridPoint != null)
|
||
{
|
||
results[gridPoint] = new List<HeightInterval>();
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
var elapsed = (DateTime.Now - startTime).TotalMilliseconds;
|
||
var totalIntervals = results.Values.Sum(list => list.Count);
|
||
|
||
// 输出缓存统计信息
|
||
if (_cacheHits + _cacheMisses > 0)
|
||
{
|
||
double hitRate = (double)_cacheHits / (_cacheHits + _cacheMisses) * 100;
|
||
LogManager.Info($"[垂直扫描处理器] 缓存统计 - 命中: {_cacheHits}, 未命中: {_cacheMisses}, 命中率: {hitRate:F1}%, 缓存大小: {_candidateCache.Count}");
|
||
}
|
||
|
||
LogManager.Info($"[垂直扫描处理器] 并行扫描完成，耗时: {elapsed:F1}ms, 扫描点: {pointsList.Count}, 总区间: {totalIntervals}");
|
||
|
||
// 清理缓存，为下次扫描做准备
|
||
_candidateCache.Clear();
|
||
_cacheHits = 0;
|
||
_cacheMisses = 0;
|
||
|
||
return new Dictionary<Point3D, List<HeightInterval>>(results);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 扫描单个垂直线上的通行区间
|
||
/// </summary>
|
||
/// <param name="basePoint">基础点（X,Y坐标）</param>
|
||
/// <param name="scanHeight">扫描高度</param>
|
||
/// <param name="vehicleHeight">车辆高度</param>
|
||
/// <returns>可通行的高度区间列表</returns>
|
||
public List<HeightInterval> ScanVerticalLine(Point3D basePoint, double scanHeight, double vehicleHeight)
|
||
{
|
||
// 第一级筛选：邻域筛选 - 获取空间哈希邻域内的候选项
|
||
var candidateItems = GetCandidateItemsFromSpatialHash(basePoint);
|
||
|
||
// 第二级筛选：高度筛选 - 只保留在扫描高度范围内的项目
|
||
var heightFilteredItems = HeightFiltering(candidateItems, basePoint.Z, basePoint.Z + scanHeight);
|
||
|
||
// 第三级筛选：空间哈希 - 精确的几何相交测试
|
||
var intersectionResults = PerformIntersectionTests(heightFilteredItems, basePoint, scanHeight);
|
||
|
||
// 计算可通行区间
|
||
var passableIntervals = CalculatePassableIntervals(intersectionResults, basePoint.Z, scanHeight, vehicleHeight);
|
||
|
||
return passableIntervals;
|
||
}
|
||
|
||
|
||
#endregion
|
||
|
||
#region 私有方法 - 多级筛选实现
|
||
|
||
/// <summary>
|
||
/// 第一级筛选：从空间哈希中获取候选模型项（邻域筛选）
|
||
/// </summary>
|
||
/// <param name="point">查询点</param>
|
||
/// <returns>候选模型项列表</returns>
|
||
private IEnumerable<ModelItem> GetCandidateItemsFromSpatialHash(Point3D point)
|
||
{
|
||
try
|
||
{
|
||
// 🔥 关键修复：检查坐标值的有效性
|
||
if (double.IsNaN(point.X) || double.IsNaN(point.Y) ||
|
||
double.IsInfinity(point.X) || double.IsInfinity(point.Y))
|
||
{
|
||
LogManager.Warning($"[垂直扫描处理器] 无效的坐标点: ({point.X}, {point.Y}, {point.Z})");
|
||
return new HashSet<ModelItem>(); // 返回空集合
|
||
}
|
||
|
||
// 使用粗粒度的缓存键（基于空间哈希桶坐标）
|
||
int gridX = (int)Math.Floor(point.X / _spatialHashSize);
|
||
int gridY = (int)Math.Floor(point.Y / _spatialHashSize);
|
||
string cacheKey = $"{gridX},{gridY}";
|
||
|
||
// 🔥 关键修复：使用ConcurrentDictionary的GetOrAdd原子操作
|
||
var candidates = _candidateCache.GetOrAdd(cacheKey, key =>
|
||
{
|
||
// 这个函数只在缓存未命中时执行
|
||
Interlocked.Increment(ref _cacheMisses);
|
||
|
||
// 构建候选集合
|
||
var items = new HashSet<ModelItem>();
|
||
var hashKeys = GetSpatialHashKeysForPoint(point);
|
||
|
||
foreach (var hkey in hashKeys)
|
||
{
|
||
if (_spatialHashMap.TryGetValue(hkey, out var bucketItems))
|
||
{
|
||
items.UnionWith(bucketItems);
|
||
}
|
||
}
|
||
|
||
// 检查缓存大小，如果超限则清空整个缓存
|
||
if (_candidateCache.Count > _maxCacheSize)
|
||
{
|
||
_candidateCache.Clear();
|
||
LogManager.Info($"[垂直扫描处理器] 缓存已满({_candidateCache.Count}>{_maxCacheSize})，清空缓存重新开始");
|
||
}
|
||
|
||
return items;
|
||
});
|
||
|
||
// 如果键已存在（GetOrAdd没有执行工厂函数），说明是缓存命中
|
||
if (_candidateCache.ContainsKey(cacheKey))
|
||
{
|
||
Interlocked.Increment(ref _cacheHits);
|
||
}
|
||
|
||
// 🔥 关键修复：返回副本以保证线程安全，避免并发修改原HashSet
|
||
return new HashSet<ModelItem>(candidates);
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"[垂直扫描处理器] 获取候选项失败: {ex.Message}，点坐标: ({point.X}, {point.Y}, {point.Z})");
|
||
return new HashSet<ModelItem>(); // 返回空集合
|
||
}
|
||
}
|
||
|
||
|
||
|
||
/// <summary>
|
||
/// 第二级筛选：高度筛选
|
||
/// </summary>
|
||
/// <param name="items">候选模型项</param>
|
||
/// <param name="minZ">最小Z坐标</param>
|
||
/// <param name="maxZ">最大Z坐标</param>
|
||
/// <returns>高度筛选后的模型项</returns>
|
||
private List<ModelItem> HeightFiltering(IEnumerable<ModelItem> items, double minZ, double maxZ)
|
||
{
|
||
var filtered = new ConcurrentBag<ModelItem>();
|
||
|
||
try
|
||
{
|
||
// 使用ConcurrentBag避免锁竞争，提高性能和稳定性
|
||
Parallel.ForEach(items, new ParallelOptions { MaxDegreeOfParallelism = _parallelDegree }, item =>
|
||
{
|
||
try
|
||
{
|
||
if (item?.HasGeometry == true)
|
||
{
|
||
var bbox = item.BoundingBox();
|
||
if (bbox != null)
|
||
{
|
||
// 检查高度范围是否有重叠（加上容差）
|
||
if (bbox.Max.Z >= (minZ - HEIGHT_TOLERANCE) &&
|
||
bbox.Min.Z <= (maxZ + HEIGHT_TOLERANCE))
|
||
{
|
||
filtered.Add(item);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Debug($"[垂直扫描处理器] 高度筛选失败: {item?.DisplayName ?? "NULL"}, {ex.Message}");
|
||
}
|
||
});
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Warning($"[垂直扫描处理器] 高度筛选并行处理失败: {ex.Message}，回退到串行处理");
|
||
|
||
// 如果并行处理失败，使用串行处理
|
||
foreach (var item in items ?? new List<ModelItem>())
|
||
{
|
||
try
|
||
{
|
||
if (item?.HasGeometry == true)
|
||
{
|
||
var bbox = item.BoundingBox();
|
||
if (bbox != null &&
|
||
bbox.Max.Z >= (minZ - HEIGHT_TOLERANCE) &&
|
||
bbox.Min.Z <= (maxZ + HEIGHT_TOLERANCE))
|
||
{
|
||
filtered.Add(item);
|
||
}
|
||
}
|
||
}
|
||
catch (Exception serialEx)
|
||
{
|
||
LogManager.Debug($"[垂直扫描处理器] 串行高度筛选失败: {item?.DisplayName ?? "NULL"}, {serialEx.Message}");
|
||
}
|
||
}
|
||
}
|
||
|
||
return filtered.ToList();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 第三级筛选：精确几何相交测试（空间哈希优化）
|
||
/// </summary>
|
||
/// <param name="items">高度筛选后的模型项</param>
|
||
/// <param name="basePoint">基础点</param>
|
||
/// <param name="scanHeight">扫描高度</param>
|
||
/// <returns>相交测试结果</returns>
|
||
private List<IntersectionResult> PerformIntersectionTests(List<ModelItem> items, Point3D basePoint, double scanHeight)
|
||
{
|
||
var results = new ConcurrentBag<IntersectionResult>();
|
||
|
||
try
|
||
{
|
||
// 并行执行相交测试，添加更强的异常处理
|
||
Parallel.ForEach(items, new ParallelOptions { MaxDegreeOfParallelism = _parallelDegree }, item =>
|
||
{
|
||
try
|
||
{
|
||
if (item != null)
|
||
{
|
||
var intersectionData = TestVerticalLineIntersection(item, basePoint, scanHeight);
|
||
if (intersectionData != null)
|
||
{
|
||
// 简化逻辑：所有与垂直射线相交的非通道元素都视为障碍物
|
||
// 因为在空间哈希构建时已经排除了通道元素
|
||
results.Add(new IntersectionResult
|
||
{
|
||
ModelItem = item,
|
||
IntersectionData = intersectionData,
|
||
IsObstacle = true, // 所有相交的非通道元素都是障碍物
|
||
IsPassable = false // 不可通行
|
||
});
|
||
}
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Debug($"【垂直扫描处理器】 相交测试失败: {item?.DisplayName ?? "NULL"}, {ex.Message}");
|
||
}
|
||
});
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Warning($"【垂直扫描处理器】 并行相交测试失败: {ex.Message}，回退到串行处理");
|
||
|
||
// 如果并行处理失败，使用串行处理
|
||
foreach (var item in items ?? new List<ModelItem>())
|
||
{
|
||
try
|
||
{
|
||
if (item != null)
|
||
{
|
||
var intersectionData = TestVerticalLineIntersection(item, basePoint, scanHeight);
|
||
if (intersectionData != null)
|
||
{
|
||
// 简化逻辑：所有与垂直射线相交的非通道元素都视为障碍物
|
||
results.Add(new IntersectionResult
|
||
{
|
||
ModelItem = item,
|
||
IntersectionData = intersectionData,
|
||
IsObstacle = true, // 所有相交的非通道元素都是障碍物
|
||
IsPassable = false // 不可通行
|
||
});
|
||
}
|
||
}
|
||
}
|
||
catch (Exception serialEx)
|
||
{
|
||
LogManager.Debug($"【垂直扫描处理器】 串行相交测试失败: {item?.DisplayName ?? "NULL"}, {serialEx.Message}");
|
||
}
|
||
}
|
||
}
|
||
|
||
return results.ToList();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算可通行区间
|
||
/// </summary>
|
||
/// <param name="intersectionResults">相交测试结果</param>
|
||
/// <param name="baseZ">基础Z坐标</param>
|
||
/// <param name="scanHeight">扫描高度</param>
|
||
/// <param name="vehicleHeight">车辆高度</param>
|
||
/// <returns>可通行区间列表</returns>
|
||
private List<HeightInterval> CalculatePassableIntervals(
|
||
List<IntersectionResult> intersectionResults,
|
||
double baseZ,
|
||
double scanHeight,
|
||
double vehicleHeight)
|
||
{
|
||
|
||
// 收集所有障碍物的高度范围
|
||
var obstacles = new List<HeightInterval>();
|
||
var floors = new List<HeightInterval>();
|
||
|
||
foreach (var result in intersectionResults)
|
||
{
|
||
if (result.IsObstacle && result.IntersectionData != null)
|
||
{
|
||
var obstacleInterval = new HeightInterval(
|
||
result.IntersectionData.MinZ,
|
||
result.IntersectionData.MaxZ
|
||
);
|
||
obstacles.Add(obstacleInterval);
|
||
}
|
||
else if (result.IsPassable && result.IntersectionData != null)
|
||
{
|
||
// 检查是否为地面/楼板
|
||
if (IsFloorLike(result.ModelItem, result.IntersectionData))
|
||
{
|
||
var floorInterval = new HeightInterval(
|
||
result.IntersectionData.MinZ,
|
||
result.IntersectionData.MaxZ
|
||
);
|
||
floors.Add(floorInterval);
|
||
}
|
||
}
|
||
}
|
||
|
||
// 合并重叠的障碍物区间
|
||
var mergedObstacles = MergeOverlappingIntervals(obstacles);
|
||
|
||
// 计算可通行区间
|
||
var passableIntervals = new List<HeightInterval>();
|
||
var scanRange = new HeightInterval(baseZ, baseZ + scanHeight);
|
||
|
||
// 如果没有找到地面，使用基础Z坐标作为默认地面
|
||
if (!floors.Any())
|
||
{
|
||
floors.Add(new HeightInterval(baseZ - 0.1, baseZ + 0.1));
|
||
}
|
||
|
||
// 对每个潜在的地面，计算其上方的可通行空间
|
||
foreach (var floor in floors)
|
||
{
|
||
double floorTop = floor.MaxZ;
|
||
double availableTop = baseZ + scanHeight;
|
||
|
||
// 检查地面上方是否有足够的净空高度
|
||
var conflictingObstacles = mergedObstacles
|
||
.Where(obs => obs.MinZ < availableTop && obs.MaxZ > floorTop)
|
||
.OrderBy(obs => obs.MinZ)
|
||
.ToList();
|
||
|
||
if (!conflictingObstacles.Any())
|
||
{
|
||
// 没有障碍物，整个高度范围都可通行
|
||
double clearHeight = availableTop - floorTop;
|
||
|
||
if (clearHeight >= MIN_PASSABLE_HEIGHT)
|
||
{
|
||
var interval = new HeightInterval(floorTop, availableTop);
|
||
passableIntervals.Add(interval);
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// 有障碍物，计算障碍物之间的可通行空间
|
||
double currentBottom = floorTop;
|
||
|
||
foreach (var obstacle in conflictingObstacles)
|
||
{
|
||
double obstacleBottom = Math.Max(obstacle.MinZ, currentBottom);
|
||
|
||
if (obstacleBottom > currentBottom)
|
||
{
|
||
double clearHeight = obstacleBottom - currentBottom;
|
||
|
||
if (clearHeight >= vehicleHeight)
|
||
{
|
||
var interval = new HeightInterval(currentBottom, obstacleBottom);
|
||
passableIntervals.Add(interval);
|
||
}
|
||
}
|
||
|
||
currentBottom = Math.Max(currentBottom, obstacle.MaxZ);
|
||
}
|
||
|
||
// 检查最后一个障碍物之后的空间
|
||
if (currentBottom < availableTop)
|
||
{
|
||
double clearHeight = availableTop - currentBottom;
|
||
|
||
if (clearHeight >= vehicleHeight)
|
||
{
|
||
var interval = new HeightInterval(currentBottom, availableTop);
|
||
passableIntervals.Add(interval);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
return passableIntervals;
|
||
}
|
||
|
||
#endregion
|
||
|
||
#region 私有辅助方法
|
||
|
||
/// <summary>
|
||
/// 检查边界框是否在指定范围内
|
||
/// </summary>
|
||
/// <param name="bbox">要检查的边界框</param>
|
||
/// <param name="bounds">范围边界框</param>
|
||
/// <returns>是否在范围内</returns>
|
||
private bool IsWithinBounds(BoundingBox3D bbox, BoundingBox3D bounds)
|
||
{
|
||
return bbox.Max.X >= bounds.Min.X && bbox.Min.X <= bounds.Max.X &&
|
||
bbox.Max.Y >= bounds.Min.Y && bbox.Min.Y <= bounds.Max.Y &&
|
||
bbox.Max.Z >= bounds.Min.Z && bbox.Min.Z <= bounds.Max.Z;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取边界框的所有空间哈希键
|
||
/// </summary>
|
||
/// <param name="bbox">边界框</param>
|
||
/// <returns>空间哈希键列表</returns>
|
||
private List<string> GetSpatialHashKeys(BoundingBox3D bbox)
|
||
{
|
||
var keys = new List<string>();
|
||
|
||
int minX = (int)Math.Floor(bbox.Min.X / _spatialHashSize);
|
||
int maxX = (int)Math.Floor(bbox.Max.X / _spatialHashSize);
|
||
int minY = (int)Math.Floor(bbox.Min.Y / _spatialHashSize);
|
||
int maxY = (int)Math.Floor(bbox.Max.Y / _spatialHashSize);
|
||
|
||
for (int x = minX; x <= maxX; x++)
|
||
{
|
||
for (int y = minY; y <= maxY; y++)
|
||
{
|
||
keys.Add($"{x},{y}");
|
||
}
|
||
}
|
||
|
||
return keys;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取点的空间哈希键（包括相邻网格）
|
||
/// </summary>
|
||
/// <param name="point">查询点</param>
|
||
/// <returns>空间哈希键列表</returns>
|
||
private List<string> GetSpatialHashKeysForPoint(Point3D point)
|
||
{
|
||
var keys = new List<string>();
|
||
int centerX = (int)Math.Floor(point.X / _spatialHashSize);
|
||
int centerY = (int)Math.Floor(point.Y / _spatialHashSize);
|
||
|
||
// 单桶模式
|
||
keys.Add($"{centerX},{centerY}");
|
||
|
||
// 智能查询范围策略：根据空间哈希大小调整查询桶数量
|
||
// - 哈希桶<1.5m：只查询中心桶（1个）
|
||
// - 哈希桶1.5-3m：查询十字形（5个桶）
|
||
// - 哈希桶>3m：查询3x3（9个桶）
|
||
|
||
// if (_spatialHashSize < 1.5)
|
||
// {
|
||
// // 单桶模式 - 对于很小的哈希桶，相邻桶基本不会有相关模型
|
||
// keys.Add($"{centerX},{centerY}");
|
||
// }
|
||
// else if (_spatialHashSize < 3.0)
|
||
// {
|
||
// // 5个桶模式（十字形）- 只查询上下左右和中心
|
||
// keys.Add($"{centerX},{centerY}");
|
||
// keys.Add($"{centerX-1},{centerY}");
|
||
// keys.Add($"{centerX+1},{centerY}");
|
||
// keys.Add($"{centerX},{centerY-1}");
|
||
// keys.Add($"{centerX},{centerY+1}");
|
||
// }
|
||
// else
|
||
// {
|
||
// // 9个桶模式（3x3）- 原有方式，用于较大的哈希桶
|
||
// for (int dx = -1; dx <= 1; dx++)
|
||
// {
|
||
// for (int dy = -1; dy <= 1; dy++)
|
||
// {
|
||
// keys.Add($"{centerX + dx},{centerY + dy}");
|
||
// }
|
||
// }
|
||
// }
|
||
|
||
return keys;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 测试垂直线与模型项的相交
|
||
/// </summary>
|
||
/// <param name="item">模型项</param>
|
||
/// <param name="basePoint">基础点</param>
|
||
/// <param name="scanHeight">扫描高度</param>
|
||
/// <returns>相交数据，如果不相交则返回null</returns>
|
||
private IntersectionData TestVerticalLineIntersection(ModelItem item, Point3D basePoint, double scanHeight)
|
||
{
|
||
try
|
||
{
|
||
if (!item.HasGeometry)
|
||
return null;
|
||
|
||
// 直接提取模型项的三角形几何数据
|
||
var triangles = GeometryHelper.ExtractTrianglesOptimized(item);
|
||
if (triangles == null || triangles.Count == 0)
|
||
{
|
||
return null;
|
||
}
|
||
|
||
// 执行射线-三角形相交检测
|
||
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
|
||
{
|
||
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;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 执行垂直射线与三角形相交检测
|
||
/// </summary>
|
||
/// <param name="basePoint">射线起点</param>
|
||
/// <param name="scanHeight">扫描高度</param>
|
||
/// <param name="triangles">三角形列表</param>
|
||
/// <returns>相交点Z坐标列表</returns>
|
||
private List<double> PerformVerticalRayIntersection(Point3D basePoint, double scanHeight, List<Triangle3D> triangles)
|
||
{
|
||
var intersectionPoints = new List<double>();
|
||
|
||
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 (GeometryHelper.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;
|
||
}
|
||
}
|
||
|
||
|
||
/// <summary>
|
||
/// 检查模型项是否类似地面/楼板
|
||
/// </summary>
|
||
/// <param name="item">模型项</param>
|
||
/// <param name="intersectionData">相交数据</param>
|
||
/// <returns>是否为地面类型</returns>
|
||
private bool IsFloorLike(ModelItem item, IntersectionData intersectionData)
|
||
{
|
||
string displayName = item.DisplayName?.ToLower() ?? "";
|
||
|
||
// 通过名称判断
|
||
string[] floorKeywords = {
|
||
"地面", "floor", "楼板", "slab", "板", "deck",
|
||
"地板", "flooring", "基础", "foundation"
|
||
};
|
||
|
||
foreach (string keyword in floorKeywords)
|
||
{
|
||
if (displayName.Contains(keyword))
|
||
{
|
||
return true;
|
||
}
|
||
}
|
||
|
||
// 通过几何特征判断（薄的水平结构）
|
||
double thickness = intersectionData.MaxZ - intersectionData.MinZ;
|
||
return thickness < 0.5; // 小于50cm厚度认为是楼板
|
||
}
|
||
|
||
/// <summary>
|
||
/// 合并重叠的区间
|
||
/// </summary>
|
||
/// <param name="intervals">区间列表</param>
|
||
/// <returns>合并后的区间列表</returns>
|
||
private List<HeightInterval> MergeOverlappingIntervals(List<HeightInterval> intervals)
|
||
{
|
||
if (!intervals.Any())
|
||
return new List<HeightInterval>();
|
||
|
||
var sortedIntervals = intervals.OrderBy(i => i.MinZ).ToList();
|
||
var merged = new List<HeightInterval>();
|
||
|
||
var current = sortedIntervals[0];
|
||
for (int i = 1; i < sortedIntervals.Count; i++)
|
||
{
|
||
var next = sortedIntervals[i];
|
||
|
||
if (next.MinZ <= current.MaxZ + HEIGHT_TOLERANCE)
|
||
{
|
||
// 合并重叠区间
|
||
current = new HeightInterval(current.MinZ, Math.Max(current.MaxZ, next.MaxZ));
|
||
}
|
||
else
|
||
{
|
||
merged.Add(current);
|
||
current = next;
|
||
}
|
||
}
|
||
merged.Add(current);
|
||
|
||
return merged;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 检查指定的ModelItem是否为通道集合中任意一个通道的子节点
|
||
/// </summary>
|
||
/// <param name="item">要检查的ModelItem</param>
|
||
/// <param name="channelItemsSet">通道集合</param>
|
||
/// <returns>如果是通道的子节点则返回true</returns>
|
||
private bool IsChildOfChannelItems(ModelItem item, HashSet<ModelItem> channelItemsSet)
|
||
{
|
||
try
|
||
{
|
||
if (item?.Parent == null)
|
||
return false;
|
||
|
||
// 递归向上检查父节点链
|
||
var currentParent = item.Parent;
|
||
while (currentParent != null)
|
||
{
|
||
if (channelItemsSet.Contains(currentParent))
|
||
{
|
||
return true; // 找到了通道父节点
|
||
}
|
||
currentParent = currentParent.Parent;
|
||
}
|
||
|
||
return false;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Debug($"【垂直扫描处理器】 检查子节点关系时出错: {item?.DisplayName ?? "NULL"}, {ex.Message}");
|
||
return false; // 出错时保守处理，不排除
|
||
}
|
||
}
|
||
|
||
#endregion
|
||
|
||
#region 内部数据结构
|
||
|
||
/// <summary>
|
||
/// 相交数据结构
|
||
/// </summary>
|
||
private class IntersectionData
|
||
{
|
||
public double MinZ { get; set; }
|
||
public double MaxZ { get; set; }
|
||
public Point3D IntersectionPoint { get; set; }
|
||
}
|
||
|
||
/// <summary>
|
||
/// 相交测试结果
|
||
/// </summary>
|
||
private class IntersectionResult
|
||
{
|
||
public ModelItem ModelItem { get; set; }
|
||
public IntersectionData IntersectionData { get; set; }
|
||
public bool IsObstacle { get; set; }
|
||
public bool IsPassable { get; set; }
|
||
}
|
||
|
||
#endregion
|
||
}
|
||
} |