完善了膨胀算法，把障碍物和边界分开计算
This commit is contained in:
parent
83a4a0e7aa
commit
df1885a352
@ -846,8 +846,8 @@ namespace NavisworksTransport.PathPlanning
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 多层膨胀算法 - 按高度层分别处理膨胀
|
||||
/// 核心原则：边界层只影响同层索引的邻居层
|
||||
/// 多层膨胀算法 - 分两次处理：障碍物膨胀 + 边界膨胀
|
||||
/// 核心原则：障碍物从外围膨胀（不含本身），边界从边界本身膨胀（包含边界）
|
||||
/// </summary>
|
||||
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<int, (int boundary, int inflated)>();
|
||||
var layerStats = new Dictionary<int, (int obstacleInflated, int boundaryCount, int boundaryInflated)>();
|
||||
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}层");
|
||||
}
|
||||
}
|
||||
|
||||
/// <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 == 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;
|
||||
}
|
||||
|
||||
/// <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 == 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);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 旧的2D膨胀算法（已废弃，保留用于参考）
|
||||
/// </summary>
|
||||
|
||||
Loading…
Reference in New Issue
Block a user