From df1885a35241a1250e07cf240a2e2d33c5bb08b7 Mon Sep 17 00:00:00 2001
From: tian <11429339@qq.com>
Date: Fri, 10 Oct 2025 23:51:13 +0800
Subject: [PATCH] =?UTF-8?q?=E5=AE=8C=E5=96=84=E4=BA=86=E8=86=A8=E8=83=80?=
=?UTF-8?q?=E7=AE=97=E6=B3=95=EF=BC=8C=E6=8A=8A=E9=9A=9C=E7=A2=8D=E7=89=A9?=
=?UTF-8?q?=E5=92=8C=E8=BE=B9=E7=95=8C=E5=88=86=E5=BC=80=E8=AE=A1=E7=AE=97?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/PathPlanning/GridMapGenerator.cs | 392 ++++++++++++++++-----------
1 file changed, 240 insertions(+), 152 deletions(-)
diff --git a/src/PathPlanning/GridMapGenerator.cs b/src/PathPlanning/GridMapGenerator.cs
index 7a32550..7a92cff 100644
--- a/src/PathPlanning/GridMapGenerator.cs
+++ b/src/PathPlanning/GridMapGenerator.cs
@@ -846,8 +846,8 @@ namespace NavisworksTransport.PathPlanning
}
///
- /// 多层膨胀算法 - 按高度层分别处理膨胀
- /// 核心原则：边界层只影响同层索引的邻居层
+ /// 多层膨胀算法 - 分两次处理：障碍物膨胀 + 边界膨胀
+ /// 核心原则：障碍物从外围膨胀（不含本身），边界从边界本身膨胀（包含边界）
///
private void ApplyVehicleInflationOptimized(GridMap gridMap, int inflationRadius)
{
@@ -855,8 +855,12 @@ namespace NavisworksTransport.PathPlanning
LogManager.Info($"【多层膨胀】开始按层处理膨胀，网格大小: {gridMap.Width}x{gridMap.Height}, 膨胀半径: {inflationRadius}");
+ // 计算高度差阈值（用于楼梯口保护）
+ double metersToModelUnitsConversionFactor = UnitsConverter.GetMetersToUnitsConversionFactor(Application.ActiveDocument.Units);
+ double maxHeightDiffInModelUnits = MAX_HEIGHT_DIFF_METERS * metersToModelUnitsConversionFactor;
+
// 统计每个层索引的处理情况
- var layerStats = new Dictionary();
+ var layerStats = new Dictionary();
int maxLayerIndex = 0;
// 第一步：找出最大层索引
@@ -877,169 +881,253 @@ namespace NavisworksTransport.PathPlanning
// 第二步：对每个层索引分别处理膨胀
for (int layerIndex = 0; layerIndex <= maxLayerIndex; layerIndex++)
{
- int boundaryCount = 0;
- int inflatedCount = 0;
+ // 步骤1：障碍物膨胀（不包括障碍物本身）
+ int obstacleInflated = InflateObstacles(gridMap, layerIndex, inflationRadius);
- // 为这一层创建距离图
- var distanceMap = new double[gridMap.Width, gridMap.Height];
- const double SQRT2 = 1.414213562373095;
+ // 步骤2：边界膨胀（包括边界本身）
+ var (boundaryCount, boundaryInflated) = InflateBoundaries(gridMap, layerIndex, inflationRadius, maxHeightDiffInModelUnits);
- // 初始化：标记障碍物、Unknown和边界层
- for (int x = 0; x < gridMap.Width; x++)
+ if (boundaryCount > 0 || obstacleInflated > 0)
{
- for (int y = 0; y < gridMap.Height; y++)
- {
- var cell = gridMap.Cells[x, y];
-
- // 默认为无穷大
- distanceMap[x, y] = double.MaxValue;
-
- // 1. Unknown网格 → 距离为0（整个网格无法通行）
- if (cell.CellType == CategoryAttributeManager.LogisticsElementType.Unknown)
- {
- distanceMap[x, y] = 0.0;
- continue;
- }
-
- // 2. 检查该层索引是否存在
- if (cell.HeightLayers != null && layerIndex < cell.HeightLayers.Count)
- {
- var layer = cell.HeightLayers[layerIndex];
-
- // 2a. 该层不可通行 → 距离为0（障碍物层）
- if (!layer.IsWalkable)
- {
- distanceMap[x, y] = 0.0;
- continue;
- }
-
- // 2b. 该层是边界 → 距离为0
- if (layer.IsBoundary)
- {
- distanceMap[x, y] = 0.0;
- boundaryCount++;
- }
- }
- else
- {
- // 3. 该层索引不存在（网格没有这一层）→ 设置为-1，不参与距离变换
- distanceMap[x, y] = -1.0;
- }
- }
+ layerStats[layerIndex] = (obstacleInflated, boundaryCount, boundaryInflated);
+ LogManager.Info($"【多层膨胀】Layer{layerIndex} 完成 - 障碍物膨胀: {obstacleInflated}层, 边界: {boundaryCount}个, 边界膨胀: {boundaryInflated}层");
}
-
- 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;
-
- // 从邻居计算距离，跳过 < 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 < gridMap.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 = gridMap.Width - 1; x >= 0; x--)
- {
- for (int y = gridMap.Height - 1; y >= 0; y--)
- {
- if (distanceMap[x, y] > 0.0) // 排除边界(0)和没有该层(-1)
- {
- double minDist = distanceMap[x, y];
-
- // 从邻居计算距离，跳过 < 0 的邻居（没有该层的网格）
- if (x < gridMap.Width - 1 && distanceMap[x + 1, y] >= 0.0)
- minDist = Math.Min(minDist, distanceMap[x + 1, y] + 1.0);
- if (y < gridMap.Height - 1 && distanceMap[x, y + 1] >= 0.0)
- minDist = Math.Min(minDist, distanceMap[x, y + 1] + 1.0);
- if (x < gridMap.Width - 1 && y < gridMap.Height - 1 && distanceMap[x + 1, y + 1] >= 0.0)
- minDist = Math.Min(minDist, distanceMap[x + 1, y + 1] + SQRT2);
- if (x < gridMap.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;
- }
- }
- }
-
- // 应用膨胀结果：设置该层的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（包括边界）且距离<半径
- // 例如：半径=2 → 膨胀距离0、1的网格（总共2格）
- // 半径=3 → 膨胀距离0、1、2的网格（总共3格）
- bool shouldInflate = false;
-
- if (distanceMap[x, y] >= 0.0 && 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已移除，通行性由HeightLayer.IsWalkable决定
- // 不再需要更新网格级别的IsWalkable
-
stopwatch.Stop();
LogManager.Info($"【多层膨胀】完成，耗时: {stopwatch.ElapsedMilliseconds}ms");
foreach (var kvp in layerStats.OrderBy(k => k.Key))
{
- LogManager.Info($"【多层膨胀】Layer{kvp.Key}: 边界{kvp.Value.boundary}个, 膨胀{kvp.Value.inflated}层");
+ LogManager.Info($"【多层膨胀】Layer{kvp.Key}: 障碍物膨胀{kvp.Value.obstacleInflated}层, 边界{kvp.Value.boundaryCount}个, 边界膨胀{kvp.Value.boundaryInflated}层");
}
}
+ ///
+ /// 距离变换算法 - 8连通距离计算
+ ///
+ 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;
+ }
+ }
+ }
+ }
+
+ ///
+ /// 障碍物膨胀 - 从障碍物外围开始膨胀，不包括障碍物本身
+ ///
+ 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 == CategoryAttributeManager.LogisticsElementType.Unknown)
+ {
+ 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 == CategoryAttributeManager.LogisticsElementType.门)
+ 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;
+ }
+
+ ///
+ /// 边界膨胀 - 从边界网格本身开始膨胀，包括边界网格
+ ///
+ 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 == CategoryAttributeManager.LogisticsElementType.门)
+ 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);
+ }
+
///
/// 旧的2D膨胀算法（已废弃，保留用于参考）
///