diff --git a/src/Core/PathPlanningManager.cs b/src/Core/PathPlanningManager.cs
index 9f42b3e..0140374 100644
--- a/src/Core/PathPlanningManager.cs
+++ b/src/Core/PathPlanningManager.cs
@@ -3453,20 +3453,21 @@ namespace NavisworksTransport
double layerZ = layer.Z;
double passableHeight = layer.PassableHeight.GetSpan();
- // 判断高度是否足够车辆通行
- bool isHeightSufficient = passableHeight >= vehicleHeightInModelUnits;
+ // 判断该层是否可通行（考虑膨胀影响和高度限制）
+ bool isLayerWalkable = layer.IsWalkable; // 膨胀影响
+ bool isHeightSufficient = passableHeight >= vehicleHeightInModelUnits; // 高度限制
- // 调试：输出高度不足的层
- if (!isHeightSufficient && multiLayerCells < 5) // 只输出前5个网格的详情
+ // 调试：输出不可通行的层
+ if ((!isLayerWalkable || !isHeightSufficient) && multiLayerCells < 5) // 只输出前5个网格的详情
{
- LogManager.Debug($"[高度判断] 网格({x},{y}) Layer Z={layerZ:F2}, PassableH={passableHeight:F2}, 车辆H={vehicleHeightInModelUnits:F2}, 高度足够={isHeightSufficient}");
+ LogManager.Debug($"[层通行性] 网格({x},{y}) Layer Z={layerZ:F2}, IsWalkable={isLayerWalkable}, PassableH={passableHeight:F2}, 车辆H={vehicleHeightInModelUnits:F2}, 高度足够={isHeightSufficient}");
}
PathRoute targetRoute = null;
string gridTypeName = "";
- // 根据高度是否足够和网格类型决定渲染方式
- if (isHeightSufficient)
+ // 根据层可通行性、高度是否足够和网格类型决定渲染方式
+ if (isLayerWalkable && isHeightSufficient)
{
// 高度足够 - 按原类型渲染
if (cell.CellType == CategoryAttributeManager.LogisticsElementType.通道)
@@ -3499,11 +3500,25 @@ namespace NavisworksTransport
}
else
{
- // 高度不足 - 渲染为障碍物（灰色）
+ // 层不可通行（被膨胀影响或高度不足）- 渲染为障碍物（灰色）
if (_showObstacleGrid)
{
targetRoute = obstacleRoute;
- gridTypeName = "障碍_高度不足"; // 必须包含"障碍"关键词才能渲染为灰色
+
+ // 区分不可通行的原因
+ if (!isLayerWalkable && !isHeightSufficient)
+ {
+ gridTypeName = "障碍_膨胀+高度不足";
+ }
+ else if (!isLayerWalkable)
+ {
+ gridTypeName = "障碍_膨胀影响";
+ }
+ else
+ {
+ gridTypeName = "障碍_高度不足";
+ }
+
heightInsufficientLayers++;
}
}
diff --git a/src/PathPlanning/AutoPathFinder.cs b/src/PathPlanning/AutoPathFinder.cs
index 2a70a1d..50dcfe8 100644
--- a/src/PathPlanning/AutoPathFinder.cs
+++ b/src/PathPlanning/AutoPathFinder.cs
@@ -168,6 +168,10 @@ namespace NavisworksTransport.PathPlanning
{
var layer = cell.HeightLayers[li];
+ // 检查该层是否可通行（膨胀影响）
+ if (!layer.IsWalkable)
+ continue;
+
// 楼梯/电梯区域跳过层0（下方楼板，不是可行走表面）
if (li == 0 && (layer.Type == CategoryAttributeManager.LogisticsElementType.楼梯 ||
layer.Type == CategoryAttributeManager.LogisticsElementType.电梯))
diff --git a/src/PathPlanning/GridMap.cs b/src/PathPlanning/GridMap.cs
index 6c8cc23..eaab45a 100644
--- a/src/PathPlanning/GridMap.cs
+++ b/src/PathPlanning/GridMap.cs
@@ -851,6 +851,19 @@ namespace NavisworksTransport.PathPlanning
///
public CategoryAttributeManager.LogisticsElementType Type { get; set; }
+ ///
+ /// 是否是边界层（用于膨胀计算）
+ /// 边界定义：当前网格层数 > 邻居网格层数，且层间高度差超过阈值
+ ///
+ public bool IsBoundary { get; set; }
+
+ ///
+ /// 该层是否可通行（用于多层膨胀）
+ /// true: 该层可以通行
+ /// false: 该层被膨胀影响，不可通行
+ ///
+ public bool IsWalkable { get; set; }
+
///
/// 构造函数
///
@@ -861,6 +874,8 @@ namespace NavisworksTransport.PathPlanning
SourceItem = sourceItem;
SpeedLimit = speedLimit;
Type = type;
+ IsBoundary = false; // 默认不是边界
+ IsWalkable = true; // 默认可通行，通过膨胀算法修改
}
}
diff --git a/src/PathPlanning/GridMapGenerator.cs b/src/PathPlanning/GridMapGenerator.cs
index ab0c8f3..fe951e5 100644
--- a/src/PathPlanning/GridMapGenerator.cs
+++ b/src/PathPlanning/GridMapGenerator.cs
@@ -29,6 +29,13 @@ namespace NavisworksTransport.PathPlanning
///
public class GridMapGenerator
{
+ ///
+ /// 边界检测的最大高度差（米）
+ /// 与AutoPathFinder中的MAX_HEIGHT_DIFF_METERS保持一致
+ /// 用于判断层间是否构成边界（超过此阈值则标记为边界）
+ ///
+ private const double MAX_HEIGHT_DIFF_METERS = 0.35;
+
private readonly CategoryAttributeManager _categoryManager;
private readonly ChannelBasedGridBuilder _channelBuilder;
@@ -145,6 +152,13 @@ namespace NavisworksTransport.PathPlanning
ProcessDoorElements(channelCoverage.GridMap, allTraversableItems);
LogManager.Info($"【阶段2.6完成】门元素处理后网格统计: {channelCoverage.GridMap.GetStatistics()}");
+ // 2.7. 标记边界层（用于膨胀计算）
+ LogManager.Info("【生成网格地图】步骤2.7: 标记边界层");
+ double maxHeightDiffInModelUnits = MAX_HEIGHT_DIFF_METERS * metersToModelUnitsConversionFactor;
+ LogManager.Info($"【生成网格地图】边界高度差阈值: {MAX_HEIGHT_DIFF_METERS}米 = {maxHeightDiffInModelUnits:F2}模型单位");
+ MarkBoundaryLayers(channelCoverage.GridMap, maxHeightDiffInModelUnits);
+ LogManager.Info($"【阶段2.7完成】边界层标记后网格统计: {channelCoverage.GridMap.GetStatistics()}");
+
// 3. 应用车辆尺寸膨胀（如果需要）
if (vehicleRadius > 0 || safetyMargin > 0)
{
@@ -410,6 +424,111 @@ namespace NavisworksTransport.PathPlanning
LogManager.Info($"【高度约束设置】其中楼梯/电梯下方层: {stairBottomLayersProcessed} 个（使用实际可通行高度）");
}
+ ///
+ /// 标记边界层（用于膨胀计算）
+ /// 边界定义：当前网格层数 > 邻居网格层数（梯度≥1），且层间高度差超过阈值
+ ///
+ /// 网格地图
+ /// 最大允许高度差（模型单位）
+ 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.IsWalkable || cell.HeightLayers == null || cell.HeightLayers.Count == 0)
+ 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];
+
+ // 边界检查
+ if (nx < 0 || nx >= gridMap.Width || ny < 0 || ny >= gridMap.Height)
+ continue;
+
+ var neighbor = gridMap.Cells[nx, ny];
+ int neighborLayerCount = (neighbor.HeightLayers != null) ? neighbor.HeightLayers.Count : 0;
+
+ // 层数梯度检测：当前层数 > 邻居层数
+ 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.Min.Z;
+ }
+
+ // 计算层间高度差
+ 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} 个边界层");
+ }
+
///
/// 从物流元素集合中过滤出门类型的元素
///
@@ -699,13 +818,222 @@ namespace NavisworksTransport.PathPlanning
}
///
- /// 优化的车辆膨胀算法 - 使用正确的8连通距离变换方法
- /// 时间复杂度: O(n*m) 而不是 O(n*m*r^2)
+ /// 多层膨胀算法 - 按高度层分别处理膨胀
+ /// 核心原则：边界层只影响同层索引的邻居层
///
private void ApplyVehicleInflationOptimized(GridMap gridMap, int inflationRadius)
{
var stopwatch = System.Diagnostics.Stopwatch.StartNew();
+ LogManager.Info($"【多层膨胀】开始按层处理膨胀，网格大小: {gridMap.Width}x{gridMap.Height}, 膨胀半径: {inflationRadius}");
+
+ // 统计每个层索引的处理情况
+ var layerStats = new Dictionary();
+ 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++)
+ {
+ int boundaryCount = 0;
+ int inflatedCount = 0;
+
+ // 为这一层创建距离图
+ var distanceMap = new double[gridMap.Width, gridMap.Height];
+ const double SQRT2 = 1.414213562373095;
+
+ // 初始化：标记障碍物、Unknown和边界层
+ 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;
+
+ // 1. 整个网格是障碍物 → 距离为0
+ if (!cell.IsWalkable)
+ {
+ distanceMap[x, y] = 0.0;
+ continue;
+ }
+
+ // 2. Unknown网格 → 距离为0
+ if (cell.CellType == CategoryAttributeManager.LogisticsElementType.Unknown)
+ {
+ distanceMap[x, y] = 0.0;
+ continue;
+ }
+
+ // 3. 该层索引存在且是边界 → 距离为0
+ if (cell.HeightLayers != null && layerIndex < cell.HeightLayers.Count)
+ {
+ var layer = cell.HeightLayers[layerIndex];
+ if (layer.IsBoundary)
+ {
+ distanceMap[x, y] = 0.0;
+ boundaryCount++;
+ }
+ }
+ }
+ }
+
+ if (boundaryCount == 0)
+ {
+ LogManager.Debug($"【多层膨胀】Layer{layerIndex} 无边界，跳过");
+ continue;
+ }
+
+ LogManager.Info($"【多层膨胀】Layer{layerIndex} 开始膨胀，边界数量: {boundaryCount}");
+
+ // 距离变换 - 正向扫描
+ for (int x = 0; x < gridMap.Width; x++)
+ {
+ for (int y = 0; y < gridMap.Height; y++)
+ {
+ if (distanceMap[x, y] == double.MaxValue)
+ {
+ double minDist = double.MaxValue;
+
+ if (x > 0)
+ minDist = Math.Min(minDist, distanceMap[x - 1, y] + 1.0);
+ if (y > 0)
+ minDist = Math.Min(minDist, distanceMap[x, y - 1] + 1.0);
+ if (x > 0 && y > 0)
+ minDist = Math.Min(minDist, distanceMap[x - 1, y - 1] + SQRT2);
+ if (x > 0 && y < gridMap.Height - 1)
+ minDist = Math.Min(minDist, distanceMap[x - 1, y + 1] + SQRT2);
+
+ if (minDist != double.MaxValue)
+ distanceMap[x, y] = minDist;
+ }
+ }
+ }
+
+ // 距离变换 - 反向扫描
+ for (int x = gridMap.Width - 1; x >= 0; x--)
+ {
+ for (int y = gridMap.Height - 1; y >= 0; y--)
+ {
+ if (distanceMap[x, y] != 0.0)
+ {
+ double minDist = distanceMap[x, y];
+
+ if (x < gridMap.Width - 1)
+ minDist = Math.Min(minDist, distanceMap[x + 1, y] + 1.0);
+ if (y < gridMap.Height - 1)
+ minDist = Math.Min(minDist, distanceMap[x, y + 1] + 1.0);
+ if (x < gridMap.Width - 1 && y < gridMap.Height - 1)
+ minDist = Math.Min(minDist, distanceMap[x + 1, y + 1] + SQRT2);
+ if (x < gridMap.Width - 1 && y > 0)
+ minDist = Math.Min(minDist, distanceMap[x + 1, y - 1] + SQRT2);
+
+ distanceMap[x, y] = minDist;
+ }
+ }
+ }
+
+ // 应用膨胀结果：设置该层的IsWalkable
+ 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 == CategoryAttributeManager.LogisticsElementType.门)
+ continue;
+
+ // 膨胀条件：距离 < 膨胀半径
+ // 边界网格距离=0，算在膨胀半径内
+ // 例如：半径=1 → 只膨胀边界本身（距离0）
+ // 半径=2 → 膨胀边界+向内1格（距离0,1）
+ // 半径=3 → 膨胀边界+向内2格（距离0,1,2）
+ bool shouldInflate = false;
+
+ if (distanceMap[x, y] < inflationRadius)
+ {
+ shouldInflate = true;
+ }
+
+ if (shouldInflate)
+ {
+ // 设置该层为不可通行
+ layer.IsWalkable = false;
+ cell.HeightLayers[layerIndex] = layer;
+ inflatedCount++;
+ }
+ }
+
+ gridMap.Cells[x, y] = cell;
+ }
+ }
+
+ layerStats[layerIndex] = (boundaryCount, inflatedCount);
+ LogManager.Info($"【多层膨胀】Layer{layerIndex} 完成，边界: {boundaryCount}, 膨胀: {inflatedCount}");
+ }
+
+ // 第三步：更新GridCell.IsWalkable（至少有一层可通行）
+ int cellsBecameUnwalkable = 0;
+ for (int x = 0; x < gridMap.Width; x++)
+ {
+ for (int y = 0; y < gridMap.Height; y++)
+ {
+ var cell = gridMap.Cells[x, y];
+ bool wasWalkable = cell.IsWalkable;
+
+ // 检查是否至少有一层可通行
+ bool hasWalkableLayer = false;
+ if (cell.HeightLayers != null && cell.HeightLayers.Count > 0)
+ {
+ hasWalkableLayer = cell.HeightLayers.Any(layer => layer.IsWalkable);
+ }
+
+ cell.IsWalkable = hasWalkableLayer;
+ gridMap.Cells[x, y] = cell;
+
+ if (wasWalkable && !hasWalkableLayer)
+ cellsBecameUnwalkable++;
+ }
+ }
+
+ stopwatch.Stop();
+ LogManager.Info($"【多层膨胀】完成，耗时: {stopwatch.ElapsedMilliseconds}ms");
+ LogManager.Info($"【多层膨胀】网格变为不可通行: {cellsBecameUnwalkable}个");
+
+ foreach (var kvp in layerStats.OrderBy(k => k.Key))
+ {
+ LogManager.Info($"【多层膨胀】Layer{kvp.Key}: 边界{kvp.Value.boundary}个, 膨胀{kvp.Value.inflated}层");
+ }
+ }
+
+ ///
+ /// 旧的2D膨胀算法（已废弃，保留用于参考）
+ ///
+ private void ApplyVehicleInflationOptimized_Old2D(GridMap gridMap, int inflationRadius)
+ {
+ var stopwatch = System.Diagnostics.Stopwatch.StartNew();
+
LogManager.Info($"【8连通膨胀】使用正确的8连通距离变换算法，网格大小: {gridMap.Width}x{gridMap.Height}");
// 使用浮点数距离矩阵支持√2对角线距离
@@ -715,6 +1043,7 @@ namespace NavisworksTransport.PathPlanning
// 初始化距离矩阵
int obstacleCount = 0;
int unknownCount = 0;
+ int boundaryCount = 0;
for (int x = 0; x < gridMap.Width; x++)
{
@@ -722,7 +1051,7 @@ namespace NavisworksTransport.PathPlanning
{
var cell = gridMap.Cells[x, y];
- // 障碍物和Unknown网格作为膨胀源点
+ // 膨胀源点：1.障碍物 2.Unknown网格 3.边界层网格
if (!cell.IsWalkable)
{
distanceMap[x, y] = 0.0;
@@ -733,6 +1062,12 @@ namespace NavisworksTransport.PathPlanning
distanceMap[x, y] = 0.0;
unknownCount++;
}
+ else if (cell.HeightLayers != null && cell.HeightLayers.Any(layer => layer.IsBoundary))
+ {
+ // 包含边界层的网格作为膨胀源点
+ distanceMap[x, y] = 0.0;
+ boundaryCount++;
+ }
else
{
distanceMap[x, y] = double.MaxValue;
@@ -740,7 +1075,7 @@ namespace NavisworksTransport.PathPlanning
}
}
- LogManager.Info($"【8连通膨胀】膨胀源点统计 - 障碍物: {obstacleCount}, Unknown: {unknownCount}");
+ LogManager.Info($"【8连通膨胀】膨胀源点统计 - 障碍物: {obstacleCount}, Unknown: {unknownCount}, 边界: {boundaryCount}");
// 使用标准的距离变换算法 - 正向扫描
for (int x = 0; x < gridMap.Width; x++)