tian/NavisworksTransport
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

为安全优先和直线优先增加高度层支持。

Browse Source
This commit is contained in:
tian 2025-10-09 19:07:34 +08:00
parent df6ba1c51e
commit ed7bc13866
2 changed files with 281 additions and 138 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
.claude/settings.local.json
View File

@ -157,7 +157,8 @@
"Bash(\"bin\\Debug\\AStarTestRunner.exe\")",
"Bash(git restore:*)",
"Read(//c/Program Files/Autodesk/Navisworks Manage 2026/**)",
"Bash(where:*)"
"Bash(where:*)",
"Read(//c/Users/Tellme/Pictures/Screenshots/**)"
],
"deny": [],
"additionalDirectories": [
@ -165,4 +166,4 @@
"C:\\Program Files\\Autodesk\\Navisworks Manage 2026\\Plugins\\NavisworksTransportPlugin"
]
}
}
}

414
src/PathPlanning/AutoPathFinder.cs
View File

@ -374,15 +374,11 @@ namespace NavisworksTransport.PathPlanning
case PathStrategy.Straightest:
LogManager.Info($"[策略路由] 使用直线优先策略");
// 暂时返回空字典，后续可以扩展支持
var straightGrid = ConvertToAStarGridStraightest(gridMap, channelCoverage, vehicleHeight, startPos, endPos);
return (straightGrid, new Dictionary<GridPoint2D, double>());
return ConvertToAStarGridStraightest(gridMap, channelCoverage, vehicleHeight, startPos, endPos);
case PathStrategy.SafestCenter:
LogManager.Info($"[策略路由] 使用安全优先策略");
// 暂时返回空字典，后续可以扩展支持
var safeGrid = ConvertToAStarGridSafestCenter(gridMap, channelCoverage, vehicleHeight);
return (safeGrid, new Dictionary<GridPoint2D, double>());
return ConvertToAStarGridSafestCenter(gridMap, channelCoverage, vehicleHeight, startPos, endPos);
default:
LogManager.Warning($"[策略路由] 未知策略 {strategy}，使用默认最短路径");
@ -400,12 +396,16 @@ namespace NavisworksTransport.PathPlanning
/// <param name="startPos">起点坐标</param>
/// <param name="endPos">终点坐标</param>
/// <returns>A*网格</returns>
private Grid ConvertToAStarGridStraightest(GridMap gridMap, ChannelCoverage channelCoverage, double vehicleHeight, Point3D startPos, Point3D endPos)
private (Grid grid, Dictionary<GridPoint2D, double> activeLayerZ) ConvertToAStarGridStraightest(GridMap gridMap, ChannelCoverage channelCoverage, double vehicleHeight, Point3D startPos, Point3D endPos)
{
try
{
LogManager.Info($"[局部直线优先] 开始实施局部直线优先算法");
// 初始化激活高度层字典和目标高度
var activeLayerZ = new Dictionary<GridPoint2D, double>();
double targetZ = endPos.Z;
// 1. 创建网格基础结构
double cellSizeInMeters = UnitsConverter.ConvertToMeters(gridMap.CellSize);
var gridSize = new GridSize(gridMap.Width, gridMap.Height);
@ -423,9 +423,6 @@ namespace NavisworksTransport.PathPlanning
for (int y = 0; y < gridMap.Height; y++)
{
var pos = new GridPosition(x, y);
var nodeBefore = grid.GetNode(pos);
var outgoingCountBefore = nodeBefore.Outgoing.Count();
grid.DisconnectNode(pos);
}
}
@ -445,118 +442,199 @@ namespace NavisworksTransport.PathPlanning
{
var cell = gridMap.Cells[x, y];
// 检查基本可通行性和高度约束
bool isPassable = IsPassableWithHeight(cell, vehicleHeight);
if (cell.IsWalkable && !isPassable)
// 检查是否有兼容的高度层
if (!HasCompatibleLayer(cell, vehicleHeight))
{
heightConstrainedCells++;
if (cell.IsWalkable)
{
heightConstrainedCells++;
}
continue;
}
if (isPassable)
// 选择当前网格的最佳高度层
var currentZ = SelectBestLayerForTarget(cell.HeightLayers, targetZ, vehicleHeight);
if (!currentZ.HasValue)
{
var pos = new GridPosition(x, y);
var gridPos = new GridPoint2D(x, y);
connectedCells++;
continue;
}
// 🔥 核心改动：右方向连接（基于局部直线距离）
if (x + 1 < gridMap.Width)
var pos = new GridPosition(x, y);
var gridPos = new GridPoint2D(x, y);
// 记录当前网格的激活高度层
activeLayerZ[gridPos] = currentZ.Value;
connectedCells++;
// 🔥 核心改动：右方向连接（基于局部直线距离）
if (x + 1 < gridMap.Width)
{
var rightCell = gridMap.Cells[x + 1, y];
// 检查右侧网格是否有兼容的高度层
if (HasCompatibleLayer(rightCell, vehicleHeight))
{
var rightCell = gridMap.Cells[x + 1, y];
if (IsPassableWithHeight(rightCell, vehicleHeight))
// 选择右侧网格的最佳高度层
var rightZ = SelectBestLayerForTarget(rightCell.HeightLayers, targetZ, vehicleHeight);
if (rightZ.HasValue)
{
var rightPos = new GridPosition(x + 1, y);
var rightGridPos = new GridPoint2D(x + 1, y);
// 记录右侧网格的激活高度层
activeLayerZ[rightGridPos] = rightZ.Value;
// 计算沿右方向的直线距离
var rightDistance = CalculateStraightDistance(gridPos, new GridPoint2D(1, 0), gridMap, vehicleHeight);
var rightVelocity = CalculateVelocityByDistance(rightDistance);
// 🔥 修复：只添加单向边，避免强制连接障碍物网格
// 反向边会在处理rightPos时自然添加（如果rightPos也可通行）
// 计算基础速度（基于直线距离）
float baseSpeed = 5.0f;
float bonusSpeed = Math.Min(rightDistance * 0.5f, 30.0f);
float straightSpeed = baseSpeed + bonusSpeed;
// 🔥 重连验证：确保不会连接到障碍物
var sourceCell = gridMap.Cells[x, y];
var targetCell = gridMap.Cells[x + 1, y];
if (!sourceCell.IsWalkable || !targetCell.IsWalkable)
{
LogManager.Error($"[重连验证失败] 尝试连接障碍物: ({x},{y})[IsWalkable={sourceCell.IsWalkable}] -> ({x+1},{y})[IsWalkable={targetCell.IsWalkable}]");
}
// 添加高度惩罚：离目标高度越远，速度越慢
double distanceToTarget = Math.Abs(rightZ.Value - targetZ);
float heightMultiplier = CalculateSpeedMultiplier(distanceToTarget);
float finalSpeed = straightSpeed * heightMultiplier;
var rightVelocity = Velocity.FromKilometersPerHour(finalSpeed);
// 添加单向边
grid.AddEdge(pos, rightPos, rightVelocity);
dynamicConnections++;
}
else if ((x >= 49 && x <= 62 && y == 25) || (x == 71 && y == 23))
{
LogManager.Info($"[A*连接诊断] 跳过连接({x},{y})->({x+1},{y}): 目标网格IsWalkable={rightCell.IsWalkable}, CellType={rightCell.CellType}");
}
}
}
// 🔥 核心改动：下方向连接（基于局部直线距离）
if (y + 1 < gridMap.Height)
// 🔥 核心改动：下方向连接（基于局部直线距离）
if (y + 1 < gridMap.Height)
{
var bottomCell = gridMap.Cells[x, y + 1];
// 检查下方网格是否有兼容的高度层
if (HasCompatibleLayer(bottomCell, vehicleHeight))
{
var bottomCell = gridMap.Cells[x, y + 1];
if (IsPassableWithHeight(bottomCell, vehicleHeight))
// 选择下方网格的最佳高度层
var bottomZ = SelectBestLayerForTarget(bottomCell.HeightLayers, targetZ, vehicleHeight);
if (bottomZ.HasValue)
{
var bottomPos = new GridPosition(x, y + 1);
var bottomGridPos = new GridPoint2D(x, y + 1);
// 记录下方网格的激活高度层
activeLayerZ[bottomGridPos] = bottomZ.Value;
// 计算沿下方向的直线距离
var downDistance = CalculateStraightDistance(gridPos, new GridPoint2D(0, 1), gridMap, vehicleHeight);
var downVelocity = CalculateVelocityByDistance(downDistance);
// 🔥 修复：只添加单向边，避免强制连接障碍物网格
// 反向边会在处理bottomPos时自然添加（如果bottomPos也可通行）
// 计算基础速度（基于直线距离）
float baseSpeed = 5.0f;
float bonusSpeed = Math.Min(downDistance * 0.5f, 30.0f);
float straightSpeed = baseSpeed + bonusSpeed;
// 添加高度惩罚：离目标高度越远，速度越慢
double distanceToTarget = Math.Abs(bottomZ.Value - targetZ);
float heightMultiplier = CalculateSpeedMultiplier(distanceToTarget);
float finalSpeed = straightSpeed * heightMultiplier;
var downVelocity = Velocity.FromKilometersPerHour(finalSpeed);
// 添加单向边
grid.AddEdge(pos, bottomPos, downVelocity);
dynamicConnections++;
}
}
}
// 🔥 核心改动：左方向连接（基于局部直线距离）
if (x - 1 >= 0)
// 🔥 核心改动：左方向连接（基于局部直线距离）
if (x - 1 >= 0)
{
var leftCell = gridMap.Cells[x - 1, y];
// 检查左侧网格是否有兼容的高度层
if (HasCompatibleLayer(leftCell, vehicleHeight))
{
var leftCell = gridMap.Cells[x - 1, y];
if (IsPassableWithHeight(leftCell, vehicleHeight))
// 选择左侧网格的最佳高度层
var leftZ = SelectBestLayerForTarget(leftCell.HeightLayers, targetZ, vehicleHeight);
if (leftZ.HasValue)
{
var leftPos = new GridPosition(x - 1, y);
var leftGridPos = new GridPoint2D(x - 1, y);
// 记录左侧网格的激活高度层
activeLayerZ[leftGridPos] = leftZ.Value;
// 计算沿左方向的直线距离
var leftDistance = CalculateStraightDistance(gridPos, new GridPoint2D(-1, 0), gridMap, vehicleHeight);
var leftVelocity = CalculateVelocityByDistance(leftDistance);
// 🔥 修复：只添加单向边，避免强制连接障碍物网格
// 反向边会在处理leftPos时自然添加（如果leftPos也可通行）
// 计算基础速度（基于直线距离）
float baseSpeed = 5.0f;
float bonusSpeed = Math.Min(leftDistance * 0.5f, 30.0f);
float straightSpeed = baseSpeed + bonusSpeed;
// 添加高度惩罚：离目标高度越远，速度越慢
double distanceToTarget = Math.Abs(leftZ.Value - targetZ);
float heightMultiplier = CalculateSpeedMultiplier(distanceToTarget);
float finalSpeed = straightSpeed * heightMultiplier;
var leftVelocity = Velocity.FromKilometersPerHour(finalSpeed);
// 添加单向边
grid.AddEdge(pos, leftPos, leftVelocity);
dynamicConnections++;
}
}
}
// 🔥 核心改动：上方向连接（基于局部直线距离）
if (y - 1 >= 0)
// 🔥 核心改动：上方向连接（基于局部直线距离）
if (y - 1 >= 0)
{
var topCell = gridMap.Cells[x, y - 1];
// 检查上方网格是否有兼容的高度层
if (HasCompatibleLayer(topCell, vehicleHeight))
{
var topCell = gridMap.Cells[x, y - 1];
if (IsPassableWithHeight(topCell, vehicleHeight))
// 选择上方网格的最佳高度层
var topZ = SelectBestLayerForTarget(topCell.HeightLayers, targetZ, vehicleHeight);
if (topZ.HasValue)
{
var topPos = new GridPosition(x, y - 1);
var topGridPos = new GridPoint2D(x, y - 1);
// 记录上方网格的激活高度层
activeLayerZ[topGridPos] = topZ.Value;
// 计算沿上方向的直线距离
var upDistance = CalculateStraightDistance(gridPos, new GridPoint2D(0, -1), gridMap, vehicleHeight);
var upVelocity = CalculateVelocityByDistance(upDistance);
// 🔥 修复：只添加单向边，避免强制连接障碍物网格
// 反向边会在处理topPos时自然添加（如果topPos也可通行）
// 计算基础速度（基于直线距离）
float baseSpeed = 5.0f;
float bonusSpeed = Math.Min(upDistance * 0.5f, 30.0f);
float straightSpeed = baseSpeed + bonusSpeed;
// 添加高度惩罚：离目标高度越远，速度越慢
double distanceToTarget = Math.Abs(topZ.Value - targetZ);
float heightMultiplier = CalculateSpeedMultiplier(distanceToTarget);
float finalSpeed = straightSpeed * heightMultiplier;
var upVelocity = Velocity.FromKilometersPerHour(finalSpeed);
// 添加单向边
grid.AddEdge(pos, topPos, upVelocity);
dynamicConnections++;
}
}
}
// 对角线连接（使用固定的较低速度）
// 右下对角线
if (x + 1 < gridMap.Width && y + 1 < gridMap.Height)
// 对角线连接（使用固定的较低速度）
// 右下对角线
if (x + 1 < gridMap.Width && y + 1 < gridMap.Height)
{
var diagonalCell = gridMap.Cells[x + 1, y + 1];
if (HasCompatibleLayer(diagonalCell, vehicleHeight))
{
var diagonalCell = gridMap.Cells[x + 1, y + 1];
if (IsPassableWithHeight(diagonalCell, vehicleHeight))
var diagonalZ = SelectBestLayerForTarget(diagonalCell.HeightLayers, targetZ, vehicleHeight);
if (diagonalZ.HasValue)
{
var diagonalPos = new GridPosition(x + 1, y + 1);
// 🔥 修复：只添加单向边，避免强制连接障碍物网格
// 反向边会在处理diagonalPos时自然添加（如果diagonalPos也可通行）
var diagonalGridPos = new GridPoint2D(x + 1, y + 1);
activeLayerZ[diagonalGridPos] = diagonalZ.Value;
grid.AddEdge(pos, diagonalPos, diagonalVelocity);
}
}
@ -570,8 +648,9 @@ namespace NavisworksTransport.PathPlanning
LogManager.Info($"[局部直线优先] - 被高度约束排除: {heightConstrainedCells}");
LogManager.Info($"[局部直线优先] - 动态连接数: {dynamicConnections}");
LogManager.Info($"[局部直线优先] - A*可用率: {(double)connectedCells / (gridMap.Width * gridMap.Height) * 100:F1}%");
LogManager.Info($"[局部直线优先] - 激活层记录数: {activeLayerZ.Count}");
return grid;
return (grid, activeLayerZ);
}
catch (Exception ex)
{
@ -1826,12 +1905,16 @@ namespace NavisworksTransport.PathPlanning
/// 安全优先的A*网格转换方法
/// 🔥 重写：参照局部直线优先算法的成功模式，重新构建网格连接
/// </summary>
private Grid ConvertToAStarGridSafestCenter(GridMap gridMap, ChannelCoverage channelCoverage, double vehicleHeight)
private (Grid grid, Dictionary<GridPoint2D, double> activeLayerZ) ConvertToAStarGridSafestCenter(GridMap gridMap, ChannelCoverage channelCoverage, double vehicleHeight, Point3D startPos, Point3D endPos)
{
try
{
LogManager.Info($"[安全优先] 开始实施安全优先算法，参照成功的直线优先模式");
// 初始化激活高度层字典和目标高度
var activeLayerZ = new Dictionary<GridPoint2D, double>();
double targetZ = endPos.Z;
// 清空日志记录集合，确保每次执行都能看到映射关系
_loggedDistances.Clear();
@ -1853,20 +1936,7 @@ namespace NavisworksTransport.PathPlanning
for (int y = 0; y < gridMap.Height; y++)
{
var pos = new GridPosition(x, y);
var nodeBefore = grid.GetNode(pos);
var outgoingCountBefore = nodeBefore.Outgoing.Count();
grid.DisconnectNode(pos);
// 🔥 诊断记录：断开后验证
var nodeAfter = grid.GetNode(pos);
var outgoingCountAfter = nodeAfter.Outgoing.Count();
// 只记录关键坐标的详细信息
if ((x >= 49 && x <= 62 && y == 25) || (x == 71 && y == 23))
{
LogManager.Info($"[安全优先断开诊断] 网格({x},{y}): 断开前边数={outgoingCountBefore} -> 断开后边数={outgoingCountAfter}");
}
}
}
LogManager.Info($"[安全优先断开诊断] 所有节点连接断开完成，准备重新构建");
@ -1887,122 +1957,193 @@ namespace NavisworksTransport.PathPlanning
{
var cell = gridMap.Cells[x, y];
// 检查基本可通行性和高度约束
bool isPassable = IsPassableWithHeight(cell, vehicleHeight);
if (cell.IsWalkable && !isPassable)
// 检查是否有兼容的高度层
if (!HasCompatibleLayer(cell, vehicleHeight))
{
heightConstrainedCells++;
if (cell.IsWalkable)
{
heightConstrainedCells++;
}
continue;
}
if (isPassable)
// 选择当前网格的最佳高度层
var currentZ = SelectBestLayerForTarget(cell.HeightLayers, targetZ, vehicleHeight);
if (!currentZ.HasValue)
{
var pos = new GridPosition(x, y);
var currentSafetyDistance = safetyDistanceMap[x, y];
connectedCells++;
continue;
}
// 🔥 核心改动：右方向连接（基于安全距离）
if (x + 1 < gridMap.Width)
var pos = new GridPosition(x, y);
var gridPos = new GridPoint2D(x, y);
var currentSafetyDistance = safetyDistanceMap[x, y];
// 记录当前网格的激活高度层
activeLayerZ[gridPos] = currentZ.Value;
connectedCells++;
// 🔥 核心改动：右方向连接（基于安全距离）
if (x + 1 < gridMap.Width)
{
var rightCell = gridMap.Cells[x + 1, y];
// 检查右侧网格是否有兼容的高度层
if (HasCompatibleLayer(rightCell, vehicleHeight))
{
var rightCell = gridMap.Cells[x + 1, y];
if (IsPassableWithHeight(rightCell, vehicleHeight))
// 选择右侧网格的最佳高度层
var rightZ = SelectBestLayerForTarget(rightCell.HeightLayers, targetZ, vehicleHeight);
if (rightZ.HasValue)
{
var rightPos = new GridPosition(x + 1, y);
var rightGridPos = new GridPoint2D(x + 1, y);
var rightSafetyDistance = safetyDistanceMap[x + 1, y];
// 记录右侧网格的激活高度层
activeLayerZ[rightGridPos] = rightZ.Value;
// 使用两个位置中更安全的距离
var maxSafetyDistance = Math.Max(currentSafetyDistance, rightSafetyDistance);
var speedValue = CalculateSpeedBySafetyDistance(maxSafetyDistance);
var rightVelocity = Velocity.FromKilometersPerHour(speedValue);
var baseSafetySpeed = CalculateSpeedBySafetyDistance(maxSafetyDistance);
// 统计速度分布
if (velocityStats.ContainsKey(speedValue))
velocityStats[speedValue]++;
// 添加高度惩罚：离目标高度越远，速度越慢
double distanceToTarget = Math.Abs(rightZ.Value - targetZ);
float heightMultiplier = CalculateSpeedMultiplier(distanceToTarget);
float finalSpeed = baseSafetySpeed * heightMultiplier;
var rightVelocity = Velocity.FromKilometersPerHour(finalSpeed);
// 统计速度分布（使用最终速度）
if (velocityStats.ContainsKey(finalSpeed))
velocityStats[finalSpeed]++;
else
velocityStats[speedValue] = 1;
velocityStats[finalSpeed] = 1;
// 🔥 修复：只添加单向边，避免强制连接障碍物网格
// 反向边会在处理rightPos时自然添加（如果rightPos也可通行）
// 添加单向边
grid.AddEdge(pos, rightPos, rightVelocity);
safetyConnections++;
}
}
}
// 🔥 核心改动：下方向连接（基于安全距离）
if (y + 1 < gridMap.Height)
// 🔥 核心改动：下方向连接（基于安全距离）
if (y + 1 < gridMap.Height)
{
var bottomCell = gridMap.Cells[x, y + 1];
// 检查下方网格是否有兼容的高度层
if (HasCompatibleLayer(bottomCell, vehicleHeight))
{
var bottomCell = gridMap.Cells[x, y + 1];
if (IsPassableWithHeight(bottomCell, vehicleHeight))
// 选择下方网格的最佳高度层
var bottomZ = SelectBestLayerForTarget(bottomCell.HeightLayers, targetZ, vehicleHeight);
if (bottomZ.HasValue)
{
var bottomPos = new GridPosition(x, y + 1);
var bottomGridPos = new GridPoint2D(x, y + 1);
var bottomSafetyDistance = safetyDistanceMap[x, y + 1];
// 记录下方网格的激活高度层
activeLayerZ[bottomGridPos] = bottomZ.Value;
// 使用两个位置中更安全的距离
var maxSafetyDistance = Math.Max(currentSafetyDistance, bottomSafetyDistance);
var speedValue = CalculateSpeedBySafetyDistance(maxSafetyDistance);
var bottomVelocity = Velocity.FromKilometersPerHour(speedValue);
var baseSafetySpeed = CalculateSpeedBySafetyDistance(maxSafetyDistance);
// 统计速度分布
if (velocityStats.ContainsKey(speedValue))
velocityStats[speedValue]++;
// 添加高度惩罚：离目标高度越远，速度越慢
double distanceToTarget = Math.Abs(bottomZ.Value - targetZ);
float heightMultiplier = CalculateSpeedMultiplier(distanceToTarget);
float finalSpeed = baseSafetySpeed * heightMultiplier;
var bottomVelocity = Velocity.FromKilometersPerHour(finalSpeed);
// 统计速度分布（使用最终速度）
if (velocityStats.ContainsKey(finalSpeed))
velocityStats[finalSpeed]++;
else
velocityStats[speedValue] = 1;
velocityStats[finalSpeed] = 1;
// 🔥 修复：只添加单向边，避免强制连接障碍物网格
// 反向边会在处理bottomPos时自然添加（如果bottomPos也可通行）
// 添加单向边
grid.AddEdge(pos, bottomPos, bottomVelocity);
safetyConnections++;
}
}
}
// 🔥 连通性修复：左方向连接（基于安全距离）
if (x - 1 >= 0)
// 🔥 连通性修复：左方向连接（基于安全距离）
if (x - 1 >= 0)
{
var leftCell = gridMap.Cells[x - 1, y];
// 检查左侧网格是否有兼容的高度层
if (HasCompatibleLayer(leftCell, vehicleHeight))
{
var leftCell = gridMap.Cells[x - 1, y];
if (IsPassableWithHeight(leftCell, vehicleHeight))
// 选择左侧网格的最佳高度层
var leftZ = SelectBestLayerForTarget(leftCell.HeightLayers, targetZ, vehicleHeight);
if (leftZ.HasValue)
{
var leftPos = new GridPosition(x - 1, y);
var leftGridPos = new GridPoint2D(x - 1, y);
var leftSafetyDistance = safetyDistanceMap[x - 1, y];
// 记录左侧网格的激活高度层
activeLayerZ[leftGridPos] = leftZ.Value;
// 使用两个位置中更安全的距离
var maxSafetyDistance = Math.Max(currentSafetyDistance, leftSafetyDistance);
var speedValue = CalculateSpeedBySafetyDistance(maxSafetyDistance);
var leftVelocity = Velocity.FromKilometersPerHour(speedValue);
var baseSafetySpeed = CalculateSpeedBySafetyDistance(maxSafetyDistance);
// 统计速度分布
if (velocityStats.ContainsKey(speedValue))
velocityStats[speedValue]++;
// 添加高度惩罚：离目标高度越远，速度越慢
double distanceToTarget = Math.Abs(leftZ.Value - targetZ);
float heightMultiplier = CalculateSpeedMultiplier(distanceToTarget);
float finalSpeed = baseSafetySpeed * heightMultiplier;
var leftVelocity = Velocity.FromKilometersPerHour(finalSpeed);
// 统计速度分布（使用最终速度）
if (velocityStats.ContainsKey(finalSpeed))
velocityStats[finalSpeed]++;
else
velocityStats[speedValue] = 1;
velocityStats[finalSpeed] = 1;
// 🔥 修复：只添加单向边，避免强制连接障碍物网格
// 反向边会在处理leftPos时自然添加（如果leftPos也可通行）
// 添加单向边
grid.AddEdge(pos, leftPos, leftVelocity);
safetyConnections++;
}
}
}
// 🔥 连通性修复：上方向连接（基于安全距离）
if (y - 1 >= 0)
// 🔥 连通性修复：上方向连接（基于安全距离）
if (y - 1 >= 0)
{
var topCell = gridMap.Cells[x, y - 1];
// 检查上方网格是否有兼容的高度层
if (HasCompatibleLayer(topCell, vehicleHeight))
{
var topCell = gridMap.Cells[x, y - 1];
if (IsPassableWithHeight(topCell, vehicleHeight))
// 选择上方网格的最佳高度层
var topZ = SelectBestLayerForTarget(topCell.HeightLayers, targetZ, vehicleHeight);
if (topZ.HasValue)
{
var topPos = new GridPosition(x, y - 1);
var topGridPos = new GridPoint2D(x, y - 1);
var topSafetyDistance = safetyDistanceMap[x, y - 1];
// 记录上方网格的激活高度层
activeLayerZ[topGridPos] = topZ.Value;
// 使用两个位置中更安全的距离
var maxSafetyDistance = Math.Max(currentSafetyDistance, topSafetyDistance);
var speedValue = CalculateSpeedBySafetyDistance(maxSafetyDistance);
var topVelocity = Velocity.FromKilometersPerHour(speedValue);
var baseSafetySpeed = CalculateSpeedBySafetyDistance(maxSafetyDistance);
// 统计速度分布
if (velocityStats.ContainsKey(speedValue))
velocityStats[speedValue]++;
// 添加高度惩罚：离目标高度越远，速度越慢
double distanceToTarget = Math.Abs(topZ.Value - targetZ);
float heightMultiplier = CalculateSpeedMultiplier(distanceToTarget);
float finalSpeed = baseSafetySpeed * heightMultiplier;
var topVelocity = Velocity.FromKilometersPerHour(finalSpeed);
// 统计速度分布（使用最终速度）
if (velocityStats.ContainsKey(finalSpeed))
velocityStats[finalSpeed]++;
else
velocityStats[speedValue] = 1;
velocityStats[finalSpeed] = 1;
// 🔥 修复：只添加单向边，避免强制连接障碍物网格
// 反向边会在处理topPos时自然添加（如果topPos也可通行）
// 添加单向边
grid.AddEdge(pos, topPos, topVelocity);
safetyConnections++;
}
@ -2017,6 +2158,7 @@ namespace NavisworksTransport.PathPlanning
LogManager.Info($"[安全优先] - 被高度约束排除: {heightConstrainedCells}");
LogManager.Info($"[安全优先] - 安全连接数: {safetyConnections}");
LogManager.Info($"[安全优先] - A*可用率: {(double)connectedCells / (gridMap.Width * gridMap.Height) * 100:F1}%");
LogManager.Info($"[安全优先] - 激活层记录数: {activeLayerZ.Count}");
// 输出速度统计信息
if (velocityStats.Count > 0)
@ -2026,7 +2168,7 @@ namespace NavisworksTransport.PathPlanning
LogManager.Info($"[安全优先速度统计] {velocityStatsText}");
}
return grid;
return (grid, activeLayerZ);
}
catch (Exception ex)
{