1844 lines
91 KiB
C#
1844 lines
91 KiB
C#
using System;
|
||
using System.Collections.Generic;
|
||
using System.Linq;
|
||
using System.Numerics;
|
||
using Autodesk.Navisworks.Api;
|
||
using NavisworksTransport.Utils;
|
||
using NavisworksTransport.Core.Config;
|
||
using NavisworksTransport.Utils.CoordinateSystem;
|
||
|
||
namespace NavisworksTransport.PathPlanning
|
||
{
|
||
/// <summary>
|
||
/// 网格生成模式
|
||
/// </summary>
|
||
public enum GridGenerationMode
|
||
{
|
||
/// <summary>
|
||
/// 通道基础2.5D模式 - 使用通道优先策略和垂直扫描的高性能模式
|
||
/// </summary>
|
||
ChannelBased2_5D,
|
||
|
||
/// <summary>
|
||
/// 边界框基础2.5D模式 - 使用直接包围盒遍历的优化算法
|
||
/// </summary>
|
||
BoundingBoxBased2_5D
|
||
}
|
||
|
||
/// <summary>
|
||
/// 网格地图生成器
|
||
/// 负责将BIM模型转换为路径规划可用的网格地图
|
||
/// 支持通道基础2.5D模式
|
||
/// </summary>
|
||
public class GridMapGenerator
|
||
{
|
||
/// <summary>
|
||
/// 获取边界检测的最大高度差（米）
|
||
/// 从配置文件读取，与AutoPathFinder保持一致
|
||
/// 用于判断层间是否构成边界（超过此阈值则标记为边界）
|
||
/// </summary>
|
||
private double GetMaxHeightDiffMeters()
|
||
{
|
||
return ConfigManager.Instance.Current.PathEditing.MaxHeightDiffMeters;
|
||
}
|
||
|
||
private readonly CategoryAttributeManager _categoryManager;
|
||
private readonly ChannelBasedGridBuilder _channelBuilder;
|
||
|
||
/// <summary>
|
||
/// 构造函数
|
||
/// </summary>
|
||
public GridMapGenerator()
|
||
{
|
||
_categoryManager = new CategoryAttributeManager();
|
||
_channelBuilder = new ChannelBasedGridBuilder();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 基于包围盒遍历的网格地图生成方法（优化版）
|
||
/// </summary>
|
||
/// <param name="document">Navisworks文档</param>
|
||
/// <param name="bounds">扫描边界</param>
|
||
/// <param name="cellSize">网格单元大小（米）</param>
|
||
/// <param name="objectRadius">物体半径（米）</param>
|
||
/// <param name="safetyMargin">安全间隙（米）</param>
|
||
/// <param name="planningStartPoint">规划起点</param>
|
||
/// <param name="planningEndPoint">规划终点</param>
|
||
/// <param name="objectHeight">物体高度（米）</param>
|
||
/// <returns>生成的网格地图</returns>
|
||
public GridMap GenerateFromBIM(BoundingBox3D bounds, double cellSize, double objectRadius, double safetyMargin, Point3D planningStartPoint, Point3D planningEndPoint, double objectHeight)
|
||
{
|
||
try
|
||
{
|
||
// 第一步：统一转换所有米制参数为模型单位
|
||
double metersToModelUnitsConversionFactor = UnitsConverter.GetMetersToUnitsConversionFactor(Application.ActiveDocument.Units);
|
||
double cellSizeInModelUnits = cellSize * metersToModelUnitsConversionFactor;
|
||
double objectRadiusInModelUnits = objectRadius * metersToModelUnitsConversionFactor;
|
||
double safetyMarginInModelUnits = safetyMargin * metersToModelUnitsConversionFactor;
|
||
double objectHeightInModelUnits = objectHeight * metersToModelUnitsConversionFactor;
|
||
double scanHeightInModelUnits = objectHeightInModelUnits + safetyMarginInModelUnits; // 扫描高度 = 物体高度 + 安全间隙
|
||
double totalInflationRadiusInModelUnits = objectRadiusInModelUnits + safetyMarginInModelUnits; // 膨胀半径 = 物体半径 + 安全间隙
|
||
|
||
LogManager.Info($"【生成网格地图】参数单位转换完成 (转换系数: {metersToModelUnitsConversionFactor:F2}):");
|
||
LogManager.Info($" 网格大小: {cellSize}米 → {cellSizeInModelUnits:F2}模型单位");
|
||
LogManager.Info($" 物体半径: {objectRadius}米 → {objectRadiusInModelUnits:F2}模型单位");
|
||
LogManager.Info($" 安全间隙: {safetyMargin}米 → {safetyMarginInModelUnits:F2}模型单位");
|
||
LogManager.Info($" 物体高度: {objectHeight}米 → {objectHeightInModelUnits:F2}模型单位");
|
||
LogManager.Info($" 扫描高度: {scanHeightInModelUnits:F2}模型单位");
|
||
LogManager.Info($" 膨胀半径: {totalInflationRadiusInModelUnits:F2}模型单位");
|
||
|
||
// 生成缓存键（使用转换后的模型单位参数确保一致性）
|
||
var cacheKey = GridMapCacheKey.CreateFrom(bounds, cellSizeInModelUnits, objectRadiusInModelUnits, safetyMarginInModelUnits, objectHeightInModelUnits);
|
||
|
||
// 尝试从缓存获取
|
||
var cachedGridMap = GlobalGridMapCache.Instance.Get(cacheKey, 5000); // 预估生成需要5秒
|
||
if (cachedGridMap != null)
|
||
{
|
||
LogManager.Info($"【GridMap缓存】使用缓存网格地图 - {cacheKey}");
|
||
LogManager.Info($"【GridMap缓存】缓存GridMap统计: {cachedGridMap.GetStatistics()}");
|
||
|
||
// 更新规划起终点信息（这些信息可能会变化，不影响核心网格结构）
|
||
if (planningStartPoint != default(Point3D) && planningEndPoint != default(Point3D))
|
||
{
|
||
cachedGridMap.PlanningStartPoint = planningStartPoint;
|
||
cachedGridMap.PlanningEndPoint = planningEndPoint;
|
||
cachedGridMap.HasPlanningPoints = true;
|
||
}
|
||
|
||
return cachedGridMap;
|
||
}
|
||
|
||
// 缓存未命中，生成新的GridMap
|
||
LogManager.Info("【生成网格地图】缓存未命中，开始生成新网格地图");
|
||
LogManager.Info($"【生成网格地图】缓存键: {cacheKey}");
|
||
var startTime = DateTime.Now;
|
||
|
||
// 1. 使用通道构建器构建基础通道覆盖网格
|
||
LogManager.Info("【生成网格地图】步骤1: 构建通道覆盖网格");
|
||
var channelCoverage = _channelBuilder.BuildChannelCoverage(cellSizeInModelUnits);
|
||
|
||
LogManager.Info($"【生成网格地图】通道覆盖构建完成: {channelCoverage.GetStatistics()}");
|
||
LogManager.Info($"【阶段1完成】通道覆盖网格统计: {channelCoverage.GridMap.GetStatistics()}");
|
||
|
||
// 1.5. 使用智能收集策略收集所有通道相关节点
|
||
LogManager.Info("【生成网格地图】步骤1.5: 智能收集通道相关节点");
|
||
|
||
// 直接获取所有可通行的物流模型项
|
||
var allTraversableItems = CategoryAttributeManager.GetAllTraversableLogisticsItems();
|
||
LogManager.Info($"【生成网格地图】直接获取到 {allTraversableItems.Count} 个可通行物流元素");
|
||
|
||
// 智能收集可通行模型相关节点（包括父节点、同级节点等）
|
||
var traversableRelatedItems = CollectRelatedItems(allTraversableItems.ToList());
|
||
LogManager.Info($"【生成网格地图】智能收集到 {traversableRelatedItems.Count} 个可通行元素相关节点");
|
||
|
||
// 1.6. 批量获取所有"无关项"（不参与网格生成的大型构件）
|
||
LogManager.Info("【生成网格地图】步骤1.6: 批量获取无关项并收集相关节点");
|
||
var irrelevantItems = CategoryAttributeManager.GetLogisticsItemsByType(
|
||
"无关项");
|
||
LogManager.Info($"【生成网格地图】直接获取到 {irrelevantItems.Count} 个无关项");
|
||
|
||
// 智能收集无关项相关节点（包括子节点等）
|
||
var irrelevantRelatedItems = CollectRelatedItems(irrelevantItems);
|
||
LogManager.Info($"【生成网格地图】智能收集到 {irrelevantRelatedItems.Count} 个无关项相关节点（将被排除）");
|
||
|
||
// 2. 先设置PassableHeights，确保障碍物高度检查时有正确的高度范围
|
||
LogManager.Info("【生成网格地图】步骤2: 为可通行网格设置高度约束");
|
||
SetChannelPassableHeights(channelCoverage.GridMap, scanHeightInModelUnits);
|
||
LogManager.Info($"【阶段2完成】高度约束设置后网格统计: {channelCoverage.GridMap.GetStatistics()}");
|
||
|
||
// 2.5. 处理障碍物（包围盒检测） - 使用高性能优化版本
|
||
LogManager.Info("【生成网格地图】步骤2.5: 高性能包围盒遍历处理障碍物");
|
||
ProcessObstaclesWithBoundingBox(channelCoverage.GridMap, traversableRelatedItems, scanHeightInModelUnits, channelCoverage, irrelevantRelatedItems);
|
||
|
||
LogManager.Info($"【阶段2.5完成】障碍物处理后网格统计: {channelCoverage.GridMap.GetStatistics()}");
|
||
|
||
// 2.6. 单独处理门元素，设置为可通行，并设置通行高度
|
||
LogManager.Info("【生成网格地图】步骤2.6: 处理门元素");
|
||
ProcessDoorElements(channelCoverage.GridMap, allTraversableItems);
|
||
LogManager.Info($"【阶段2.6完成】门元素处理后网格统计: {channelCoverage.GridMap.GetStatistics()}");
|
||
|
||
// 2.7. 标记边界层（用于膨胀计算）
|
||
LogManager.Info("【生成网格地图】步骤2.7: 标记边界层");
|
||
double maxHeightDiff = ConfigManager.Instance.Current.PathEditing.MaxHeightDiff;
|
||
LogManager.Info($"【生成网格地图】边界高度差阈值: {ConfigManager.Instance.Current.PathEditing.MaxHeightDiffMeters}米 = {maxHeightDiff:F2}模型单位");
|
||
MarkBoundaryLayers(channelCoverage.GridMap, maxHeightDiff);
|
||
LogManager.Info($"【阶段2.7完成】边界层标记后网格统计: {channelCoverage.GridMap.GetStatistics()}");
|
||
|
||
// 3. 应用物体尺寸膨胀
|
||
if (objectRadius > 0 || safetyMargin > 0)
|
||
{
|
||
LogManager.Info("【生成网格地图】步骤3: 应用物体膨胀");
|
||
ApplyObjectInflation(channelCoverage.GridMap, totalInflationRadiusInModelUnits);
|
||
LogManager.Info($"【生成网格地图】物体膨胀完成，膨胀半径: {totalInflationRadiusInModelUnits:F2}模型单位");
|
||
LogManager.Info($"【阶段3完成】物体膨胀后网格统计: {channelCoverage.GridMap.GetStatistics()}");
|
||
}
|
||
|
||
// 4. 设置规划起点和终点信息
|
||
if (planningStartPoint != default(Point3D) && planningEndPoint != default(Point3D))
|
||
{
|
||
channelCoverage.GridMap.PlanningStartPoint = planningStartPoint;
|
||
channelCoverage.GridMap.PlanningEndPoint = planningEndPoint;
|
||
channelCoverage.GridMap.HasPlanningPoints = true;
|
||
}
|
||
|
||
// 将通道数据设置到GridMap中
|
||
if (traversableRelatedItems != null && traversableRelatedItems.Any())
|
||
{
|
||
channelCoverage.GridMap.ChannelItems = traversableRelatedItems.ToList();
|
||
}
|
||
else if (channelCoverage.ChannelItems != null && channelCoverage.ChannelItems.Any())
|
||
{
|
||
channelCoverage.GridMap.ChannelItems = new List<ModelItem>(channelCoverage.ChannelItems);
|
||
}
|
||
|
||
var elapsed = (DateTime.Now - startTime).TotalMilliseconds;
|
||
LogManager.Info($"【生成网格地图】网格地图生成完成: {channelCoverage.GridMap.GetStatistics()}");
|
||
LogManager.Info($"【生成网格地图】总耗时: {elapsed:F1}ms");
|
||
|
||
// 将生成的GridMap添加到缓存
|
||
GlobalGridMapCache.Instance.Put(cacheKey, channelCoverage.GridMap);
|
||
LogManager.Info($"【GridMap缓存】网格地图已缓存，缓存键: {cacheKey}");
|
||
|
||
return channelCoverage.GridMap;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"【生成网格地图】生成失败: {ex.Message}");
|
||
return null;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取GridMap缓存统计信息
|
||
/// </summary>
|
||
/// <returns>缓存统计报告</returns>
|
||
public static string GetCacheStatistics()
|
||
{
|
||
return GlobalGridMapCache.Instance.Statistics.GenerateReport();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取GridMap缓存详细统计报告
|
||
/// </summary>
|
||
/// <returns>详细统计报告</returns>
|
||
public static string GetDetailedCacheReport()
|
||
{
|
||
return GlobalGridMapCache.Instance.GenerateDetailedReport();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 清除GridMap缓存
|
||
/// </summary>
|
||
public static void ClearCache()
|
||
{
|
||
GlobalGridMapCache.Instance.Clear();
|
||
LogManager.Info("[GridMap缓存] 手动清除所有缓存");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 处理门元素，将其设置为可通行网格
|
||
/// </summary>
|
||
private void ProcessDoorElements(GridMap gridMap, ICollection<ModelItem> allTraversableItems)
|
||
{
|
||
try
|
||
{
|
||
// 过滤出门类型的元素
|
||
var doorItems = FilterDoorItems(allTraversableItems);
|
||
LogManager.Info($"【门元素处理】找到 {doorItems.Count} 个门元素");
|
||
|
||
if (!doorItems.Any())
|
||
{
|
||
LogManager.Info("【门元素处理】没有找到门元素，跳过处理");
|
||
return;
|
||
}
|
||
|
||
int processedCount = 0;
|
||
int skippedCount = 0;
|
||
foreach (var doorItem in doorItems)
|
||
{
|
||
try
|
||
{
|
||
// 获取门元素的包围盒
|
||
var bbox = doorItem.BoundingBox();
|
||
if (bbox == null)
|
||
{
|
||
LogManager.Debug($"【门元素处理】门元素无有效包围盒，跳过: {doorItem.DisplayName}");
|
||
skippedCount++;
|
||
continue;
|
||
}
|
||
|
||
// 使用限宽计算门的实际开口区域
|
||
var coveredCells = CalculateDoorOpeningCoverage(doorItem, bbox, gridMap);
|
||
|
||
if (!coveredCells.Any())
|
||
{
|
||
LogManager.Warning($"【门元素处理】门元素 {doorItem.DisplayName} 无法计算有效开口区域，跳过处理");
|
||
skippedCount++;
|
||
continue;
|
||
}
|
||
|
||
// 获取门的限高属性
|
||
string heightLimitStr = CategoryAttributeManager.GetLogisticsPropertyValue(doorItem, CategoryAttributeManager.LogisticsProperties.HEIGHT_LIMIT);
|
||
double configuredHeight = CategoryAttributeManager.ParseLogisticsLimitValue(heightLimitStr, "限高");
|
||
|
||
var (doorBottomElevation, doorTopElevation) = GetObstacleElevationRange(
|
||
bbox.Min.X, bbox.Min.Y, bbox.Min.Z,
|
||
bbox.Max.X, bbox.Max.Y, bbox.Max.Z,
|
||
gridMap.CoordinateSystemType);
|
||
|
||
// 计算门的实际物理高度
|
||
double actualDoorHeight = doorTopElevation - doorBottomElevation;
|
||
|
||
double doorHeight;
|
||
if (configuredHeight <= 0)
|
||
{
|
||
// 没有限高配置，使用实际门高
|
||
doorHeight = actualDoorHeight;
|
||
LogManager.Debug($"【门元素处理】门元素 {doorItem.DisplayName} 未设置限高，使用包围盒高度: {doorHeight:F2}");
|
||
}
|
||
else
|
||
{
|
||
// 转换限高到模型单位
|
||
double metersToModelUnits = UnitsConverter.GetMetersToUnitsConversionFactor(Application.ActiveDocument.Units);
|
||
double configuredHeightInModelUnits = configuredHeight * metersToModelUnits;
|
||
|
||
// 取实际门高和配置限高的最小值
|
||
doorHeight = Math.Min(actualDoorHeight, configuredHeightInModelUnits);
|
||
LogManager.Debug($"【门元素处理】门元素 {doorItem.DisplayName} 实际高度: {actualDoorHeight:F2}, 限高:{configuredHeightInModelUnits:F2}, 有效高度: {doorHeight:F2}");
|
||
}
|
||
|
||
// 获取门的限速
|
||
double doorSpeedLimit = CategoryAttributeManager.GetSpeedLimit(doorItem);
|
||
LogManager.Info($"[门网格限速] 门物品 '{doorItem.DisplayName}' 限速: {doorSpeedLimit:F2}m/s");
|
||
|
||
// 设置开口区域的网格为可通行的门类型
|
||
foreach (var (x, y) in coveredCells)
|
||
{
|
||
var gridPos = new GridPoint2D(x, y);
|
||
|
||
// 计算门网格的精确世界坐标
|
||
var world2D = gridMap.GridToWorld2D(gridPos);
|
||
var preciseWorldPosition = gridMap.SetWorldElevation(world2D, doorBottomElevation);
|
||
|
||
// 使用GridCellBuilder创建完整配置的门GridCell
|
||
var cell = GridCellBuilder.Door(preciseWorldPosition, doorItem, doorSpeedLimit);
|
||
LogManager.Info($"[门网格创建] 位置({preciseWorldPosition.X:F2},{preciseWorldPosition.Y:F2},{preciseWorldPosition.Z:F2}) 限速: {cell.SpeedLimit:F2}m/s");
|
||
|
||
// 多层架构：PassableHeight现在在HeightLayer中设置
|
||
// 门的高度范围由AddHeightLayer时设置
|
||
|
||
// 一次性放置完整配置的GridCell
|
||
gridMap.PlaceCell(gridPos, cell);
|
||
LogManager.Info($"[门网格放置] 网格坐标({gridPos.X},{gridPos.Y}) 最终限速: {gridMap.GetCell(gridPos)?.SpeedLimit:F2}m/s");
|
||
|
||
// 为门添加可通行的高度层
|
||
var doorLayer = new HeightLayer
|
||
{
|
||
Type = "门",
|
||
Z = doorBottomElevation,
|
||
PassableHeight = new HeightInterval(doorBottomElevation, doorBottomElevation + doorHeight),
|
||
IsWalkable = true, // 关键：门是可通行的
|
||
SpeedLimit = doorSpeedLimit,
|
||
SourceItem = doorItem,
|
||
IsBoundary = false
|
||
};
|
||
gridMap.AddHeightLayer(gridPos, doorLayer);
|
||
LogManager.Info($"[门高度层添加] 网格({gridPos.X},{gridPos.Y}) 高度范围: [{doorBottomElevation:F2}, {doorBottomElevation + doorHeight:F2}], 可通行高度: {doorHeight:F2}");
|
||
}
|
||
|
||
processedCount++;
|
||
LogManager.Debug($"【门元素处理】已处理门元素: {doorItem.DisplayName}，开口区域覆盖网格数量: {coveredCells.Count}");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"【门元素处理】处理门元素失败: {doorItem.DisplayName}, 错误: {ex.Message}");
|
||
skippedCount++;
|
||
}
|
||
}
|
||
|
||
LogManager.Info($"【门元素处理】完成，已处理 {processedCount} 个门元素，跳过 {skippedCount} 个");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"【门元素处理】处理失败: {ex.Message}");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 为所有可通行网格设置PassableHeights确保高度约束检查
|
||
/// </summary>
|
||
/// <param name="gridMap">网格地图</param>
|
||
/// <param name="scanHeightInModelUnits">扫描高度（模型单位）</param>
|
||
private void SetChannelPassableHeights(GridMap gridMap, double scanHeightInModelUnits)
|
||
{
|
||
LogManager.Info($"【高度约束设置】开始为所有可通行网格设置高度约束，扫描高度: {scanHeightInModelUnits:F2}模型单位");
|
||
|
||
int processedCount = 0;
|
||
int layerCount = 0;
|
||
int stairBottomLayersProcessed = 0;
|
||
|
||
for (int x = 0; x < gridMap.Width; x++)
|
||
{
|
||
for (int y = 0; y < gridMap.Height; y++)
|
||
{
|
||
var cell = gridMap.Cells[x, y];
|
||
if (cell.HasAnyWalkableLayer())
|
||
{
|
||
// 检查是否是楼梯或电梯网格（有多层且包含楼梯/电梯层）
|
||
bool hasStairsOrElevator = cell.HeightLayers.Any(layer =>
|
||
layer.Type == "楼梯" ||
|
||
layer.Type == "电梯");
|
||
|
||
// 为每个高度层设置PassableHeight
|
||
for (int i = 0; i < cell.HeightLayers.Count; i++)
|
||
{
|
||
var layer = cell.HeightLayers[i];
|
||
|
||
// 特殊处理：楼梯/电梯的第0层（下方通道层）
|
||
if (hasStairsOrElevator && i == 0 && cell.HeightLayers.Count >= 2)
|
||
{
|
||
// Layer 0是楼梯下方的通道层
|
||
// PassableHeight应该是从层顶到上方楼梯底面的距离
|
||
var upperLayer = cell.HeightLayers[1]; // 楼梯层
|
||
double bottomLayerTop = layer.Z; // 下层顶部
|
||
double upperLayerBottom = upperLayer.Z; // 上层底部
|
||
|
||
// 计算实际可通行高度
|
||
double actualPassableHeight = upperLayerBottom - bottomLayerTop;
|
||
|
||
if (actualPassableHeight > 0)
|
||
{
|
||
layer.PassableHeight = new HeightInterval(0, actualPassableHeight);
|
||
stairBottomLayersProcessed++;
|
||
}
|
||
else
|
||
{
|
||
// 如果计算出负值或0，使用默认值
|
||
layer.PassableHeight = new HeightInterval(0, scanHeightInModelUnits);
|
||
LogManager.Warning($"【楼梯下方高度】网格({x},{y}) Layer{i} 计算高度异常: {actualPassableHeight:F2}，使用默认值");
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// 普通层或楼梯层本身，使用标准扫描高度
|
||
layer.PassableHeight = new HeightInterval(0, scanHeightInModelUnits);
|
||
}
|
||
|
||
cell.HeightLayers[i] = layer;
|
||
layerCount++;
|
||
}
|
||
|
||
gridMap.Cells[x, y] = cell;
|
||
processedCount++;
|
||
}
|
||
}
|
||
}
|
||
|
||
LogManager.Info($"【高度约束设置】完成，已处理 {processedCount} 个网格，{layerCount} 个高度层");
|
||
LogManager.Info($"【高度约束设置】其中楼梯/电梯下方层: {stairBottomLayersProcessed} 个（使用实际可通行高度）");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 标记边界层（用于膨胀计算）
|
||
/// 边界定义：当前网格层数 > 邻居网格层数（梯度≥1），且层间高度差超过阈值
|
||
/// </summary>
|
||
/// <param name="gridMap">网格地图</param>
|
||
/// <param name="maxHeightDiffInModelUnits">最大允许高度差（模型单位）</param>
|
||
private void MarkBoundaryLayers(GridMap gridMap, double maxHeightDiffInModelUnits)
|
||
{
|
||
LogManager.Info($"【边界检测】开始标记边界层，高度差阈值: {maxHeightDiffInModelUnits:F2}模型单位");
|
||
|
||
int totalBoundaryLayers = 0;
|
||
int totalCheckedCells = 0;
|
||
|
||
// 4个方向：上下左右
|
||
int[] dx = { 0, 0, -1, 1 };
|
||
int[] dy = { -1, 1, 0, 0 };
|
||
string[] dirNames = { "下", "上", "左", "右" };
|
||
|
||
for (int x = 0; x < gridMap.Width; x++)
|
||
{
|
||
for (int y = 0; y < gridMap.Height; y++)
|
||
{
|
||
var cell = gridMap.Cells[x, y];
|
||
|
||
// 只检查可通行且有高度层的网格
|
||
if (!cell.HasAnyWalkableLayer())
|
||
continue;
|
||
|
||
totalCheckedCells++;
|
||
int currentLayerCount = cell.HeightLayers.Count;
|
||
|
||
// 检查4个方向的邻居
|
||
for (int dir = 0; dir < 4; dir++)
|
||
{
|
||
int nx = x + dx[dir];
|
||
int ny = y + dy[dir];
|
||
|
||
// 情况1：网格地图边界 - 标记当前网格所有层为边界
|
||
if (nx < 0 || nx >= gridMap.Width || ny < 0 || ny >= gridMap.Height)
|
||
{
|
||
for (int li = 0; li < currentLayerCount; li++)
|
||
{
|
||
var layer = cell.HeightLayers[li];
|
||
if (!layer.IsBoundary)
|
||
{
|
||
layer.IsBoundary = true;
|
||
cell.HeightLayers[li] = layer;
|
||
totalBoundaryLayers++;
|
||
//LogManager.Debug($"【边界检测】网格({x},{y}) Layer{li} 标记为边界（地图边界），方向={dirNames[dir]}");
|
||
}
|
||
}
|
||
continue;
|
||
}
|
||
|
||
var neighbor = gridMap.Cells[nx, ny];
|
||
int neighborLayerCount = (neighbor.HeightLayers != null) ? neighbor.HeightLayers.Count : 0;
|
||
|
||
// 情况2：邻居是Unknown - 标记当前网格所有层为边界
|
||
if (neighbor.CellType == "空洞")
|
||
{
|
||
for (int li = 0; li < currentLayerCount; li++)
|
||
{
|
||
var layer = cell.HeightLayers[li];
|
||
if (!layer.IsBoundary)
|
||
{
|
||
layer.IsBoundary = true;
|
||
cell.HeightLayers[li] = layer;
|
||
totalBoundaryLayers++;
|
||
//LogManager.Debug($"【边界检测】网格({x},{y}) Layer{li} 标记为边界（邻居Unknown），方向={dirNames[dir]}");
|
||
}
|
||
}
|
||
continue;
|
||
}
|
||
|
||
// 情况3：层数梯度检测：当前层数 > 邻居层数
|
||
if (currentLayerCount <= neighborLayerCount)
|
||
continue;
|
||
|
||
int gradient = currentLayerCount - neighborLayerCount;
|
||
|
||
// 检查每个梯度层是否构成边界
|
||
// 例如：当前2层，邻居1层，梯度=1，检查当前Layer[1]是否是边界
|
||
// 例如：当前3层，邻居1层，梯度=2，检查Layer[1]和Layer[2]
|
||
for (int layerOffset = 1; layerOffset <= gradient; layerOffset++)
|
||
{
|
||
int currentLayerIndex = neighborLayerCount + layerOffset - 1;
|
||
|
||
if (currentLayerIndex >= currentLayerCount)
|
||
continue;
|
||
|
||
var currentLayer = cell.HeightLayers[currentLayerIndex];
|
||
|
||
// 获取对比层的Z值
|
||
double currentZ = currentLayer.Z;
|
||
double neighborZ = 0;
|
||
|
||
if (neighborLayerCount > 0)
|
||
{
|
||
// 邻居有层：取邻居最高层的Z值
|
||
neighborZ = neighbor.HeightLayers[neighborLayerCount - 1].Z;
|
||
}
|
||
else
|
||
{
|
||
// 邻居无层（Unknown）：使用网格底部Z值
|
||
neighborZ = gridMap.Bounds != null
|
||
? ResolveBoundsMinElevation(
|
||
gridMap.Bounds.Min.X,
|
||
gridMap.Bounds.Min.Y,
|
||
gridMap.Bounds.Min.Z,
|
||
gridMap.CoordinateSystemType)
|
||
: 0.0;
|
||
}
|
||
|
||
// 计算层间高度差
|
||
double heightDiff = Math.Abs(currentZ - neighborZ);
|
||
|
||
// 高度差判定
|
||
if (heightDiff > maxHeightDiffInModelUnits)
|
||
{
|
||
// 高度差超过阈值，标记为边界
|
||
if (!currentLayer.IsBoundary)
|
||
{
|
||
currentLayer.IsBoundary = true;
|
||
cell.HeightLayers[currentLayerIndex] = currentLayer;
|
||
totalBoundaryLayers++;
|
||
|
||
// LogManager.Debug($"【边界检测】网格({x},{y}) Layer{currentLayerIndex} 标记为边界，" +
|
||
// $"方向={dirNames[dir]}, 当前Z={currentZ:F2}, 邻居Z={neighborZ:F2}, " +
|
||
// $"高度差={heightDiff:F2}, 梯度={gradient}");
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
gridMap.Cells[x, y] = cell;
|
||
}
|
||
}
|
||
|
||
LogManager.Info($"【边界检测】完成，检查了 {totalCheckedCells} 个网格，标记了 {totalBoundaryLayers} 个边界层");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 从物流元素集合中过滤出门类型的元素
|
||
/// </summary>
|
||
private List<ModelItem> FilterDoorItems(ICollection<ModelItem> items)
|
||
{
|
||
var doorItems = new List<ModelItem>();
|
||
|
||
if (items == null || !items.Any())
|
||
{
|
||
return doorItems;
|
||
}
|
||
|
||
foreach (var item in items)
|
||
{
|
||
try
|
||
{
|
||
var logisticsType = CategoryAttributeManager.GetCategoryId(item);
|
||
if (logisticsType == "门")
|
||
{
|
||
doorItems.Add(item);
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Debug($"【门元素过滤】获取物流类型失败: {item.DisplayName}, 错误: {ex.Message}");
|
||
}
|
||
}
|
||
|
||
return doorItems;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 生成网格点坐标用于垂直扫描
|
||
/// </summary>
|
||
/// <param name="gridMap">网格地图</param>
|
||
/// <returns>网格点坐标列表</returns>
|
||
private List<Point3D> GenerateGridPoints(GridMap gridMap)
|
||
{
|
||
var points = new List<Point3D>();
|
||
|
||
// 只为通道网格生成扫描点，不扫描整个网格
|
||
for (int x = 0; x < gridMap.Width; x++)
|
||
{
|
||
for (int y = 0; y < gridMap.Height; y++)
|
||
{
|
||
var cell = gridMap.Cells[x, y];
|
||
|
||
// 只为通道类型的网格生成扫描点
|
||
if (cell.CellType == "通道" && cell.IsInChannel)
|
||
{
|
||
// 使用网格的实际Z坐标
|
||
var worldPos = gridMap.GridToWorld3D(new GridPoint2D(x, y));
|
||
|
||
// 重要修复：使用通道顶面作为垂直扫描的起点，而不是底面
|
||
// 从通道构建器的日志可知通道总边界MaxZ = 38.8，这是正确的扫描起点
|
||
double channelTopElevation = GetChannelTopZ(worldPos, gridMap);
|
||
|
||
points.Add(gridMap.SetWorldElevation(worldPos, channelTopElevation));
|
||
}
|
||
}
|
||
}
|
||
|
||
return points;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取通道顶面Z坐标作为垂直扫描起点
|
||
/// </summary>
|
||
/// <param name="worldPos">世界坐标位置</param>
|
||
/// <param name="gridMap">网格地图</param>
|
||
/// <returns>通道顶面Z坐标</returns>
|
||
private double GetChannelTopZ(Point3D worldPos, GridMap gridMap)
|
||
{
|
||
try
|
||
{
|
||
if (gridMap.Bounds != null)
|
||
{
|
||
var (minElevation, maxElevation) =
|
||
GetObstacleElevationRange(gridMap.Bounds.Min, gridMap.Bounds.Max, gridMap);
|
||
|
||
if (maxElevation > minElevation)
|
||
{
|
||
return maxElevation;
|
||
}
|
||
}
|
||
|
||
double fallbackTopElevation = gridMap.GetWorldElevation(worldPos) + 3.4;
|
||
LogManager.Debug($"[通道顶面计算] 使用备选方案: 底面高程{gridMap.GetWorldElevation(worldPos):F2} + 3.4m = 顶面高程{fallbackTopElevation:F2}");
|
||
return fallbackTopElevation;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Warning($"[通道顶面计算] 计算失败: {ex.Message}，使用世界位置高程");
|
||
return gridMap.GetWorldElevation(worldPos);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将高度区间信息集成到网格地图中
|
||
/// </summary>
|
||
/// <param name="gridMap">网格地图</param>
|
||
/// <param name="heightIntervals">高度区间数据</param>
|
||
private void IntegrateHeightIntervalsToGrid(GridMap gridMap, Dictionary<Point3D, HeightInterval> heightIntervals)
|
||
{
|
||
int updatedCells = 0;
|
||
int channelCellsWithHeightData = 0;
|
||
int channelCellsBecomingUnwalkable = 0;
|
||
|
||
foreach (var kvp in heightIntervals)
|
||
{
|
||
var point = kvp.Key;
|
||
var interval = kvp.Value;
|
||
|
||
// 将世界坐标转换为网格坐标
|
||
var gridPos = gridMap.WorldToGrid(point);
|
||
int gridX = gridPos.X;
|
||
int gridY = gridPos.Y;
|
||
|
||
// 确保坐标在有效范围内
|
||
if (gridX >= 0 && gridX < gridMap.Width && gridY >= 0 && gridY < gridMap.Height)
|
||
{
|
||
// 更新网格单元格的高度区间信息
|
||
var cell = gridMap.Cells[gridX, gridY];
|
||
|
||
// 多层架构：高度区间在HeightLayer中设置，这里不再需要
|
||
// cell.PassableHeight = interval;
|
||
|
||
// 关键修复：只处理通道单元格的高度信息集成
|
||
// 非通道单元格不应该通过高度扫描而改变其类型或可通行性
|
||
if (cell.CellType == "通道" && cell.IsInChannel)
|
||
{
|
||
channelCellsWithHeightData++;
|
||
|
||
if (interval.MaxZ > interval.MinZ)
|
||
{
|
||
// 通道区域有足够净空高度，设置为可通行
|
||
var cellPos = new GridPoint2D(gridX, gridY);
|
||
|
||
// 创建更新后的通道GridCell，保持原有属性但更新位置和高度
|
||
var updatedCell = new GridCell
|
||
{
|
||
CellType = "通道",
|
||
IsInChannel = true,
|
||
ChannelType = ChannelType.Corridor,
|
||
RelatedModelItem = cell.RelatedModelItem, // 保持原有关联
|
||
SpeedLimit = cell.SpeedLimit, // 保持原有限速
|
||
HeightLayers = cell.HeightLayers, // 保持高度层
|
||
Cost = 1.0
|
||
};
|
||
|
||
// 放置更新的GridCell
|
||
gridMap.PlaceCell(cellPos, updatedCell);
|
||
}
|
||
else
|
||
{
|
||
// 通道区域但净空高度不足，标记为不可通行但保持通道类型
|
||
var cellPos = new GridPoint2D(gridX, gridY);
|
||
|
||
// 创建高度不足的通道GridCell，保持原有属性但设为不可通行
|
||
var updatedCell = new GridCell
|
||
{
|
||
CellType = "通道",
|
||
IsInChannel = true,
|
||
ChannelType = ChannelType.Corridor,
|
||
RelatedModelItem = cell.RelatedModelItem, // 保持原有关联
|
||
SpeedLimit = cell.SpeedLimit, // 保持原有限速
|
||
HeightLayers = cell.HeightLayers, // 保持高度层
|
||
Cost = double.MaxValue
|
||
};
|
||
|
||
// 放置更新的GridCell
|
||
gridMap.PlaceCell(cellPos, updatedCell);
|
||
|
||
channelCellsBecomingUnwalkable++;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// 非通道单元格：不应该通过垂直扫描改变其状态
|
||
// 这种情况理论上不应该发生，因为只对通道单元格进行扫描
|
||
LogManager.Warning($"[通道2.5D模式] 警告：非通道单元格 ({gridX},{gridY}) 收到高度数据，类型：{cell.CellType}, IsInChannel：{cell.IsInChannel}");
|
||
|
||
// 多层架构：高度区间在HeightLayer中设置
|
||
// cell.PassableHeight = interval;
|
||
gridMap.Cells[gridX, gridY] = cell;
|
||
}
|
||
updatedCells++;
|
||
}
|
||
}
|
||
|
||
LogManager.Info($"[通道2.5D模式] 高度信息集成完成，更新了 {updatedCells} 个网格单元格");
|
||
LogManager.Info($"[通道2.5D模式] 处理的通道单元格数量: {channelCellsWithHeightData}, 因高度不足变为不可通行: {channelCellsBecomingUnwalkable}");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证网格统计一致性，确保可通行单元格数量不超过预期
|
||
/// </summary>
|
||
/// <param name="gridMap">要验证的网格地图</param>
|
||
/// <param name="stage">验证阶段描述</param>
|
||
private void ValidateGridStatisticsConsistency(GridMap gridMap, string stage)
|
||
{
|
||
int totalCells = gridMap.Width * gridMap.Height;
|
||
int walkableCount = 0;
|
||
int channelCount = 0;
|
||
int openSpaceCount = 0;
|
||
int obstacleCount = 0;
|
||
int doorCount = 0;
|
||
|
||
for (int x = 0; x < gridMap.Width; x++)
|
||
{
|
||
for (int y = 0; y < gridMap.Height; y++)
|
||
{
|
||
var cell = gridMap.Cells[x, y];
|
||
|
||
if (cell.HasAnyWalkableLayer())
|
||
{
|
||
walkableCount++;
|
||
switch (cell.CellType)
|
||
{
|
||
case "通道":
|
||
channelCount++;
|
||
break;
|
||
case "楼板":
|
||
openSpaceCount++;
|
||
break;
|
||
case "门":
|
||
doorCount++;
|
||
break;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if (cell.CellType == "障碍物")
|
||
{
|
||
obstacleCount++;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
LogManager.Info($"[网格验证] {stage} - 总单元格: {totalCells}, 可通行: {walkableCount}");
|
||
LogManager.Info($"[网格验证] 可通行单元格明细 - 通道: {channelCount}, 开放空间: {openSpaceCount}, 门: {doorCount}");
|
||
LogManager.Info($"[网格验证] 不可通行单元格 - 障碍物: {obstacleCount}, 其他: {totalCells - walkableCount - obstacleCount}");
|
||
|
||
// 验证：可通行的开放空间不应该在通道2.5D模式中出现，除非是合理的扩展
|
||
if (openSpaceCount > 0)
|
||
{
|
||
LogManager.Warning($"[网格验证] 警告: 发现 {openSpaceCount} 个可通行的开放空间单元格，这在通道2.5D模式中可能是异常的");
|
||
}
|
||
|
||
// 验证：总可通行数量的合理性检查
|
||
if (walkableCount > channelCount * 1.2) // 允许20%的合理扩展
|
||
{
|
||
LogManager.Warning($"[网格验证] 警告: 可通行单元格数量({walkableCount})明显超过通道单元格数量({channelCount})");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 应用物体尺寸膨胀
|
||
/// 在障碍物周围扩展不可通行区域，考虑物体尺寸
|
||
/// </summary>
|
||
/// <param name="gridMap">网格地图</param>
|
||
/// <param name="objectRadiusInModelUnits">物体半径（模型单位）</param>
|
||
private void ApplyObjectInflation(GridMap gridMap, double objectRadiusInModelUnits)
|
||
{
|
||
if (objectRadiusInModelUnits <= 0) return;
|
||
|
||
try
|
||
{
|
||
// 计算膨胀半径（网格单元格数）
|
||
int inflationRadius = (int)Math.Ceiling(objectRadiusInModelUnits / gridMap.CellSize);
|
||
LogManager.Info($"[高效膨胀] 膨胀半径: {inflationRadius}个网格单元");
|
||
|
||
// 使用高效的距离变换算法
|
||
ApplyObjectInflation(gridMap, inflationRadius);
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"[高效膨胀] 应用物体膨胀时发生错误: {ex.Message}");
|
||
throw new AutoPathPlanningException($"物体膨胀失败: {ex.Message}", ex);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 多层膨胀算法 - 分两次处理：障碍物膨胀 + 边界膨胀
|
||
/// 核心原则：障碍物从外围膨胀（不含本身），边界从边界本身膨胀（包含边界）
|
||
/// </summary>
|
||
private void ApplyObjectInflation(GridMap gridMap, int inflationRadius)
|
||
{
|
||
var stopwatch = System.Diagnostics.Stopwatch.StartNew();
|
||
|
||
LogManager.Info($"【多层膨胀】开始按层处理膨胀，网格大小: {gridMap.Width}x{gridMap.Height}, 膨胀半径: {inflationRadius}");
|
||
|
||
// 计算高度差阈值（用于楼梯口保护）
|
||
double maxHeightDiff = ConfigManager.Instance.Current.PathEditing.MaxHeightDiff;
|
||
|
||
// 统计每个层索引的处理情况
|
||
var layerStats = new Dictionary<int, (int obstacleInflated, int boundaryCount, int boundaryInflated)>();
|
||
int maxLayerIndex = 0;
|
||
|
||
// 第一步：找出最大层索引
|
||
for (int x = 0; x < gridMap.Width; x++)
|
||
{
|
||
for (int y = 0; y < gridMap.Height; y++)
|
||
{
|
||
var cell = gridMap.Cells[x, y];
|
||
if (cell.HeightLayers != null && cell.HeightLayers.Count > 0)
|
||
{
|
||
maxLayerIndex = Math.Max(maxLayerIndex, cell.HeightLayers.Count - 1);
|
||
}
|
||
}
|
||
}
|
||
|
||
LogManager.Info($"【多层膨胀】检测到最大层索引: {maxLayerIndex}");
|
||
|
||
// 第二步：对每个层索引分别处理膨胀
|
||
for (int layerIndex = 0; layerIndex <= maxLayerIndex; layerIndex++)
|
||
{
|
||
// 步骤1：障碍物膨胀（不包括障碍物本身）
|
||
int obstacleInflated = InflateObstacles(gridMap, layerIndex, inflationRadius);
|
||
|
||
// 步骤2：边界膨胀（包括边界本身）
|
||
var (boundaryCount, boundaryInflated) = InflateBoundaries(gridMap, layerIndex, inflationRadius, maxHeightDiff);
|
||
|
||
if (boundaryCount > 0 || obstacleInflated > 0)
|
||
{
|
||
layerStats[layerIndex] = (obstacleInflated, boundaryCount, boundaryInflated);
|
||
LogManager.Info($"【多层膨胀】Layer{layerIndex} 完成 - 障碍物膨胀: {obstacleInflated}层, 边界: {boundaryCount}个, 边界膨胀: {boundaryInflated}层");
|
||
}
|
||
}
|
||
|
||
stopwatch.Stop();
|
||
LogManager.Info($"【多层膨胀】完成，耗时: {stopwatch.ElapsedMilliseconds}ms");
|
||
|
||
foreach (var kvp in layerStats.OrderBy(k => k.Key))
|
||
{
|
||
LogManager.Info($"【多层膨胀】Layer{kvp.Key}: 障碍物膨胀{kvp.Value.obstacleInflated}层, 边界{kvp.Value.boundaryCount}个, 边界膨胀{kvp.Value.boundaryInflated}层");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 距离变换算法 - 8连通距离计算
|
||
/// </summary>
|
||
private void DistanceTransform(double[,] distanceMap, int width, int height)
|
||
{
|
||
const double SQRT2 = 1.414213562373095;
|
||
|
||
// 正向扫描（从左上到右下）
|
||
for (int x = 0; x < width; x++)
|
||
{
|
||
for (int y = 0; y < height; y++)
|
||
{
|
||
if (distanceMap[x, y] == double.MaxValue)
|
||
{
|
||
double minDist = double.MaxValue;
|
||
|
||
// 从邻居计算距离，跳过 < 0 的邻居（没有该层的网格）
|
||
if (x > 0 && distanceMap[x - 1, y] >= 0.0)
|
||
minDist = Math.Min(minDist, distanceMap[x - 1, y] + 1.0);
|
||
if (y > 0 && distanceMap[x, y - 1] >= 0.0)
|
||
minDist = Math.Min(minDist, distanceMap[x, y - 1] + 1.0);
|
||
if (x > 0 && y > 0 && distanceMap[x - 1, y - 1] >= 0.0)
|
||
minDist = Math.Min(minDist, distanceMap[x - 1, y - 1] + SQRT2);
|
||
if (x > 0 && y < height - 1 && distanceMap[x - 1, y + 1] >= 0.0)
|
||
minDist = Math.Min(minDist, distanceMap[x - 1, y + 1] + SQRT2);
|
||
|
||
if (minDist != double.MaxValue)
|
||
distanceMap[x, y] = minDist;
|
||
}
|
||
}
|
||
}
|
||
|
||
// 反向扫描（从右下到左上）
|
||
for (int x = width - 1; x >= 0; x--)
|
||
{
|
||
for (int y = height - 1; y >= 0; y--)
|
||
{
|
||
if (distanceMap[x, y] > 0.0) // 排除distance=0和没有该层(-1)
|
||
{
|
||
double minDist = distanceMap[x, y];
|
||
|
||
// 从邻居计算距离，跳过 < 0 的邻居（没有该层的网格）
|
||
if (x < width - 1 && distanceMap[x + 1, y] >= 0.0)
|
||
minDist = Math.Min(minDist, distanceMap[x + 1, y] + 1.0);
|
||
if (y < height - 1 && distanceMap[x, y + 1] >= 0.0)
|
||
minDist = Math.Min(minDist, distanceMap[x, y + 1] + 1.0);
|
||
if (x < width - 1 && y < height - 1 && distanceMap[x + 1, y + 1] >= 0.0)
|
||
minDist = Math.Min(minDist, distanceMap[x + 1, y + 1] + SQRT2);
|
||
if (x < width - 1 && y > 0 && distanceMap[x + 1, y - 1] >= 0.0)
|
||
minDist = Math.Min(minDist, distanceMap[x + 1, y - 1] + SQRT2);
|
||
|
||
distanceMap[x, y] = minDist;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 障碍物膨胀 - 从障碍物外围开始膨胀，不包括障碍物本身
|
||
/// </summary>
|
||
private int InflateObstacles(GridMap gridMap, int layerIndex, int inflationRadius)
|
||
{
|
||
int inflatedCount = 0;
|
||
var distanceMap = new double[gridMap.Width, gridMap.Height];
|
||
|
||
// 初始化：只标记障碍物
|
||
for (int x = 0; x < gridMap.Width; x++)
|
||
{
|
||
for (int y = 0; y < gridMap.Height; y++)
|
||
{
|
||
var cell = gridMap.Cells[x, y];
|
||
distanceMap[x, y] = double.MaxValue;
|
||
|
||
// Unknown网格 → distance=0（整个网格无法通行）
|
||
if (cell.CellType == "空洞")
|
||
{
|
||
distanceMap[x, y] = 0.0;
|
||
continue;
|
||
}
|
||
|
||
// 检查该层索引是否存在
|
||
if (cell.HeightLayers != null && layerIndex < cell.HeightLayers.Count)
|
||
{
|
||
var layer = cell.HeightLayers[layerIndex];
|
||
|
||
// 该层不可通行 → distance=0（障碍物层）
|
||
if (!layer.IsWalkable)
|
||
{
|
||
distanceMap[x, y] = 0.0;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// 该层索引不存在 → distance=-1，不参与距离变换
|
||
distanceMap[x, y] = -1.0;
|
||
}
|
||
}
|
||
}
|
||
|
||
// 距离变换
|
||
DistanceTransform(distanceMap, gridMap.Width, gridMap.Height);
|
||
|
||
// 应用膨胀：distance > 0 && distance <= inflationRadius（不包括障碍物本身）
|
||
for (int x = 0; x < gridMap.Width; x++)
|
||
{
|
||
for (int y = 0; y < gridMap.Height; y++)
|
||
{
|
||
var cell = gridMap.Cells[x, y];
|
||
|
||
if (cell.HeightLayers != null && layerIndex < cell.HeightLayers.Count)
|
||
{
|
||
var layer = cell.HeightLayers[layerIndex];
|
||
|
||
// 门保护：不膨胀门类型的层
|
||
if (layer.Type == "门")
|
||
continue;
|
||
|
||
// 障碍物膨胀条件：不包括障碍物本身（distance=0）
|
||
// 例如：半径=2 → 膨胀distance=1,2（障碍物外围2格）
|
||
if (distanceMap[x, y] > 0.0 && distanceMap[x, y] <= inflationRadius)
|
||
{
|
||
layer.IsWalkable = false;
|
||
cell.HeightLayers[layerIndex] = layer;
|
||
inflatedCount++;
|
||
}
|
||
}
|
||
|
||
gridMap.Cells[x, y] = cell;
|
||
}
|
||
}
|
||
|
||
return inflatedCount;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 边界膨胀 - 从边界网格本身开始膨胀，包括边界网格
|
||
/// </summary>
|
||
private (int boundaryCount, int inflatedCount) InflateBoundaries(GridMap gridMap, int layerIndex, int inflationRadius, double maxHeightDiffInModelUnits)
|
||
{
|
||
int boundaryCount = 0;
|
||
int inflatedCount = 0;
|
||
var distanceMap = new double[gridMap.Width, gridMap.Height];
|
||
|
||
// 初始化：只标记边界
|
||
for (int x = 0; x < gridMap.Width; x++)
|
||
{
|
||
for (int y = 0; y < gridMap.Height; y++)
|
||
{
|
||
var cell = gridMap.Cells[x, y];
|
||
distanceMap[x, y] = double.MaxValue;
|
||
|
||
// 检查该层索引是否存在
|
||
if (cell.HeightLayers != null && layerIndex < cell.HeightLayers.Count)
|
||
{
|
||
var layer = cell.HeightLayers[layerIndex];
|
||
|
||
// 该层是边界 → distance=0
|
||
if (layer.IsBoundary)
|
||
{
|
||
distanceMap[x, y] = 0.0;
|
||
boundaryCount++;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// 该层索引不存在 → distance=-1，不参与距离变换
|
||
distanceMap[x, y] = -1.0;
|
||
}
|
||
}
|
||
}
|
||
|
||
if (boundaryCount == 0)
|
||
{
|
||
return (0, 0); // 无边界，跳过
|
||
}
|
||
|
||
// 距离变换
|
||
DistanceTransform(distanceMap, gridMap.Width, gridMap.Height);
|
||
|
||
// 应用膨胀：distance >= 0 && distance < inflationRadius（包括边界本身）
|
||
for (int x = 0; x < gridMap.Width; x++)
|
||
{
|
||
for (int y = 0; y < gridMap.Height; y++)
|
||
{
|
||
var cell = gridMap.Cells[x, y];
|
||
|
||
if (cell.HeightLayers != null && layerIndex < cell.HeightLayers.Count)
|
||
{
|
||
var layer = cell.HeightLayers[layerIndex];
|
||
|
||
// 门保护：不膨胀门类型的层
|
||
if (layer.Type == "门")
|
||
continue;
|
||
|
||
// 楼梯口保护：Layer[0]且存在Layer[1]且高度差小于阈值
|
||
if (layerIndex == 0 && cell.HeightLayers.Count > 1)
|
||
{
|
||
var layer0 = cell.HeightLayers[0];
|
||
var layer1 = cell.HeightLayers[1];
|
||
double heightDiff = Math.Abs(layer1.Z - layer0.Z);
|
||
|
||
if (heightDiff <= maxHeightDiffInModelUnits)
|
||
{
|
||
// 这是楼梯口：Layer[0]不膨胀，保持通行以便进入楼梯
|
||
continue;
|
||
}
|
||
}
|
||
|
||
// 边界膨胀条件：包括边界本身（distance=0）
|
||
// 例如：半径=2 → 膨胀distance=0,1（边界+向外1格）
|
||
if (distanceMap[x, y] >= 0.0 && distanceMap[x, y] < inflationRadius)
|
||
{
|
||
layer.IsWalkable = false;
|
||
cell.HeightLayers[layerIndex] = layer;
|
||
inflatedCount++;
|
||
}
|
||
}
|
||
|
||
gridMap.Cells[x, y] = cell;
|
||
}
|
||
}
|
||
|
||
return (boundaryCount, inflatedCount);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 收集物流项目的相关节点
|
||
/// 根据节点类型决定收集策略：几何体节点收集父节点和同级节点，集合节点收集子节点
|
||
/// </summary>
|
||
/// <param name="items">物流项目节点列表</param>
|
||
/// <returns>包含所有相关节点的集合</returns>
|
||
private HashSet<ModelItem> CollectRelatedItems(List<ModelItem> items)
|
||
{
|
||
var relatedItems = new HashSet<ModelItem>();
|
||
|
||
foreach (var item in items)
|
||
{
|
||
try
|
||
{
|
||
LogManager.Info($"[节点收集] 开始处理节点: '{item.DisplayName}'");
|
||
|
||
// 使用 ModelItemAnalysisHelper 的通用方法收集相关节点
|
||
var itemRelatedNodes = ModelItemAnalysisHelper.CollectRelatedNodes(item);
|
||
|
||
// 将结果添加到 HashSet 中
|
||
foreach (var node in itemRelatedNodes)
|
||
{
|
||
relatedItems.Add(node);
|
||
}
|
||
|
||
LogManager.Info($"[节点收集] 从 '{item.DisplayName}' 收集到 {itemRelatedNodes.Count} 个相关节点");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Warning($"[节点收集] 处理节点 '{item?.DisplayName ?? "NULL"}' 时出错: {ex.Message}");
|
||
// 出错时至少保证节点本身被添加
|
||
relatedItems.Add(item);
|
||
}
|
||
}
|
||
|
||
LogManager.Info($"[节点收集] 共收集到 {relatedItems.Count} 个相关节点");
|
||
return relatedItems;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 轻量级后处理：只对预筛选的几何体项目进行必要的属性提取
|
||
/// 大幅减少API调用次数
|
||
/// </summary>
|
||
private Dictionary<ModelItem, ItemProperties> PostProcessGeometryItems(
|
||
List<ModelItem> geometryItems,
|
||
HashSet<ModelItem> traversableItemsSet,
|
||
HashSet<ModelItem> irrelevantItemsSet)
|
||
{
|
||
LogManager.Info("[轻量级后处理] 开始处理Search API筛选结果");
|
||
var startTime = DateTime.Now;
|
||
var itemCache = new Dictionary<ModelItem, ItemProperties>();
|
||
|
||
var totalItems = geometryItems.Count;
|
||
var apiCalls = 0;
|
||
var skippedByChannel = 0;
|
||
var skippedByIrrelevant = 0;
|
||
var skippedByBoundingBox = 0;
|
||
|
||
foreach (var item in geometryItems)
|
||
{
|
||
try
|
||
{
|
||
var properties = new ItemProperties
|
||
{
|
||
// 已知条件：HasGeometry = true（Search API保证）
|
||
HasGeometry = true,
|
||
|
||
// 快速筛除1：通道相关项目（HashSet查找，极快）
|
||
IsChannelItem = traversableItemsSet.Contains(item)
|
||
};
|
||
if (properties.IsChannelItem)
|
||
{
|
||
skippedByChannel++;
|
||
continue; // 跳过通道项目，不进行昂贵的API调用
|
||
}
|
||
|
||
properties.IsChildOfChannel = IsChildOfChannelItems(item, traversableItemsSet);
|
||
if (properties.IsChildOfChannel)
|
||
{
|
||
skippedByChannel++;
|
||
continue; // 跳过通道子项目
|
||
}
|
||
|
||
// 快速筛除2：无关项（不参与网格生成的大型构件）
|
||
properties.IsIrrelevantItem = irrelevantItemsSet.Contains(item);
|
||
if (properties.IsIrrelevantItem)
|
||
{
|
||
skippedByIrrelevant++;
|
||
continue; // 跳过无关项，避免昂贵的BoundingBox API调用
|
||
}
|
||
|
||
// 设为已知值（避免后续筛选中的判断错误）
|
||
properties.IsContainer = false; // 基于测试验证，几何体项目都不是容器
|
||
|
||
// API调用1: BoundingBox() - 获取边界框（唯一保留的API调用）
|
||
properties.BoundingBox = item.BoundingBox();
|
||
apiCalls++;
|
||
|
||
if (properties.BoundingBox == null)
|
||
{
|
||
skippedByBoundingBox++;
|
||
continue; // 跳过无边界框项目
|
||
}
|
||
|
||
// 设置为默认值，实际的高度检查在后续阶段进行
|
||
properties.IsInScanHeightRange = true;
|
||
|
||
// 只有通过所有筛选的项目才加入结果
|
||
itemCache[item] = properties;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Debug($"[轻量级后处理] 处理模型项失败: {item?.DisplayName ?? "NULL"}, {ex.Message}");
|
||
}
|
||
}
|
||
|
||
var elapsed = (DateTime.Now - startTime).TotalMilliseconds;
|
||
LogManager.Info($"[轻量级后处理] 完成，输入 {totalItems} 项，有效结果 {itemCache.Count} 项，API调用 {apiCalls} 次，耗时: {elapsed:F1}ms");
|
||
LogManager.Info($"[轻量级后处理] 筛除统计 - 通道相关: {skippedByChannel}, 无关项: {skippedByIrrelevant}, 无边界框: {skippedByBoundingBox}");
|
||
|
||
return itemCache;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 使用包围盒遍历方式处理障碍物（高性能优化版）
|
||
/// 分离API访问和数值计算，利用并行处理加速纯计算部分
|
||
/// </summary>
|
||
/// <param name="gridMap">网格地图</param>
|
||
/// <param name="channelItems">通道模型项列表</param>
|
||
/// <param name="scanHeightInModelUnits">扫描高度范围（模型单位）</param>
|
||
/// <param name="channelCoverage">通道覆盖信息，包含通道边界</param>
|
||
/// <param name="irrelevantItemsSet">无关项集合（不参与网格生成的大型构件）</param>
|
||
private void ProcessObstaclesWithBoundingBox(GridMap gridMap, HashSet<ModelItem> channelItems, double scanHeightInModelUnits, ChannelCoverage channelCoverage, HashSet<ModelItem> irrelevantItemsSet)
|
||
{
|
||
try
|
||
{
|
||
LogManager.Info("[高性能障碍物处理] 开始处理障碍物 - Search API优化版本");
|
||
var startTime = DateTime.Now;
|
||
|
||
var traversableItemsSet = channelItems ?? new HashSet<ModelItem>();
|
||
|
||
// 阶段1：Search API预筛选（一次性批量获取几何体项目）
|
||
var geometryItems = ModelItemAnalysisHelper.GetAllVisibleGeometryItems();
|
||
|
||
LogManager.Info($"[高性能障碍物处理] 输入统计 - 几何体项目: {geometryItems.Count}, 通道元素: {traversableItemsSet.Count}");
|
||
|
||
// 阶段2：轻量级后处理（只对预筛选结果进行必要的API调用）
|
||
var itemCache = PostProcessGeometryItems(geometryItems, traversableItemsSet, irrelevantItemsSet);
|
||
|
||
// 阶段3：条件过滤（并行数值计算）- 使用50%CPU核心优化
|
||
LogManager.Info("[高性能障碍物处理] 阶段3: 并行条件过滤");
|
||
var filterStart = DateTime.Now;
|
||
|
||
// 🔥 性能优化：限制并行度为CPU核心数的50%，避免过度并行化
|
||
var optimalParallelism = Math.Max(1, Environment.ProcessorCount / 2);
|
||
LogManager.Info($"[并行优化] CPU核心: {Environment.ProcessorCount}, 使用并行度: {optimalParallelism}");
|
||
|
||
var validItems = itemCache.AsParallel()
|
||
.WithDegreeOfParallelism(optimalParallelism)
|
||
.Where(kvp => kvp.Value.HasGeometry &&
|
||
!kvp.Value.IsContainer &&
|
||
!kvp.Value.IsChannelItem &&
|
||
!kvp.Value.IsChildOfChannel &&
|
||
kvp.Value.BoundingBox != null &&
|
||
kvp.Value.IsInScanHeightRange)
|
||
.ToList();
|
||
|
||
var filterElapsed = (DateTime.Now - filterStart).TotalMilliseconds;
|
||
LogManager.Info($"[高性能障碍物处理] 阶段3完成: 从缓存中筛选出 {validItems.Count} 个有效项，耗时: {filterElapsed:F1}ms");
|
||
|
||
int horizontalOverlapCount = validItems.Count(kvp => IntersectsGridPlanarExtents(kvp.Value.BoundingBox, gridMap));
|
||
LogManager.Info($"[高性能障碍物处理] 阶段3.5: 与当前网格平面范围相交的有效几何体: {horizontalOverlapCount}/{validItems.Count}");
|
||
|
||
// 获取可通行网格的实际高度范围信息
|
||
var walkableMinZ = channelCoverage.WalkableMinZ;
|
||
var walkableMaxZ = channelCoverage.WalkableMaxZ;
|
||
var walkableGridCount = channelCoverage.WalkableGridCount;
|
||
var walkableHeightRange = walkableMaxZ - walkableMinZ;
|
||
LogManager.Info($"[高性能障碍物处理] 可通行网格高度信息 - 最低点: {walkableMinZ:F2}, 最高点: {walkableMaxZ:F2}");
|
||
LogManager.Info($"[高性能障碍物处理] 可通行网格数量: {walkableGridCount}, 高度范围: {walkableHeightRange:F2}, 加物体高度后扫描范围: [{walkableMinZ:F2}, {walkableMaxZ + scanHeightInModelUnits:F2}]");
|
||
|
||
// 阶段4：网格覆盖计算（并行几何计算）- 使用50%CPU核心优化 + 基于通道的精确高度检查
|
||
LogManager.Info("[高性能障碍物处理] 阶段4: 并行网格覆盖计算 + 基于通道高度的精确高度检查");
|
||
var gridCalcStart = DateTime.Now;
|
||
|
||
var heightCheckedItems = validItems.AsParallel()
|
||
.WithDegreeOfParallelism(optimalParallelism)
|
||
.Where(kvp =>
|
||
{
|
||
// 精确高度检查：使用几何体中心点对应的网格高度
|
||
var bbox = kvp.Value.BoundingBox;
|
||
var centerPoint = new Point3D(
|
||
(bbox.Min.X + bbox.Max.X) / 2,
|
||
(bbox.Min.Y + bbox.Max.Y) / 2,
|
||
(bbox.Min.Z + bbox.Max.Z) / 2);
|
||
|
||
var centerGridPos = gridMap.WorldToGrid(centerPoint);
|
||
if (!gridMap.IsValidGridPosition(centerGridPos))
|
||
{
|
||
return false;
|
||
}
|
||
|
||
var centerCell = gridMap.Cells[centerGridPos.X, centerGridPos.Y];
|
||
|
||
// 使用可通行网格高度范围计算扫描范围
|
||
// 扫描范围：从可通行网格最低点开始，到可通行网格最高点+物体高度+安全间隙
|
||
var scanMin = walkableMinZ;
|
||
var scanMax = walkableMaxZ + scanHeightInModelUnits;
|
||
|
||
// 只有与该网格高度范围重叠的几何体才进入处理
|
||
var (obstacleMinElevation, obstacleMaxElevation) =
|
||
GetObstacleElevationRange(bbox.Min, bbox.Max, gridMap);
|
||
bool isInRange = !(obstacleMaxElevation <= scanMin || obstacleMinElevation > scanMax);
|
||
|
||
return isInRange;
|
||
})
|
||
.ToList();
|
||
|
||
var gridUpdates = heightCheckedItems.AsParallel()
|
||
.WithDegreeOfParallelism(optimalParallelism)
|
||
.SelectMany(kvp =>
|
||
{
|
||
var coveredCells = CalculateBoundingBoxGridCoverage(kvp.Value.BoundingBox, gridMap);
|
||
return coveredCells.Select(cell => new GridUpdate
|
||
{
|
||
Item = kvp.Key,
|
||
X = cell.x,
|
||
Y = cell.y,
|
||
BoundingBox = kvp.Value.BoundingBox
|
||
});
|
||
})
|
||
.ToList();
|
||
|
||
var gridCalcElapsed = (DateTime.Now - gridCalcStart).TotalMilliseconds;
|
||
var totalCheckedItems = validItems.Count;
|
||
var filteredItems = totalCheckedItems - heightCheckedItems.Count;
|
||
int overlappingItemsRetained = heightCheckedItems.Count(kvp => IntersectsGridPlanarExtents(kvp.Value.BoundingBox, gridMap));
|
||
int overlappingItemsFiltered = validItems.Count(kvp => IntersectsGridPlanarExtents(kvp.Value.BoundingBox, gridMap)) - overlappingItemsRetained;
|
||
|
||
LogManager.Info($"[高性能障碍物处理] 阶段4完成: 生成 {gridUpdates.Count} 个网格更新操作，耗时: {gridCalcElapsed:F1}ms");
|
||
LogManager.Info($"[精确高度检查] 检查了 {totalCheckedItems} 个几何体，过滤掉 {filteredItems} 个高度范围外的几何体，保留 {heightCheckedItems.Count} 个");
|
||
LogManager.Info($"[精确高度检查] 其中与当前网格平面范围相交的有效几何体: 过滤 {overlappingItemsFiltered} 个, 保留 {overlappingItemsRetained} 个");
|
||
|
||
// 阶段5：网格状态更新（单线程写入，3D高度检查）
|
||
LogManager.Info("[高性能障碍物处理] 阶段5: 单线程网格状态更新（3D高度检查）");
|
||
var updateStart = DateTime.Now;
|
||
|
||
var updatedCells = 0;
|
||
var obstacleItems = 0;
|
||
var layersBlocked = 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 == "通道" && cell.IsInChannel)
|
||
{
|
||
var bbox = update.BoundingBox;
|
||
|
||
// 统计有多少网格有高度层
|
||
if (cell.HeightLayers != null && cell.HeightLayers.Count > 0)
|
||
{
|
||
cellsWithLayers++;
|
||
totalLayersChecked += cell.HeightLayers.Count;
|
||
}
|
||
|
||
// 3D高度检查：标记与障碍物重叠的层为不可通行（保留层结构）
|
||
bool anyLayerModified = false;
|
||
|
||
for (int i = 0; i < cell.HeightLayers.Count; i++)
|
||
{
|
||
var layer = cell.HeightLayers[i];
|
||
|
||
// 计算该层的高度范围
|
||
double layerMinZ = layer.Z;
|
||
double layerMaxZ = layer.Z + layer.PassableHeight.GetSpan();
|
||
|
||
// 障碍物的高度范围
|
||
var (obstacleMinZ, obstacleMaxZ) = GetObstacleElevationRange(bbox.Min, bbox.Max, gridMap);
|
||
|
||
// 检查是否重叠
|
||
bool overlaps = !(obstacleMaxZ <= layerMinZ || obstacleMinZ > layerMaxZ);
|
||
|
||
if (overlaps && layer.IsWalkable)
|
||
{
|
||
// 重叠：标记为不可通行，但保留层结构
|
||
layer.IsWalkable = false;
|
||
cell.HeightLayers[i] = layer;
|
||
layersBlocked++;
|
||
anyLayerModified = true;
|
||
}
|
||
}
|
||
|
||
// 如果所有层都被标记为不可通行，标记整个网格为障碍物
|
||
if (anyLayerModified && !cell.HeightLayers.Any(l => l.IsWalkable))
|
||
{
|
||
cell.CellType = "障碍物";
|
||
cell.Cost = double.MaxValue;
|
||
cell.IsInChannel = false;
|
||
cell.RelatedModelItem = update.Item;
|
||
cellsFullyBlocked++;
|
||
}
|
||
|
||
gridMap.Cells[update.X, update.Y] = cell;
|
||
updatedCells++;
|
||
}
|
||
}
|
||
|
||
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}, 阻挡了 {layersBlocked} 个高度层");
|
||
LogManager.Info($"[3D高度检查] {cellsFullyBlocked} 个网格被完全阻挡，{updatedCells - cellsFullyBlocked} 个网格保留了部分层");
|
||
|
||
// 输出总体统计
|
||
var totalElapsed = (DateTime.Now - startTime).TotalMilliseconds;
|
||
LogManager.Info($"[高性能障碍物处理] 处理完成，总耗时: {totalElapsed:F1}ms");
|
||
|
||
var searchTime = (DateTime.Now - startTime).TotalMilliseconds - filterElapsed - gridCalcElapsed - updateElapsed;
|
||
LogManager.Info($"[高性能障碍物处理] 性能分解 - Search API预筛选+后处理: {searchTime:F1}ms, 过滤: {filterElapsed:F1}ms, 计算: {gridCalcElapsed:F1}ms, 更新: {updateElapsed:F1}ms");
|
||
|
||
// 性能对比统计
|
||
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)
|
||
{
|
||
LogManager.Error($"[高性能障碍物处理] 处理失败: {ex.Message}");
|
||
throw;
|
||
}
|
||
}
|
||
|
||
|
||
/// <summary>
|
||
/// 获取世界坐标范围在当前网格坐标语义下的高程区间。
|
||
/// </summary>
|
||
private (double min, double max) GetObstacleElevationRange(Point3D minPoint, Point3D maxPoint, GridMap gridMap)
|
||
{
|
||
return GetObstacleElevationRange(
|
||
minPoint.X, minPoint.Y, minPoint.Z,
|
||
maxPoint.X, maxPoint.Y, maxPoint.Z,
|
||
gridMap.CoordinateSystemType);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 根据世界坐标最小/最大点，计算其在当前网格坐标语义下覆盖的平面网格范围。
|
||
/// </summary>
|
||
private List<(int x, int y)> CalculateGridCoverageFromWorldExtents(Point3D minPoint, Point3D maxPoint, GridMap gridMap)
|
||
{
|
||
return CalculateGridCoverageFromWorldExtents(
|
||
minPoint.X, minPoint.Y, minPoint.Z,
|
||
maxPoint.X, maxPoint.Y, maxPoint.Z,
|
||
gridMap.Origin.X, gridMap.Origin.Y, gridMap.Origin.Z,
|
||
gridMap.Width, gridMap.Height, gridMap.CellSize,
|
||
gridMap.CoordinateSystemType);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取世界坐标范围在当前宿主坐标语义下的高程区间。
|
||
/// 仅使用纯数值输入，便于单元测试验证多坐标系语义。
|
||
/// </summary>
|
||
private (double min, double max) GetObstacleElevationRange(
|
||
double minX, double minY, double minZ,
|
||
double maxX, double maxY, double maxZ,
|
||
CoordinateSystemType coordinateSystemType)
|
||
{
|
||
var adapter = new HostCoordinateAdapter(coordinateSystemType);
|
||
double elevation1 = HostPlanarGridHelper.GetElevation3(new Vector3((float)minX, (float)minY, (float)minZ), adapter);
|
||
double elevation2 = HostPlanarGridHelper.GetElevation3(new Vector3((float)maxX, (float)maxY, (float)maxZ), adapter);
|
||
return (Math.Min(elevation1, elevation2), Math.Max(elevation1, elevation2));
|
||
}
|
||
|
||
/// <summary>
|
||
/// 解析边界最小点在当前宿主坐标语义下的高程值。
|
||
/// 仅修正“高程轴”的解释，不改变原有边界层业务逻辑。
|
||
/// </summary>
|
||
private static double ResolveBoundsMinElevation(
|
||
double minX, double minY, double minZ,
|
||
CoordinateSystemType coordinateSystemType)
|
||
{
|
||
var adapter = new HostCoordinateAdapter(coordinateSystemType);
|
||
return HostPlanarGridHelper.GetElevation3(new Vector3((float)minX, (float)minY, (float)minZ), adapter);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 根据世界坐标最小/最大点与网格元数据，计算覆盖的平面网格范围。
|
||
/// 仅使用纯数值输入，便于单元测试验证多坐标系语义。
|
||
/// </summary>
|
||
private List<(int x, int y)> CalculateGridCoverageFromWorldExtents(
|
||
double minX, double minY, double minZ,
|
||
double maxX, double maxY, double maxZ,
|
||
double originX, double originY, double originZ,
|
||
int width, int height, double cellSize,
|
||
CoordinateSystemType coordinateSystemType)
|
||
{
|
||
var adapter = new HostCoordinateAdapter(coordinateSystemType);
|
||
var minPoint = new Vector3((float)minX, (float)minY, (float)minZ);
|
||
var maxPoint = new Vector3((float)maxX, (float)maxY, (float)maxZ);
|
||
var originPoint = new Vector3((float)originX, (float)originY, (float)originZ);
|
||
|
||
var (minH1, minH2) = HostPlanarGridHelper.GetHorizontalCoords3(minPoint, adapter);
|
||
var (maxH1, maxH2) = HostPlanarGridHelper.GetHorizontalCoords3(maxPoint, adapter);
|
||
var (originH1, originH2) = HostPlanarGridHelper.GetHorizontalCoords3(originPoint, adapter);
|
||
|
||
int minGridX = (int)Math.Round((minH1 - originH1) / cellSize);
|
||
int minGridY = (int)Math.Round((minH2 - originH2) / cellSize);
|
||
int maxGridX = (int)Math.Round((maxH1 - originH1) / cellSize);
|
||
int maxGridY = (int)Math.Round((maxH2 - originH2) / cellSize);
|
||
|
||
int clampedMinX = Math.Max(0, Math.Min(minGridX, maxGridX));
|
||
int clampedMinY = Math.Max(0, Math.Min(minGridY, maxGridY));
|
||
int clampedMaxX = Math.Min(width - 1, Math.Max(minGridX, maxGridX));
|
||
int clampedMaxY = Math.Min(height - 1, Math.Max(minGridY, maxGridY));
|
||
|
||
if (clampedMaxX < clampedMinX) clampedMaxX = clampedMinX;
|
||
if (clampedMaxY < clampedMinY) clampedMaxY = clampedMinY;
|
||
|
||
var coveredCells = new List<(int x, int y)>();
|
||
for (int x = clampedMinX; x <= clampedMaxX; x++)
|
||
{
|
||
for (int y = clampedMinY; y <= clampedMaxY; y++)
|
||
{
|
||
coveredCells.Add((x, y));
|
||
}
|
||
}
|
||
|
||
return coveredCells;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 判断包围盒在当前宿主平面语义下，是否与当前网格的平面范围真实相交。
|
||
/// 这里只做诊断，不改变原有网格覆盖或裁剪逻辑。
|
||
/// </summary>
|
||
private bool IntersectsGridPlanarExtents(BoundingBox3D bbox, GridMap gridMap)
|
||
{
|
||
if (bbox == null || gridMap == null || gridMap.Bounds == null)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
var adapter = new HostCoordinateAdapter(gridMap.CoordinateSystemType);
|
||
var hostMin = new Vector3((float)bbox.Min.X, (float)bbox.Min.Y, (float)bbox.Min.Z);
|
||
var hostMax = new Vector3((float)bbox.Max.X, (float)bbox.Max.Y, (float)bbox.Max.Z);
|
||
var gridHostMin = new Vector3((float)gridMap.Bounds.Min.X, (float)gridMap.Bounds.Min.Y, (float)gridMap.Bounds.Min.Z);
|
||
var gridHostMax = new Vector3((float)gridMap.Bounds.Max.X, (float)gridMap.Bounds.Max.Y, (float)gridMap.Bounds.Max.Z);
|
||
|
||
var (bboxMinH1, bboxMaxH1, bboxMinH2, bboxMaxH2) = HostPlanarGridHelper.GetHorizontalRange3(hostMin, hostMax, adapter);
|
||
var (gridMinH1, gridMaxH1, gridMinH2, gridMaxH2) = HostPlanarGridHelper.GetHorizontalRange3(gridHostMin, gridHostMax, adapter);
|
||
|
||
bool overlapH1 = !(bboxMaxH1 < gridMinH1 || bboxMinH1 > gridMaxH1);
|
||
bool overlapH2 = !(bboxMaxH2 < gridMinH2 || bboxMinH2 > gridMaxH2);
|
||
return overlapH1 && overlapH2;
|
||
}
|
||
|
||
private static (double minH1, double maxH1, double minH2, double maxH2) GetPlanarRange(
|
||
Point3D minPoint,
|
||
Point3D maxPoint,
|
||
CoordinateSystemType coordinateSystemType)
|
||
{
|
||
var adapter = new HostCoordinateAdapter(coordinateSystemType);
|
||
var hostMin = new Vector3((float)minPoint.X, (float)minPoint.Y, (float)minPoint.Z);
|
||
var hostMax = new Vector3((float)maxPoint.X, (float)maxPoint.Y, (float)maxPoint.Z);
|
||
return HostPlanarGridHelper.GetHorizontalRange3(hostMin, hostMax, adapter);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算包围盒在网格上覆盖的单元坐标
|
||
/// 严格按照"网格坐标代表左下角"的设计原则进行边界处理
|
||
/// </summary>
|
||
/// <param name="bbox">包围盒</param>
|
||
/// <param name="gridMap">网格地图</param>
|
||
/// <returns>覆盖的网格单元坐标列表</returns>
|
||
private List<(int x, int y)> CalculateBoundingBoxGridCoverage(BoundingBox3D bbox, GridMap gridMap)
|
||
{
|
||
try
|
||
{
|
||
return CalculateGridCoverageFromWorldExtents(bbox.Min, bbox.Max, gridMap);
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Debug($"[包围盒覆盖计算] 计算失败: {ex.Message}");
|
||
return new List<(int x, int y)>();
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 根据门包围盒、限宽和当前宿主平面语义，计算门开口在网格上的覆盖范围。
|
||
/// 关键约束：
|
||
/// - 仅替换“哪两个轴组成水平平面”的解释，不改变历史限宽业务规则。
|
||
/// - 门宽仍取平面上较大的那一维；限宽仍只裁剪门宽方向。
|
||
/// </summary>
|
||
private List<(int x, int y)> CalculateDoorOpeningCoverageFromWorldExtents(
|
||
double minX, double minY, double minZ,
|
||
double maxX, double maxY, double maxZ,
|
||
double limitWidthInModelUnits,
|
||
double originX, double originY, double originZ,
|
||
int width, int height, double cellSize,
|
||
CoordinateSystemType coordinateSystemType)
|
||
{
|
||
var adapter = new HostCoordinateAdapter(coordinateSystemType);
|
||
var hostMin = new Vector3((float)minX, (float)minY, (float)minZ);
|
||
var hostMax = new Vector3((float)maxX, (float)maxY, (float)maxZ);
|
||
var hostOrigin = new Vector3((float)originX, (float)originY, (float)originZ);
|
||
var (minH1, maxH1, minH2, maxH2) = HostPlanarGridHelper.GetHorizontalRange3(hostMin, hostMax, adapter);
|
||
var (originH1, originH2) = HostPlanarGridHelper.GetHorizontalCoords3(hostOrigin, adapter);
|
||
|
||
double planarWidth = maxH1 - minH1;
|
||
double planarDepth = maxH2 - minH2;
|
||
bool widthAlongH1 = planarWidth > planarDepth;
|
||
double doorWidth = Math.Max(planarWidth, planarDepth);
|
||
double effectiveWidth = Math.Min(limitWidthInModelUnits, doorWidth);
|
||
|
||
double openingMinH1;
|
||
double openingMaxH1;
|
||
double openingMinH2;
|
||
double openingMaxH2;
|
||
|
||
if (widthAlongH1)
|
||
{
|
||
double centerH1 = (minH1 + maxH1) / 2.0;
|
||
double halfWidth = effectiveWidth / 2.0;
|
||
openingMinH1 = centerH1 - halfWidth;
|
||
openingMaxH1 = centerH1 + halfWidth;
|
||
openingMinH2 = minH2;
|
||
openingMaxH2 = maxH2;
|
||
}
|
||
else
|
||
{
|
||
double centerH2 = (minH2 + maxH2) / 2.0;
|
||
double halfWidth = effectiveWidth / 2.0;
|
||
openingMinH1 = minH1;
|
||
openingMaxH1 = maxH1;
|
||
openingMinH2 = centerH2 - halfWidth;
|
||
openingMaxH2 = centerH2 + halfWidth;
|
||
}
|
||
|
||
int minGridX = (int)Math.Round((openingMinH1 - originH1) / cellSize);
|
||
int minGridY = (int)Math.Round((openingMinH2 - originH2) / cellSize);
|
||
int maxGridX = (int)Math.Round((openingMaxH1 - originH1) / cellSize);
|
||
int maxGridY = (int)Math.Round((openingMaxH2 - originH2) / cellSize);
|
||
|
||
int clampedMinX = Math.Max(0, Math.Min(minGridX, maxGridX));
|
||
int clampedMinY = Math.Max(0, Math.Min(minGridY, maxGridY));
|
||
int clampedMaxX = Math.Min(width - 1, Math.Max(minGridX, maxGridX));
|
||
int clampedMaxY = Math.Min(height - 1, Math.Max(minGridY, maxGridY));
|
||
|
||
if (clampedMaxX < clampedMinX) clampedMaxX = clampedMinX;
|
||
if (clampedMaxY < clampedMinY) clampedMaxY = clampedMinY;
|
||
|
||
var coveredCells = new List<(int x, int y)>();
|
||
for (int x = clampedMinX; x <= clampedMaxX; x++)
|
||
{
|
||
for (int y = clampedMinY; y <= clampedMaxY; y++)
|
||
{
|
||
coveredCells.Add((x, y));
|
||
}
|
||
}
|
||
|
||
return coveredCells;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算门开口区域在网格上覆盖的单元坐标（考虑限宽）
|
||
/// </summary>
|
||
/// <param name="doorItem">门模型项</param>
|
||
/// <param name="bbox">门的包围盒</param>
|
||
/// <param name="gridMap">网格地图</param>
|
||
/// <returns>开口区域覆盖的网格单元坐标列表</returns>
|
||
private List<(int x, int y)> CalculateDoorOpeningCoverage(ModelItem doorItem, BoundingBox3D bbox, GridMap gridMap)
|
||
{
|
||
var coveredCells = new List<(int x, int y)>();
|
||
|
||
try
|
||
{
|
||
// 1. 获取门的限宽属性
|
||
string widthLimitStr = CategoryAttributeManager.GetLogisticsPropertyValue(doorItem, CategoryAttributeManager.LogisticsProperties.WIDTH_LIMIT);
|
||
|
||
if (string.IsNullOrEmpty(widthLimitStr))
|
||
{
|
||
LogManager.Warning($"【门开口计算】门元素 {doorItem.DisplayName} 没有设置限宽属性，跳过处理");
|
||
return coveredCells;
|
||
}
|
||
|
||
// 2. 解析限宽属性值（支持带单位的格式）
|
||
double limitWidthMeters = CategoryAttributeManager.ParseLogisticsLimitValue(widthLimitStr, "限宽");
|
||
|
||
if (limitWidthMeters <= 0)
|
||
{
|
||
LogManager.Warning($"【门开口计算】门元素 {doorItem.DisplayName} 限宽属性解析失败: {widthLimitStr}");
|
||
return coveredCells;
|
||
}
|
||
|
||
// 3. 转换限宽到模型单位
|
||
double metersToModelUnits = UnitsConverter.GetMetersToUnitsConversionFactor(Application.ActiveDocument.Units);
|
||
double limitWidthInModelUnits = limitWidthMeters * metersToModelUnits;
|
||
|
||
var adapter = new HostCoordinateAdapter(gridMap.CoordinateSystemType);
|
||
var hostMin = new Vector3((float)bbox.Min.X, (float)bbox.Min.Y, (float)bbox.Min.Z);
|
||
var hostMax = new Vector3((float)bbox.Max.X, (float)bbox.Max.Y, (float)bbox.Max.Z);
|
||
var (minH1, maxH1, minH2, maxH2) = HostPlanarGridHelper.GetHorizontalRange3(hostMin, hostMax, adapter);
|
||
double planarWidth = maxH1 - minH1;
|
||
double planarDepth = maxH2 - minH2;
|
||
double doorWidth = Math.Max(planarWidth, planarDepth);
|
||
|
||
coveredCells = CalculateDoorOpeningCoverageFromWorldExtents(
|
||
bbox.Min.X, bbox.Min.Y, bbox.Min.Z,
|
||
bbox.Max.X, bbox.Max.Y, bbox.Max.Z,
|
||
limitWidthInModelUnits,
|
||
gridMap.Origin.X, gridMap.Origin.Y, gridMap.Origin.Z,
|
||
gridMap.Width, gridMap.Height, gridMap.CellSize,
|
||
gridMap.CoordinateSystemType);
|
||
|
||
if (limitWidthInModelUnits > doorWidth)
|
||
{
|
||
LogManager.Info($"【门开口计算】{doorItem.DisplayName} 限宽({limitWidthMeters}m)超过门宽({doorWidth / metersToModelUnits:F2}m)，按门宽处理");
|
||
}
|
||
|
||
if (coveredCells.Any())
|
||
{
|
||
int gridMinX = coveredCells.Min(cell => cell.x);
|
||
int gridMinY = coveredCells.Min(cell => cell.y);
|
||
int gridMaxX = coveredCells.Max(cell => cell.x);
|
||
int gridMaxY = coveredCells.Max(cell => cell.y);
|
||
LogManager.Info($"【门开口计算】{doorItem.DisplayName} 成功处理 - 门宽: {doorWidth / metersToModelUnits:F2}m, 限宽: {limitWidthMeters}m, 开口覆盖: {coveredCells.Count}个网格 (范围: [{gridMinX},{gridMinY}]-[{gridMaxX},{gridMaxY}])");
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"【门开口计算】计算失败: {doorItem.DisplayName}, 错误: {ex.Message}");
|
||
// 出错时返回空列表，不影响其他门的处理
|
||
}
|
||
|
||
return coveredCells;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 检查模型项是否是通道项的子项
|
||
/// </summary>
|
||
/// <param name="item">要检查的模型项</param>
|
||
/// <param name="traversableItemsSet">通道项集合</param>
|
||
/// <returns>是否是通道项的子项</returns>
|
||
private bool IsChildOfChannelItems(ModelItem item, HashSet<ModelItem> traversableItemsSet)
|
||
{
|
||
try
|
||
{
|
||
var parent = item.Parent;
|
||
while (parent != null)
|
||
{
|
||
if (traversableItemsSet.Contains(parent))
|
||
{
|
||
return true;
|
||
}
|
||
parent = parent.Parent;
|
||
}
|
||
return false;
|
||
}
|
||
catch
|
||
{
|
||
return false;
|
||
}
|
||
}
|
||
}
|
||
|
||
#region 性能优化数据结构
|
||
|
||
/// <summary>
|
||
/// 模型项缓存属性，存储昂贵API调用的结果
|
||
/// </summary>
|
||
internal class ItemProperties
|
||
{
|
||
public bool HasGeometry { get; set; }
|
||
public bool IsContainer { get; set; }
|
||
public BoundingBox3D BoundingBox { get; set; }
|
||
public bool IsChannelItem { get; set; }
|
||
public bool IsChildOfChannel { get; set; }
|
||
public bool IsIrrelevantItem { get; set; }
|
||
public bool IsInScanHeightRange { get; set; }
|
||
}
|
||
|
||
/// <summary>
|
||
/// 网格更新操作，用于并行计算后的批量更新
|
||
/// </summary>
|
||
internal class GridUpdate
|
||
{
|
||
public ModelItem Item { get; set; }
|
||
public int X { get; set; }
|
||
public int Y { get; set; }
|
||
public BoundingBox3D BoundingBox { get; set; }
|
||
}
|
||
|
||
#endregion
|
||
}
|