diff --git a/.claude/settings.local.json b/.claude/settings.local.json index c02678d..551525e 100644 --- a/.claude/settings.local.json +++ b/.claude/settings.local.json @@ -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" ] } -} \ No newline at end of file +} diff --git a/src/PathPlanning/AutoPathFinder.cs b/src/PathPlanning/AutoPathFinder.cs index e174d40..e8395d4 100644 --- a/src/PathPlanning/AutoPathFinder.cs +++ b/src/PathPlanning/AutoPathFinder.cs @@ -374,15 +374,11 @@ namespace NavisworksTransport.PathPlanning case PathStrategy.Straightest: LogManager.Info($"[策略路由] 使用直线优先策略"); - // 暂时返回空字典，后续可以扩展支持 - var straightGrid = ConvertToAStarGridStraightest(gridMap, channelCoverage, vehicleHeight, startPos, endPos); - return (straightGrid, new Dictionary()); + return ConvertToAStarGridStraightest(gridMap, channelCoverage, vehicleHeight, startPos, endPos); case PathStrategy.SafestCenter: LogManager.Info($"[策略路由] 使用安全优先策略"); - // 暂时返回空字典，后续可以扩展支持 - var safeGrid = ConvertToAStarGridSafestCenter(gridMap, channelCoverage, vehicleHeight); - return (safeGrid, new Dictionary()); + return ConvertToAStarGridSafestCenter(gridMap, channelCoverage, vehicleHeight, startPos, endPos); default: LogManager.Warning($"[策略路由] 未知策略 {strategy}，使用默认最短路径"); @@ -400,12 +396,16 @@ namespace NavisworksTransport.PathPlanning /// 起点坐标 /// 终点坐标 /// A*网格 - private Grid ConvertToAStarGridStraightest(GridMap gridMap, ChannelCoverage channelCoverage, double vehicleHeight, Point3D startPos, Point3D endPos) + private (Grid grid, Dictionary activeLayerZ) ConvertToAStarGridStraightest(GridMap gridMap, ChannelCoverage channelCoverage, double vehicleHeight, Point3D startPos, Point3D endPos) { try { LogManager.Info($"[局部直线优先] 开始实施局部直线优先算法"); + // 初始化激活高度层字典和目标高度 + var activeLayerZ = new Dictionary(); + 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*网格转换方法 /// 🔥 重写：参照局部直线优先算法的成功模式，重新构建网格连接 /// - private Grid ConvertToAStarGridSafestCenter(GridMap gridMap, ChannelCoverage channelCoverage, double vehicleHeight) + private (Grid grid, Dictionary activeLayerZ) ConvertToAStarGridSafestCenter(GridMap gridMap, ChannelCoverage channelCoverage, double vehicleHeight, Point3D startPos, Point3D endPos) { try { LogManager.Info($"[安全优先] 开始实施安全优先算法，参照成功的直线优先模式"); + // 初始化激活高度层字典和目标高度 + var activeLayerZ = new Dictionary(); + 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) {