1985 lines
96 KiB
C#
1985 lines
96 KiB
C#
using System;
|
||
using System.Collections.Generic;
|
||
using System.Linq;
|
||
using Autodesk.Navisworks.Api;
|
||
using Roy_T.AStar.Grids;
|
||
using Roy_T.AStar.Primitives;
|
||
using Roy_T.AStar.Paths;
|
||
using NavisworksTransport.Utils;
|
||
|
||
namespace NavisworksTransport.PathPlanning
|
||
{
|
||
/// <summary>
|
||
/// 路径查找结果
|
||
/// </summary>
|
||
public class PathFindingResult
|
||
{
|
||
/// <summary>
|
||
/// 路径点列表
|
||
/// </summary>
|
||
public List<Point3D> PathPoints { get; set; } = new List<Point3D>();
|
||
|
||
/// <summary>
|
||
/// 路径是否完整
|
||
/// </summary>
|
||
public bool IsComplete { get; set; } = true;
|
||
|
||
/// <summary>
|
||
/// 原始终点（用户指定的）
|
||
/// </summary>
|
||
public Point3D OriginalEndPoint { get; set; }
|
||
|
||
/// <summary>
|
||
/// 实际到达点
|
||
/// </summary>
|
||
public Point3D ActualEndPoint { get; set; }
|
||
|
||
/// <summary>
|
||
/// 完成百分比
|
||
/// </summary>
|
||
public double CompletionPercentage { get; set; } = 100.0;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 自动路径查找器
|
||
/// 基于A*算法的路径规划核心类
|
||
/// 支持传统模式和2.5D高度约束的路径规划
|
||
/// </summary>
|
||
public class AutoPathFinder
|
||
{
|
||
/// <summary>
|
||
/// 2.5D路径规划（使用通道覆盖数据、高度约束和路径策略）
|
||
/// </summary>
|
||
/// <param name="start">起点（世界坐标）</param>
|
||
/// <param name="end">终点（世界坐标）</param>
|
||
/// <param name="gridMap">网格地图</param>
|
||
/// <param name="channelCoverage">通道覆盖数据</param>
|
||
/// <param name="vehicleHeight">车辆高度（模型单位）</param>
|
||
/// <param name="strategy">路径规划策略</param>
|
||
/// <returns>路径查找结果</returns>
|
||
public PathFindingResult FindPath(Point3D start, Point3D end, GridMap gridMap, ChannelCoverage channelCoverage, double vehicleHeight, PathStrategy strategy = PathStrategy.Shortest)
|
||
{
|
||
try
|
||
{
|
||
LogManager.Info("[2.5D路径规划] 开始执行");
|
||
|
||
// 保存原始起点和终点，用于最终路径修正
|
||
var originalStart = start;
|
||
var originalEnd = end;
|
||
|
||
// 1. 检查起点和终点的高度约束
|
||
if (!IsPointPassableAtHeight(start, gridMap, vehicleHeight))
|
||
{
|
||
LogManager.Warning("[2.5D路径规划] 起点位置不满足高度约束");
|
||
}
|
||
|
||
if (!IsPointPassableAtHeight(end, gridMap, vehicleHeight))
|
||
{
|
||
LogManager.Warning("[2.5D路径规划] 终点位置不满足高度约束");
|
||
}
|
||
|
||
// 2. 根据策略选择合适的网格转换方法
|
||
var astarGrid = ConvertToAStarGridWithStrategy(gridMap, channelCoverage, vehicleHeight, start, end, strategy);
|
||
|
||
// 3. 执行A*算法
|
||
var pathResult = ExecuteAStarAlgorithm(start, end, gridMap, astarGrid);
|
||
|
||
if (pathResult == null || !pathResult.PathPoints.Any())
|
||
{
|
||
LogManager.Warning("[2.5D路径规划] 未找到有效路径");
|
||
return new PathFindingResult
|
||
{
|
||
PathPoints = new List<Point3D>(),
|
||
IsComplete = false,
|
||
OriginalEndPoint = end,
|
||
CompletionPercentage = 0.0
|
||
};
|
||
}
|
||
|
||
LogManager.Info($"[2.5D路径规划] A*算法找到原始路径，包含 {pathResult.PathPoints.Count} 个点");
|
||
|
||
// 5. 根据网格点通行高度区间对路径点Z坐标调整
|
||
var enhancedPath = ApplyGridHeightConstraints(pathResult.PathPoints, gridMap, vehicleHeight);
|
||
|
||
// 6. 验证路径中所有点的高度约束（特别是门网格）
|
||
ValidatePathHeightConstraints(enhancedPath, gridMap, vehicleHeight);
|
||
|
||
// 更新PathFindingResult
|
||
pathResult.PathPoints = enhancedPath;
|
||
pathResult.OriginalEndPoint = originalEnd;
|
||
|
||
LogManager.Info($"[2.5D路径规划] 路径生成完成，最终包含 {enhancedPath.Count} 个路径点，完成度: {pathResult.CompletionPercentage:F1}%");
|
||
|
||
// 路径优化 - 去除共线点
|
||
pathResult = OptimizePath(pathResult, gridMap);
|
||
|
||
// 记录缓存统计信息
|
||
LogCacheStatistics();
|
||
|
||
return pathResult;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"[2.5D路径规划] 路径查找失败: {ex.Message}");
|
||
return new PathFindingResult
|
||
{
|
||
PathPoints = new List<Point3D>(),
|
||
IsComplete = false,
|
||
OriginalEndPoint = end,
|
||
CompletionPercentage = 0.0
|
||
};
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将网格地图转换为A*算法使用的格式（2.5D模式）
|
||
/// </summary>
|
||
/// <param name="gridMap">网格地图</param>
|
||
/// <param name="channelCoverage">通道覆盖数据</param>
|
||
/// <param name="vehicleHeight">车辆高度（模型单位）</param>
|
||
/// <returns>A*算法网格</returns>
|
||
private Grid ConvertToAStarGridWith2_5D(GridMap gridMap, ChannelCoverage channelCoverage, double vehicleHeight)
|
||
{
|
||
try
|
||
{
|
||
LogManager.Info($"[A*转换-2.5D] 开始转换网格格式: {gridMap.Width}x{gridMap.Height}，车辆高度: {vehicleHeight}m");
|
||
|
||
// 输出详细的网格统计信息
|
||
LogManager.Info($"[A*转换-2.5D] 输入网格统计信息:\n{gridMap.GetStatistics()}");
|
||
|
||
// 单位转换
|
||
double cellSizeInMeters = UnitsConverter.ConvertToMeters(gridMap.CellSize);
|
||
LogManager.Info($"[A*转换-2.5D] 单位转换: {gridMap.CellSize:F2}模型单位 -> {cellSizeInMeters:F2}米");
|
||
|
||
// 创建A*网格
|
||
var gridSize = new GridSize(gridMap.Width, gridMap.Height);
|
||
var cellSize = new Size(Distance.FromMeters((float)cellSizeInMeters), Distance.FromMeters((float)cellSizeInMeters));
|
||
var traversalVelocity = Velocity.FromKilometersPerHour(5); // 5 km/h 的遍历速度
|
||
|
||
var grid = Grid.CreateGridWithLateralConnections(gridSize, cellSize, traversalVelocity);
|
||
LogManager.Info($"[A*转换-2.5D] A*网格创建完成");
|
||
|
||
// 先断开所有节点连接
|
||
LogManager.Info($"[2.5D断开诊断] 开始断开所有节点连接，网格尺寸: {gridMap.Width}x{gridMap.Height}");
|
||
for (int x = 0; x < gridMap.Width; x++)
|
||
{
|
||
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($"[2.5D断开诊断] 网格({x},{y}): 断开前边数={outgoingCountBefore} -> 断开后边数={outgoingCountAfter}");
|
||
}
|
||
}
|
||
}
|
||
LogManager.Info($"[2.5D断开诊断] 所有节点连接断开完成");
|
||
|
||
// 只连接可通行的节点
|
||
int connectedCells = 0;
|
||
int totalWalkableCells = 0;
|
||
int totalNonWalkableCells = 0;
|
||
int heightConstrainedCells = 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.IsWalkable)
|
||
{
|
||
totalWalkableCells++;
|
||
}
|
||
else
|
||
{
|
||
totalNonWalkableCells++;
|
||
}
|
||
|
||
// 检查基本可通行性和高度约束
|
||
bool isPassable = cell.IsWalkable;
|
||
|
||
if (isPassable)
|
||
{
|
||
if (cell.PassableHeight.MaxZ > cell.PassableHeight.MinZ)
|
||
{
|
||
// vehicleHeight已经是模型单位，直接比较即可
|
||
bool heightOk = cell.PassableHeight.GetSpan() >= vehicleHeight;
|
||
if (!heightOk)
|
||
{
|
||
heightConstrainedCells++;
|
||
|
||
// 详细日志：记录被高度约束排除的单元格（仅记录前10个避免日志过多）
|
||
if (heightConstrainedCells <= 10)
|
||
{
|
||
LogManager.Info($"[A*高度约束] 单元格({x},{y})被高度约束排除：车辆高度{vehicleHeight:F2}模型单位，可用区间：{cell.PassableHeight.MinZ:F2}-{cell.PassableHeight.MaxZ:F2}(跨度{cell.PassableHeight.GetSpan():F2})");
|
||
}
|
||
else if (heightConstrainedCells == 11)
|
||
{
|
||
LogManager.Info($"[A*高度约束] 还有更多单元格被高度约束排除，不再详细记录...");
|
||
}
|
||
}
|
||
isPassable = heightOk;
|
||
}
|
||
else
|
||
{
|
||
// 没有高度信息的网格直接标记为不可通行
|
||
isPassable = false;
|
||
}
|
||
}
|
||
|
||
if (isPassable)
|
||
{
|
||
// 直接使用网格索引（RoyT.AStar内部处理米坐标转换）
|
||
var pos = new GridPosition(x, y);
|
||
connectedCells++;
|
||
|
||
// 连接右侧邻居
|
||
if (x + 1 < gridMap.Width)
|
||
{
|
||
var rightCell = gridMap.Cells[x + 1, y];
|
||
bool rightPassable = rightCell.IsWalkable;
|
||
if (rightPassable && rightCell.PassableHeight.MaxZ > rightCell.PassableHeight.MinZ)
|
||
{
|
||
rightPassable = rightCell.PassableHeight.GetSpan() >= vehicleHeight;
|
||
}
|
||
if (rightPassable)
|
||
{
|
||
var rightPos = new GridPosition(x + 1, y);
|
||
// 🔥 修复：只添加单向边，避免强制连接障碍物网格
|
||
// 反向边会在处理rightPos时自然添加（如果rightPos也可通行）
|
||
grid.AddEdge(pos, rightPos, traversalVelocity);
|
||
}
|
||
}
|
||
|
||
// 连接下方邻居
|
||
if (y + 1 < gridMap.Height)
|
||
{
|
||
var bottomCell = gridMap.Cells[x, y + 1];
|
||
bool bottomPassable = bottomCell.IsWalkable;
|
||
if (bottomPassable && bottomCell.PassableHeight.MaxZ > bottomCell.PassableHeight.MinZ)
|
||
{
|
||
bottomPassable = bottomCell.PassableHeight.GetSpan() >= vehicleHeight;
|
||
}
|
||
if (bottomPassable)
|
||
{
|
||
var bottomPos = new GridPosition(x, y + 1);
|
||
// 🔥 修复：只添加单向边，避免强制连接障碍物网格
|
||
// 反向边会在处理bottomPos时自然添加（如果bottomPos也可通行）
|
||
grid.AddEdge(pos, bottomPos, traversalVelocity);
|
||
}
|
||
}
|
||
|
||
// 🔥 连通性修复：添加左侧邻居连接
|
||
if (x - 1 >= 0)
|
||
{
|
||
var leftCell = gridMap.Cells[x - 1, y];
|
||
bool leftPassable = leftCell.IsWalkable;
|
||
if (leftPassable && leftCell.PassableHeight.MaxZ > leftCell.PassableHeight.MinZ)
|
||
{
|
||
leftPassable = leftCell.PassableHeight.GetSpan() >= vehicleHeight;
|
||
}
|
||
if (leftPassable)
|
||
{
|
||
var leftPos = new GridPosition(x - 1, y);
|
||
// 🔥 修复：只添加单向边，避免强制连接障碍物网格
|
||
// 反向边会在处理leftPos时自然添加（如果leftPos也可通行）
|
||
grid.AddEdge(pos, leftPos, traversalVelocity);
|
||
}
|
||
}
|
||
|
||
// 🔥 连通性修复：添加上方邻居连接
|
||
if (y - 1 >= 0)
|
||
{
|
||
var topCell = gridMap.Cells[x, y - 1];
|
||
bool topPassable = topCell.IsWalkable;
|
||
if (topPassable && topCell.PassableHeight.MaxZ > topCell.PassableHeight.MinZ)
|
||
{
|
||
topPassable = topCell.PassableHeight.GetSpan() >= vehicleHeight;
|
||
}
|
||
if (topPassable)
|
||
{
|
||
var topPos = new GridPosition(x, y - 1);
|
||
// 🔥 修复：只添加单向边，避免强制连接障碍物网格
|
||
// 反向边会在处理topPos时自然添加（如果topPos也可通行）
|
||
grid.AddEdge(pos, topPos, traversalVelocity);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
LogManager.Info($"[A*转换-2.5D] 网格转换完成，A*节点连接统计:");
|
||
LogManager.Info($"[A*转换-2.5D] - 总网格单元格: {gridMap.Width * gridMap.Height}");
|
||
LogManager.Info($"[A*转换-2.5D] - 基本可通行单元格: {totalWalkableCells}");
|
||
LogManager.Info($"[A*转换-2.5D] - 基本不可通行单元格: {totalNonWalkableCells}");
|
||
LogManager.Info($"[A*转换-2.5D] - 被高度约束排除: {heightConstrainedCells}");
|
||
LogManager.Info($"[A*转换-2.5D] - 最终连接到A*网格的节点: {connectedCells}");
|
||
LogManager.Info($"[A*转换-2.5D] - A*可用率: {(double)connectedCells / (gridMap.Width * gridMap.Height) * 100:F1}%");
|
||
return grid;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"[A*转换-2.5D] 转换网格格式时发生错误: {ex.Message}");
|
||
throw new AutoPathPlanningException($"2.5D网格转换失败: {ex.Message}", ex);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 根据路径策略选择合适的A*网格转换方法
|
||
/// </summary>
|
||
/// <param name="gridMap">网格地图</param>
|
||
/// <param name="channelCoverage">通道覆盖数据</param>
|
||
/// <param name="vehicleHeight">车辆高度（模型单位）</param>
|
||
/// <param name="startPos">起点坐标</param>
|
||
/// <param name="endPos">终点坐标</param>
|
||
/// <param name="strategy">路径规划策略</param>
|
||
/// <returns>A*网格</returns>
|
||
private Grid ConvertToAStarGridWithStrategy(GridMap gridMap, ChannelCoverage channelCoverage, double vehicleHeight, Point3D startPos, Point3D endPos, PathStrategy strategy)
|
||
{
|
||
switch (strategy)
|
||
{
|
||
case PathStrategy.Shortest:
|
||
LogManager.Info($"[策略路由] 使用最短路径策略");
|
||
return ConvertToAStarGridWith2_5D(gridMap, channelCoverage, vehicleHeight);
|
||
|
||
case PathStrategy.Straightest:
|
||
LogManager.Info($"[策略路由] 使用直线优先策略");
|
||
return ConvertToAStarGridStraightest(gridMap, channelCoverage, vehicleHeight, startPos, endPos);
|
||
|
||
case PathStrategy.SafestCenter:
|
||
LogManager.Info($"[策略路由] 使用安全优先策略");
|
||
return ConvertToAStarGridSafestCenter(gridMap, channelCoverage, vehicleHeight);
|
||
|
||
default:
|
||
LogManager.Warning($"[策略路由] 未知策略 {strategy}，使用默认最短路径");
|
||
return ConvertToAStarGridWith2_5D(gridMap, channelCoverage, vehicleHeight);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 直线优先的A*网格转换方法
|
||
/// 通过调整不同方向边的速度来引导算法优先选择主方向路径
|
||
/// </summary>
|
||
/// <param name="gridMap">网格地图</param>
|
||
/// <param name="channelCoverage">通道覆盖数据</param>
|
||
/// <param name="vehicleHeight">车辆高度（模型单位）</param>
|
||
/// <param name="startPos">起点坐标</param>
|
||
/// <param name="endPos">终点坐标</param>
|
||
/// <returns>A*网格</returns>
|
||
private Grid ConvertToAStarGridStraightest(GridMap gridMap, ChannelCoverage channelCoverage, double vehicleHeight, Point3D startPos, Point3D endPos)
|
||
{
|
||
try
|
||
{
|
||
LogManager.Info($"[局部直线优先] 开始实施局部直线优先算法");
|
||
|
||
// 1. 创建网格基础结构
|
||
double cellSizeInMeters = UnitsConverter.ConvertToMeters(gridMap.CellSize);
|
||
var gridSize = new GridSize(gridMap.Width, gridMap.Height);
|
||
var cellSize = new Size(Distance.FromMeters((float)cellSizeInMeters), Distance.FromMeters((float)cellSizeInMeters));
|
||
|
||
// 使用基础速度创建网格，后续会动态调整
|
||
var baseVelocity = Velocity.FromKilometersPerHour(5); // 基础速度
|
||
var grid = Grid.CreateGridWithLateralConnections(gridSize, cellSize, baseVelocity);
|
||
|
||
LogManager.Info($"[局部直线优先] A*网格创建完成，尺寸: {gridMap.Width}x{gridMap.Height}");
|
||
|
||
// 2. 断开所有连接，准备重新按局部直线距离连接
|
||
for (int x = 0; x < gridMap.Width; x++)
|
||
{
|
||
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);
|
||
}
|
||
}
|
||
LogManager.Info($"[直线优先断开诊断] 所有节点连接断开完成");
|
||
|
||
// 3. 对角线固定速度（较低优先级）
|
||
var diagonalVelocity = Velocity.FromKilometersPerHour(6);
|
||
|
||
// 4. 智能连接：基于局部直线距离设置速度
|
||
int connectedCells = 0;
|
||
int heightConstrainedCells = 0;
|
||
int dynamicConnections = 0;
|
||
|
||
for (int x = 0; x < gridMap.Width; x++)
|
||
{
|
||
for (int y = 0; y < gridMap.Height; y++)
|
||
{
|
||
var cell = gridMap.Cells[x, y];
|
||
|
||
// 检查基本可通行性和高度约束
|
||
bool isPassable = cell.IsWalkable;
|
||
if (isPassable && cell.PassableHeight.MaxZ > cell.PassableHeight.MinZ)
|
||
{
|
||
// vehicleHeight已经是模型单位，直接比较即可
|
||
bool heightOk = cell.PassableHeight.GetSpan() >= vehicleHeight;
|
||
if (!heightOk)
|
||
{
|
||
heightConstrainedCells++;
|
||
isPassable = false;
|
||
}
|
||
}
|
||
|
||
if (isPassable)
|
||
{
|
||
var pos = new GridPosition(x, y);
|
||
var gridPos = new GridPoint2D(x, y);
|
||
connectedCells++;
|
||
|
||
// 🔥 核心改动：右方向连接（基于局部直线距离）
|
||
if (x + 1 < gridMap.Width)
|
||
{
|
||
var rightCell = gridMap.Cells[x + 1, y];
|
||
if (IsPassableWithHeight(rightCell, vehicleHeight))
|
||
{
|
||
var rightPos = new GridPosition(x + 1, y);
|
||
|
||
// 计算沿右方向的直线距离
|
||
var rightDistance = CalculateStraightDistance(gridPos, new GridPoint2D(1, 0), gridMap, vehicleHeight);
|
||
var rightVelocity = CalculateVelocityByDistance(rightDistance);
|
||
|
||
// 🔥 修复：只添加单向边，避免强制连接障碍物网格
|
||
// 反向边会在处理rightPos时自然添加（如果rightPos也可通行）
|
||
|
||
// 🔥 重连验证：确保不会连接到障碍物
|
||
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}]");
|
||
}
|
||
|
||
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)
|
||
{
|
||
var bottomCell = gridMap.Cells[x, y + 1];
|
||
if (IsPassableWithHeight(bottomCell, vehicleHeight))
|
||
{
|
||
var bottomPos = new GridPosition(x, y + 1);
|
||
|
||
// 计算沿下方向的直线距离
|
||
var downDistance = CalculateStraightDistance(gridPos, new GridPoint2D(0, 1), gridMap, vehicleHeight);
|
||
var downVelocity = CalculateVelocityByDistance(downDistance);
|
||
|
||
// 🔥 修复：只添加单向边，避免强制连接障碍物网格
|
||
// 反向边会在处理bottomPos时自然添加（如果bottomPos也可通行）
|
||
grid.AddEdge(pos, bottomPos, downVelocity);
|
||
dynamicConnections++;
|
||
}
|
||
}
|
||
|
||
// 🔥 核心改动：左方向连接（基于局部直线距离）
|
||
if (x - 1 >= 0)
|
||
{
|
||
var leftCell = gridMap.Cells[x - 1, y];
|
||
if (IsPassableWithHeight(leftCell, vehicleHeight))
|
||
{
|
||
var leftPos = new GridPosition(x - 1, y);
|
||
|
||
// 计算沿左方向的直线距离
|
||
var leftDistance = CalculateStraightDistance(gridPos, new GridPoint2D(-1, 0), gridMap, vehicleHeight);
|
||
var leftVelocity = CalculateVelocityByDistance(leftDistance);
|
||
|
||
// 🔥 修复：只添加单向边，避免强制连接障碍物网格
|
||
// 反向边会在处理leftPos时自然添加（如果leftPos也可通行）
|
||
grid.AddEdge(pos, leftPos, leftVelocity);
|
||
dynamicConnections++;
|
||
}
|
||
}
|
||
|
||
// 🔥 核心改动：上方向连接（基于局部直线距离）
|
||
if (y - 1 >= 0)
|
||
{
|
||
var topCell = gridMap.Cells[x, y - 1];
|
||
if (IsPassableWithHeight(topCell, vehicleHeight))
|
||
{
|
||
var topPos = new GridPosition(x, y - 1);
|
||
|
||
// 计算沿上方向的直线距离
|
||
var upDistance = CalculateStraightDistance(gridPos, new GridPoint2D(0, -1), gridMap, vehicleHeight);
|
||
var upVelocity = CalculateVelocityByDistance(upDistance);
|
||
|
||
// 🔥 修复：只添加单向边，避免强制连接障碍物网格
|
||
// 反向边会在处理topPos时自然添加（如果topPos也可通行）
|
||
grid.AddEdge(pos, topPos, upVelocity);
|
||
dynamicConnections++;
|
||
}
|
||
}
|
||
|
||
// 对角线连接（使用固定的较低速度）
|
||
// 右下对角线
|
||
if (x + 1 < gridMap.Width && y + 1 < gridMap.Height)
|
||
{
|
||
var diagonalCell = gridMap.Cells[x + 1, y + 1];
|
||
if (IsPassableWithHeight(diagonalCell, vehicleHeight))
|
||
{
|
||
var diagonalPos = new GridPosition(x + 1, y + 1);
|
||
// 🔥 修复：只添加单向边，避免强制连接障碍物网格
|
||
// 反向边会在处理diagonalPos时自然添加（如果diagonalPos也可通行）
|
||
grid.AddEdge(pos, diagonalPos, diagonalVelocity);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
LogManager.Info($"[局部直线优先] 网格转换完成，A*节点连接统计:");
|
||
LogManager.Info($"[局部直线优先] - 总网格单元格: {gridMap.Width * gridMap.Height}");
|
||
LogManager.Info($"[局部直线优先] - 连接的节点: {connectedCells}");
|
||
LogManager.Info($"[局部直线优先] - 被高度约束排除: {heightConstrainedCells}");
|
||
LogManager.Info($"[局部直线优先] - 动态连接数: {dynamicConnections}");
|
||
LogManager.Info($"[局部直线优先] - A*可用率: {(double)connectedCells / (gridMap.Width * gridMap.Height) * 100:F1}%");
|
||
|
||
return grid;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"[局部直线优先] 网格转换失败: {ex.Message}");
|
||
throw new AutoPathPlanningException($"局部直线优先网格转换失败: {ex.Message}", ex);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 检查单元格是否满足高度约束
|
||
/// </summary>
|
||
/// <param name="cell">网格单元格</param>
|
||
/// <param name="vehicleHeight">车辆高度（模型单位）</param>
|
||
/// <returns>是否可通行</returns>
|
||
private bool IsPassableWithHeight(GridCell cell, double vehicleHeight)
|
||
{
|
||
if (!cell.IsWalkable)
|
||
return false;
|
||
|
||
if (cell.PassableHeight.MaxZ > cell.PassableHeight.MinZ)
|
||
{
|
||
// vehicleHeight已经是模型单位，直接比较即可
|
||
return cell.PassableHeight.GetSpan() >= vehicleHeight;
|
||
}
|
||
|
||
return false;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算从指定位置沿指定方向能走的最大直线距离
|
||
/// </summary>
|
||
/// <param name="startPos">起始网格位置</param>
|
||
/// <param name="direction">方向 (1,0)=右, (0,1)=下, (-1,0)=左, (0,-1)=上</param>
|
||
/// <param name="gridMap">网格地图</param>
|
||
/// <param name="vehicleHeight">车辆高度（模型单位）</param>
|
||
/// <returns>直线距离（网格单位）</returns>
|
||
private int CalculateStraightDistance(GridPoint2D startPos, GridPoint2D direction,
|
||
GridMap gridMap, double vehicleHeight)
|
||
{
|
||
int distance = 0;
|
||
var currentPos = new GridPoint2D(startPos.X + direction.X, startPos.Y + direction.Y);
|
||
|
||
// 沿方向前进，直到遇到障碍
|
||
while (IsValidAndPassable(currentPos, gridMap, vehicleHeight))
|
||
{
|
||
distance++;
|
||
currentPos = new GridPoint2D(currentPos.X + direction.X, currentPos.Y + direction.Y);
|
||
|
||
// 防止无限循环，设置最大搜索距离
|
||
if (distance > 50) break; // 最多看前50格
|
||
}
|
||
|
||
return distance;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 检查网格位置是否有效且可通行
|
||
/// </summary>
|
||
/// <param name="pos">网格位置</param>
|
||
/// <param name="gridMap">网格地图</param>
|
||
/// <param name="vehicleHeight">车辆高度（模型单位）</param>
|
||
/// <returns>是否有效且可通行</returns>
|
||
private bool IsValidAndPassable(GridPoint2D pos, GridMap gridMap, double vehicleHeight)
|
||
{
|
||
// 检查边界
|
||
if (pos.X < 0 || pos.X >= gridMap.Width || pos.Y < 0 || pos.Y >= gridMap.Height)
|
||
return false;
|
||
|
||
// 检查可通行性
|
||
var cell = gridMap.Cells[pos.X, pos.Y];
|
||
return IsPassableWithHeight(cell, vehicleHeight);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 根据直线距离计算速度权重
|
||
/// </summary>
|
||
/// <param name="straightDistance">直线距离</param>
|
||
/// <returns>对应的速度</returns>
|
||
private Velocity CalculateVelocityByDistance(int straightDistance)
|
||
{
|
||
// 基础速度：5km/h
|
||
float baseSpeed = 5.0f;
|
||
|
||
// 奖励机制：每能走1格直线，速度提升0.5km/h，最大提升到15km/h
|
||
float bonusSpeed = Math.Min(straightDistance * 0.5f, 30.0f);
|
||
|
||
return Velocity.FromKilometersPerHour(baseSpeed + bonusSpeed);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将A*路径的米坐标转换为网格坐标
|
||
/// </summary>
|
||
/// <param name="astarPath">A*路径</param>
|
||
/// <param name="gridMap">网格地图</param>
|
||
/// <returns>网格坐标路径</returns>
|
||
public List<GridPoint2D> ConvertPathToGridCoordinates(Path astarPath, GridMap gridMap)
|
||
{
|
||
var gridPath = new List<GridPoint2D>();
|
||
double cellSizeInMeters = UnitsConverter.ConvertToMeters(gridMap.CellSize);
|
||
|
||
// 只添加起始节点一次，然后只添加每条边的终点
|
||
if (astarPath.Edges.Count > 0)
|
||
{
|
||
// 添加第一条边的起点
|
||
var startNode = astarPath.Edges[0].Start;
|
||
var startGridPos = ConvertAStarPositionToGrid(startNode.Position, gridMap);
|
||
gridPath.Add(startGridPos);
|
||
|
||
// 添加每条边的终点（避免重复）
|
||
foreach (var edge in astarPath.Edges)
|
||
{
|
||
var gridPoint = ConvertAStarPositionToGrid(edge.End.Position, gridMap);
|
||
|
||
// 检查是否与上一个点重复（理论上A*不应该产生重复，但保险起见）
|
||
if (gridPath.Count == 0 || !gridPath[gridPath.Count - 1].Equals(gridPoint))
|
||
{
|
||
gridPath.Add(gridPoint);
|
||
}
|
||
}
|
||
}
|
||
|
||
return gridPath;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将A*路径转换为世界坐标
|
||
/// </summary>
|
||
/// <param name="astarPath">A*路径</param>
|
||
/// <param name="gridMap">网格地图</param>
|
||
/// <returns>世界坐标路径</returns>
|
||
private List<Point3D> ConvertPathToWorldCoordinates(Path astarPath, GridMap gridMap)
|
||
{
|
||
var worldPath = new List<Point3D>();
|
||
|
||
LogManager.Info($"[路径转换] 开始转换A*路径，共{astarPath.Edges.Count}条边");
|
||
|
||
// A*返回的是网格左下角坐标，使用统一的转换方法
|
||
if (astarPath.Edges.Count > 0)
|
||
{
|
||
// 🔥 修复：使用统一方法转换A*米坐标为网格坐标，然后转为世界坐标
|
||
var startNode = astarPath.Edges[0].Start;
|
||
var startGridPos = ConvertAStarPositionToGrid(startNode.Position, gridMap);
|
||
var startWorldPos = gridMap.GridToWorld3D(startGridPos);
|
||
|
||
LogManager.Info($"[路径转换] 起始节点: A*米坐标({startNode.Position.X:F2}, {startNode.Position.Y:F2}) -> 世界坐标({startWorldPos.X:F2}, {startWorldPos.Y:F2}, {startWorldPos.Z:F2})");
|
||
worldPath.Add(startWorldPos);
|
||
|
||
// 添加每条边的终点（避免重复）
|
||
foreach (var edge in astarPath.Edges)
|
||
{
|
||
// 使用统一方法转换A*米坐标为网格坐标
|
||
var endGridPos = ConvertAStarPositionToGrid(edge.End.Position, gridMap);
|
||
|
||
// 将网格坐标转换为世界坐标以获取Z坐标
|
||
var endWorldPos = gridMap.GridToWorld3D(endGridPos);
|
||
var endCell = gridMap.GetCell(endGridPos);
|
||
|
||
if (endCell.HasValue)
|
||
{
|
||
// 调试：检查门网格的Z坐标
|
||
if (endCell.Value.CellType == CategoryAttributeManager.LogisticsElementType.门)
|
||
{
|
||
LogManager.Info($"[路径转换-门] 门网格({endGridPos.X},{endGridPos.Y}) WorldPosition.Z={endCell.Value.WorldPosition.Z:F3}, 位置({endWorldPos.X:F2},{endWorldPos.Y:F2})");
|
||
}
|
||
endWorldPos = new Point3D(endWorldPos.X, endWorldPos.Y, endCell.Value.WorldPosition.Z);
|
||
}
|
||
else
|
||
{
|
||
LogManager.Warning($"[路径转换] 网格({endGridPos.X},{endGridPos.Y})获取失败，位置({endWorldPos.X:F2},{endWorldPos.Y:F2})，Z坐标保持为0");
|
||
}
|
||
|
||
// 检查是否与上一个点重复（A*可能在转弯点有重复）
|
||
bool isDuplicate = worldPath.Count > 0 &&
|
||
Math.Abs(worldPath[worldPath.Count - 1].X - endWorldPos.X) < 0.001 &&
|
||
Math.Abs(worldPath[worldPath.Count - 1].Y - endWorldPos.Y) < 0.001;
|
||
|
||
if (!isDuplicate)
|
||
{
|
||
worldPath.Add(endWorldPos);
|
||
}
|
||
}
|
||
}
|
||
|
||
LogManager.Info($"[路径转换] 转换完成，生成{worldPath.Count}个世界坐标点（网格左下角）");
|
||
return worldPath;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 专门的斜线路径优化算法
|
||
/// 在现有优化基础上，通过更激进的斜线连接进一步减少路径点数
|
||
/// </summary>
|
||
/// <param name="optimizedPath">已经过初步优化的路径</param>
|
||
/// <param name="gridMap">网格地图</param>
|
||
/// <returns>经过斜线优化后的路径</returns>
|
||
private List<Point3D> ApplyDiagonalOptimization(List<Point3D> optimizedPath, GridMap gridMap)
|
||
{
|
||
if (optimizedPath.Count <= 2)
|
||
return optimizedPath;
|
||
|
||
try
|
||
{
|
||
LogManager.Info($"[斜线优化] 开始斜线路径优化，输入路径包含 {optimizedPath.Count} 个点");
|
||
|
||
var diagonalOptimizedPath = new List<Point3D> { optimizedPath[0] }; // 保留起点
|
||
|
||
int currentIndex = 0;
|
||
while (currentIndex < optimizedPath.Count - 1)
|
||
{
|
||
int farthestIndex = currentIndex + 1; // 默认下一个点
|
||
|
||
// 🔥 更激进的斜线检测：从P+2开始尝试，直到找到最远可连接点
|
||
for (int testIndex = currentIndex + 2; testIndex < optimizedPath.Count; testIndex++)
|
||
{
|
||
var startPoint = optimizedPath[currentIndex];
|
||
var endPoint = optimizedPath[testIndex];
|
||
var distance = Math.Sqrt(Math.Pow(endPoint.X - startPoint.X, 2) +
|
||
Math.Pow(endPoint.Y - startPoint.Y, 2));
|
||
|
||
// 🔥 新增：在尝试连接远点之前，先检查是否会跳过高度变化点
|
||
bool canSkip = true;
|
||
const double heightTolerance = 1e-6; // 与PathOptimizer保持一致
|
||
|
||
for (int checkIdx = currentIndex + 1; checkIdx < testIndex; checkIdx++)
|
||
{
|
||
// 检查被跳过的点是否有高度变化
|
||
if (Math.Abs(optimizedPath[checkIdx].Z - optimizedPath[checkIdx-1].Z) > heightTolerance ||
|
||
(checkIdx + 1 < optimizedPath.Count &&
|
||
Math.Abs(optimizedPath[checkIdx+1].Z - optimizedPath[checkIdx].Z) > heightTolerance))
|
||
{
|
||
canSkip = false;
|
||
break;
|
||
}
|
||
}
|
||
|
||
if (canSkip && IsDirectPathClear(startPoint, endPoint, gridMap))
|
||
{
|
||
// 可以直线连接，更新最远索引
|
||
farthestIndex = testIndex;
|
||
//LogManager.Debug($"[斜线优化] ✓ 成功连接：点{currentIndex}→点{testIndex}，距离={distance:F2}m");
|
||
}
|
||
else
|
||
{
|
||
// 新增：记录失败详情
|
||
var startGrid = gridMap.WorldToGrid(startPoint);
|
||
var endGrid = gridMap.WorldToGrid(endPoint);
|
||
// LogManager.Debug($"[斜线优化] ✗ 连接失败：" +
|
||
// $"点{currentIndex}[网格({startGrid.X},{startGrid.Y})]→点{testIndex}[网格({endGrid.X},{endGrid.Y})]，" +
|
||
// $"世界坐标：({startPoint.X:F2},{startPoint.Y:F2})→({endPoint.X:F2},{endPoint.Y:F2})，" +
|
||
// $"距离={distance:F2}m，跨越{Math.Abs(endGrid.X-startGrid.X)+Math.Abs(endGrid.Y-startGrid.Y)}个网格");
|
||
}
|
||
// 🔧 关键改进：不像现有算法那样遇到失败就停止，而是继续尝试更远的点
|
||
// 这样可以发现更多斜线连接机会
|
||
}
|
||
|
||
// 如果找到了比相邻点更远的连接，记录优化效果
|
||
if (farthestIndex > currentIndex + 1)
|
||
{
|
||
int skippedPoints = farthestIndex - currentIndex - 1;
|
||
//LogManager.Info($"[斜线优化] 从点{currentIndex}直连到点{farthestIndex}，跳过{skippedPoints}个中间点");
|
||
}
|
||
|
||
currentIndex = farthestIndex;
|
||
if (currentIndex < optimizedPath.Count)
|
||
{
|
||
diagonalOptimizedPath.Add(optimizedPath[currentIndex]);
|
||
}
|
||
}
|
||
|
||
int reducedPoints = optimizedPath.Count - diagonalOptimizedPath.Count;
|
||
LogManager.Info($"[斜线优化] 斜线优化完成，点数变化: {optimizedPath.Count} -> {diagonalOptimizedPath.Count} (减少{reducedPoints}个点)");
|
||
|
||
return diagonalOptimizedPath;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Warning($"[斜线优化] 斜线优化失败: {ex.Message}，使用输入路径");
|
||
return optimizedPath;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 检查两点间的直线路径是否畅通
|
||
/// 使用密集采样确保整条线段都在可通行区域内，避免穿越障碍网格边缘
|
||
/// </summary>
|
||
/// <param name="start">起点</param>
|
||
/// <param name="end">终点</param>
|
||
/// <param name="gridMap">网格地图</param>
|
||
/// <returns>是否畅通</returns>
|
||
private bool IsDirectPathClear(Point3D start, Point3D end, GridMap gridMap)
|
||
{
|
||
try
|
||
{
|
||
// 计算线段长度
|
||
var distance = Math.Sqrt(Math.Pow(end.X - start.X, 2) + Math.Pow(end.Y - start.Y, 2));
|
||
|
||
// 如果距离太小，直接检查起点和终点
|
||
if (distance < gridMap.CellSize * 0.1)
|
||
{
|
||
var startGrid = gridMap.WorldToGrid(start);
|
||
var endGrid = gridMap.WorldToGrid(end);
|
||
bool result = gridMap.IsValidGridPosition(startGrid) && gridMap.IsWalkable(startGrid) &&
|
||
gridMap.IsValidGridPosition(endGrid) && gridMap.IsWalkable(endGrid);
|
||
return result;
|
||
}
|
||
|
||
// 密集采样：每半个网格单位采样一次，确保不会跳过任何障碍网格
|
||
int samples = (int)Math.Ceiling(distance / (gridMap.CellSize * 0.5));
|
||
// 最少采样5个点，最多10000个点（提高性能限制）
|
||
samples = Math.Max(5, Math.Min(samples, 10000));
|
||
|
||
for (int i = 0; i <= samples; i++)
|
||
{
|
||
// 线性插值计算采样点
|
||
double t = samples > 0 ? (double)i / samples : 0;
|
||
var samplePoint = new Point3D(
|
||
start.X + t * (end.X - start.X),
|
||
start.Y + t * (end.Y - start.Y),
|
||
start.Z + t * (end.Z - start.Z)
|
||
);
|
||
|
||
// 基础检查：采样点是否在可通行网格内
|
||
var gridPos = gridMap.WorldToGrid(samplePoint);
|
||
if (!gridMap.IsValidGridPosition(gridPos) || !gridMap.IsWalkable(gridPos))
|
||
{
|
||
// 🔥 修复：使用Origin计算网格左下角，而不是Bounds.Min
|
||
var gridMinX = gridMap.Origin.X + gridPos.X * gridMap.CellSize;
|
||
var gridMinY = gridMap.Origin.Y + gridPos.Y * gridMap.CellSize;
|
||
|
||
// 计算网格中心点世界坐标（基于左下角）
|
||
var gridCenterX = gridMinX + 0.5 * gridMap.CellSize;
|
||
var gridCenterY = gridMinY + 0.5 * gridMap.CellSize;
|
||
var gridCenterZ = samplePoint.Z; // Z保持不变
|
||
|
||
// 计算偏差
|
||
var deltaX = samplePoint.X - gridCenterX;
|
||
var deltaY = samplePoint.Y - gridCenterY;
|
||
|
||
// LogManager.Debug($"[斜线检查] 采样点{i}/{samples}失败：" +
|
||
// $"采样点({samplePoint.X:F3},{samplePoint.Y:F3},{samplePoint.Z:F3})，" +
|
||
// $"网格({gridPos.X},{gridPos.Y})左下角({gridMinX:F3},{gridMinY:F3})，" +
|
||
// $"网格中心({gridCenterX:F3},{gridCenterY:F3},{gridCenterZ:F3})，" +
|
||
// $"偏差(ΔX={deltaX:F3}, ΔY={deltaY:F3})，" +
|
||
// $"原因：{(!gridMap.IsValidGridPosition(gridPos) ? "网格无效" : "不可通行")}");
|
||
return false;
|
||
}
|
||
|
||
// 🔥 新增：邻居障碍位置检查
|
||
if (!IsSamplePointSafeFromNeighborObstacles(samplePoint, gridPos, gridMap))
|
||
{
|
||
// 🔥 修复：使用Origin计算网格左下角
|
||
var gridMinX = gridMap.Origin.X + gridPos.X * gridMap.CellSize;
|
||
var gridMinY = gridMap.Origin.Y + gridPos.Y * gridMap.CellSize;
|
||
|
||
// 计算网格中心点世界坐标
|
||
var gridCenterX = gridMinX + 0.5 * gridMap.CellSize;
|
||
var gridCenterY = gridMinY + 0.5 * gridMap.CellSize;
|
||
var deltaX = samplePoint.X - gridCenterX;
|
||
var deltaY = samplePoint.Y - gridCenterY;
|
||
|
||
// LogManager.Debug($"[斜线检查] 采样点{i}/{samples}邻居障碍检查失败：" +
|
||
// $"采样点({samplePoint.X:F3},{samplePoint.Y:F3})，" +
|
||
// $"网格左下角({gridMinX:F3},{gridMinY:F3})，" +
|
||
// $"网格中心({gridCenterX:F3},{gridCenterY:F3})，" +
|
||
// $"偏差(ΔX={deltaX:F3}, ΔY={deltaY:F3})");
|
||
return false;
|
||
}
|
||
}
|
||
|
||
return true;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"[斜线检查] 检查过程异常: {ex.Message}");
|
||
return false; // 发生错误时保守返回false
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 检查采样点是否在网格内远离相邻障碍物的安全位置
|
||
/// </summary>
|
||
/// <param name="samplePoint">采样点世界坐标</param>
|
||
/// <param name="gridPos">采样点所在网格坐标</param>
|
||
/// <param name="gridMap">网格地图</param>
|
||
/// <returns>是否安全</returns>
|
||
private bool IsSamplePointSafeFromNeighborObstacles(Point3D samplePoint, GridPoint2D gridPos, GridMap gridMap)
|
||
{
|
||
try
|
||
{
|
||
// 🔥 修复：使用Origin计算网格左下角，而不是Bounds.Min
|
||
// 计算采样点在所在网格内的相对位置 (0.0 到 1.0)
|
||
var gridMinX = gridMap.Origin.X + gridPos.X * gridMap.CellSize;
|
||
var gridMinY = gridMap.Origin.Y + gridPos.Y * gridMap.CellSize;
|
||
|
||
var relativeX = (samplePoint.X - gridMinX) / gridMap.CellSize;
|
||
var relativeY = (samplePoint.Y - gridMinY) / gridMap.CellSize;
|
||
|
||
// 确保相对位置在有效范围内
|
||
relativeX = Math.Max(0.0, Math.Min(1.0, relativeX));
|
||
relativeY = Math.Max(0.0, Math.Min(1.0, relativeY));
|
||
|
||
// 定义8个邻居方向的偏移 (左上、上、右上、左、右、左下、下、右下)
|
||
int[] dx = { -1, 0, 1, -1, 1, -1, 0, 1 };
|
||
int[] dy = { -1, -1, -1, 0, 0, 1, 1, 1 };
|
||
string[] directionNames = { "左上", "上", "右上", "左", "右", "左下", "下", "右下" };
|
||
|
||
var obstacleNeighbors = new List<string>();
|
||
|
||
// 检查每个邻居网格
|
||
for (int i = 0; i < 8; i++)
|
||
{
|
||
var neighborX = gridPos.X + dx[i];
|
||
var neighborY = gridPos.Y + dy[i];
|
||
var neighborPos = new GridPoint2D(neighborX, neighborY);
|
||
|
||
bool isValidNeighbor = gridMap.IsValidGridPosition(neighborPos);
|
||
bool isWalkableNeighbor = isValidNeighbor && gridMap.IsWalkable(neighborPos);
|
||
|
||
// 如果邻居网格无效或是障碍物，应用位置约束
|
||
if (!isValidNeighbor || !isWalkableNeighbor)
|
||
{
|
||
obstacleNeighbors.Add(directionNames[i]);
|
||
|
||
// 根据邻居方向应用对应的位置约束
|
||
bool constraintSatisfied = CheckPositionConstraintForObstacleNeighbor(i, relativeX, relativeY);
|
||
|
||
if (!constraintSatisfied)
|
||
{
|
||
// 🔥 修复：基于正确的网格左下角计算网格中心点
|
||
var gridCenterX = gridMinX + 0.5 * gridMap.CellSize;
|
||
var gridCenterY = gridMinY + 0.5 * gridMap.CellSize;
|
||
var deltaX = samplePoint.X - gridCenterX;
|
||
var deltaY = samplePoint.Y - gridCenterY;
|
||
|
||
LogManager.Debug($"[邻居障碍] 位置约束失败：" +
|
||
$"{directionNames[i]}方向有障碍，" +
|
||
$"网格({gridPos.X},{gridPos.Y})，" +
|
||
$"采样点({samplePoint.X:F3},{samplePoint.Y:F3})，" +
|
||
$"网格左下角({gridMinX:F3},{gridMinY:F3})，" +
|
||
$"网格中心({gridCenterX:F3},{gridCenterY:F3})，" +
|
||
$"偏差(ΔX={deltaX:F3}, ΔY={deltaY:F3})，" +
|
||
$"相对位置({relativeX:F3},{relativeY:F3})");
|
||
return false;
|
||
}
|
||
}
|
||
}
|
||
|
||
return true; // 所有约束都满足，位置安全
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"[邻居障碍检查] 检查过程异常: {ex.Message}");
|
||
return false; // 发生错误时保守返回false
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 根据障碍邻居的方向检查位置约束
|
||
/// </summary>
|
||
/// <param name="neighborDirection">邻居方向索引 (0-7)</param>
|
||
/// <param name="relativeX">采样点在网格内的相对X位置 (0.0-1.0)</param>
|
||
/// <param name="relativeY">采样点在网格内的相对Y位置 (0.0-1.0)</param>
|
||
/// <returns>是否满足位置约束</returns>
|
||
private bool CheckPositionConstraintForObstacleNeighbor(int neighborDirection, double relativeX, double relativeY)
|
||
{
|
||
// 容差值仅用于处理浮点数精度问题
|
||
const double tolerance = 1e-3;
|
||
bool result;
|
||
|
||
switch (neighborDirection)
|
||
{
|
||
case 0: // 左上邻居是障碍
|
||
// 采样点不能同时X<0且Y>1
|
||
result = (relativeX >= -tolerance) || (relativeY <= 1.0 + tolerance);
|
||
break;
|
||
|
||
case 1: // 上邻居是障碍
|
||
// 采样点不能Y>1（允许Y=1）
|
||
result = relativeY <= 1.0 + tolerance;
|
||
break;
|
||
|
||
case 2: // 右上邻居是障碍
|
||
// 采样点不能同时X>1且Y>1
|
||
result = (relativeX <= 1.0 + tolerance) || (relativeY <= 1.0 + tolerance);
|
||
break;
|
||
|
||
case 3: // 左邻居是障碍
|
||
// 采样点不能X<0（允许X=0）
|
||
result = relativeX >= -tolerance;
|
||
break;
|
||
|
||
case 4: // 右邻居是障碍
|
||
// 采样点不能X>1（允许X=1）
|
||
result = relativeX <= 1.0 + tolerance;
|
||
break;
|
||
|
||
case 5: // 左下邻居是障碍
|
||
// 采样点不能同时X<0且Y<0
|
||
result = (relativeX >= -tolerance) || (relativeY >= -tolerance);
|
||
break;
|
||
|
||
case 6: // 下邻居是障碍
|
||
// 采样点不能Y<0（允许Y=0）
|
||
result = relativeY >= -tolerance;
|
||
break;
|
||
|
||
case 7: // 右下邻居是障碍
|
||
// 采样点不能同时X>1且Y<0
|
||
result = (relativeX <= 1.0 + tolerance) || (relativeY >= -tolerance);
|
||
break;
|
||
|
||
default:
|
||
result = true; // 未知方向，默认通过
|
||
break;
|
||
}
|
||
|
||
return result;
|
||
}
|
||
|
||
|
||
/// <summary>
|
||
/// 计算路径总长度
|
||
/// </summary>
|
||
/// <param name="path">路径</param>
|
||
/// <returns>总长度</returns>
|
||
public double CalculatePathLength(List<Point3D> path)
|
||
{
|
||
if (path == null || path.Count < 2)
|
||
return 0;
|
||
|
||
double totalLength = 0;
|
||
for (int i = 1; i < path.Count; i++)
|
||
{
|
||
double dx = path[i].X - path[i - 1].X;
|
||
double dy = path[i].Y - path[i - 1].Y;
|
||
totalLength += Math.Sqrt(dx * dx + dy * dy);
|
||
}
|
||
|
||
return totalLength;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算路径复杂度（转弯次数）
|
||
/// </summary>
|
||
/// <param name="path">路径</param>
|
||
/// <returns>转弯次数</returns>
|
||
public int CalculatePathComplexity(List<Point3D> path)
|
||
{
|
||
if (path == null || path.Count < 3)
|
||
return 0;
|
||
|
||
int turns = 0;
|
||
const double angleThreshold = 0.1; // 弧度
|
||
|
||
for (int i = 1; i < path.Count - 1; i++)
|
||
{
|
||
var v1 = new GridPoint2D(
|
||
(int)(path[i].X - path[i - 1].X),
|
||
(int)(path[i].Y - path[i - 1].Y));
|
||
var v2 = new GridPoint2D(
|
||
(int)(path[i + 1].X - path[i].X),
|
||
(int)(path[i + 1].Y - path[i].Y));
|
||
|
||
double angle = Math.Atan2(v2.Y, v2.X) - Math.Atan2(v1.Y, v1.X);
|
||
if (Math.Abs(angle) > angleThreshold)
|
||
{
|
||
turns++;
|
||
}
|
||
}
|
||
|
||
return turns;
|
||
}
|
||
|
||
|
||
/// <summary>
|
||
/// 修正路径的起点和终点为原始用户指定的坐标，避免网格转换造成的错位
|
||
/// </summary>
|
||
/// <param name="path">原始路径</param>
|
||
/// <param name="originalStart">原始起点坐标</param>
|
||
/// <param name="originalEnd">原始终点坐标</param>
|
||
/// <returns>修正后的路径</returns>
|
||
private List<Point3D> CorrectStartEndPoints(List<Point3D> path, Point3D originalStart, Point3D originalEnd)
|
||
{
|
||
if (path == null || path.Count == 0)
|
||
return path;
|
||
|
||
try
|
||
{
|
||
LogManager.Info($"[坐标修正] 开始修正路径起终点坐标");
|
||
var correctedPath = new List<Point3D>(path);
|
||
|
||
// 修正起点：保持原始XY坐标，使用路径的Z坐标
|
||
if (correctedPath.Count > 0)
|
||
{
|
||
var originalStartWithZ = new Point3D(originalStart.X, originalStart.Y, correctedPath[0].Z);
|
||
LogManager.Info($"[坐标修正] 起点: ({correctedPath[0].X:F3}, {correctedPath[0].Y:F3}, {correctedPath[0].Z:F3}) -> ({originalStartWithZ.X:F3}, {originalStartWithZ.Y:F3}, {originalStartWithZ.Z:F3})");
|
||
correctedPath[0] = originalStartWithZ;
|
||
}
|
||
|
||
// 修正终点：保持原始XY坐标，使用路径的Z坐标
|
||
if (correctedPath.Count > 1)
|
||
{
|
||
var lastIndex = correctedPath.Count - 1;
|
||
var originalEndWithZ = new Point3D(originalEnd.X, originalEnd.Y, correctedPath[lastIndex].Z);
|
||
LogManager.Info($"[坐标修正] 终点: ({correctedPath[lastIndex].X:F3}, {correctedPath[lastIndex].Y:F3}, {correctedPath[lastIndex].Z:F3}) -> ({originalEndWithZ.X:F3}, {originalEndWithZ.Y:F3}, {originalEndWithZ.Z:F3})");
|
||
correctedPath[lastIndex] = originalEndWithZ;
|
||
}
|
||
|
||
LogManager.Info($"[坐标修正] 路径起终点坐标修正完成");
|
||
return correctedPath;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"[坐标修正] 修正路径起终点坐标失败: {ex.Message}，使用原始路径");
|
||
return path;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 检查指定点在给定高度约束下是否可通行
|
||
/// </summary>
|
||
/// <param name="point">检查点</param>
|
||
/// <param name="gridMap">网格地图</param>
|
||
/// <param name="vehicleHeight">车辆高度（模型单位）</param>
|
||
/// <returns>是否可通行</returns>
|
||
/// <summary>
|
||
/// 检查指定点在给定高度约束下是否可通行
|
||
/// </summary>
|
||
/// <param name="point">检查点</param>
|
||
/// <param name="gridMap">网格地图</param>
|
||
/// <param name="vehicleHeight">车辆高度（模型单位）</param>
|
||
/// <returns>是否可通行</returns>
|
||
private bool IsPointPassableAtHeight(Point3D point, GridMap gridMap, double vehicleHeight)
|
||
{
|
||
try
|
||
{
|
||
// 将世界坐标转换为网格坐标
|
||
var gridPos = gridMap.WorldToGrid(point);
|
||
int gridX = gridPos.X;
|
||
int gridY = gridPos.Y;
|
||
|
||
// 确保坐标在有效范围内
|
||
if (gridX < 0 || gridX >= gridMap.Width || gridY < 0 || gridY >= gridMap.Height)
|
||
{
|
||
LogManager.Warning($"[高度检查] 网格坐标超出范围：({gridX}, {gridY})，网格大小：{gridMap.Width}x{gridMap.Height}");
|
||
return false;
|
||
}
|
||
|
||
var cell = gridMap.Cells[gridX, gridY];
|
||
// 检查基本可行走性
|
||
if (!cell.IsWalkable)
|
||
{
|
||
LogManager.Info($"[高度检查] 单元格不可通行：({gridX}, {gridY}) - IsWalkable={cell.IsWalkable}, CellType={cell.CellType}, SpeedLimit={cell.SpeedLimit:F2}m/s, 原因：网格标记为不可通行");
|
||
return false;
|
||
}
|
||
|
||
// 检查高度约束
|
||
if (cell.PassableHeight.MaxZ > cell.PassableHeight.MinZ)
|
||
{
|
||
bool spanOk = cell.PassableHeight.GetSpan() >= vehicleHeight;
|
||
// 将绝对坐标转换为相对于网格底面的坐标
|
||
double relativeZ = point.Z - cell.WorldPosition.Z;
|
||
bool containsHeight = cell.PassableHeight.Contains(relativeZ);
|
||
|
||
if (spanOk && containsHeight)
|
||
{
|
||
return true;
|
||
}
|
||
|
||
LogManager.Warning($"[高度检查] ❌ 高度区间不满足要求：({gridX}, {gridY}) - PassableHeight=[{cell.PassableHeight.MinZ:F2}, {cell.PassableHeight.MaxZ:F2}], VehicleHeight={vehicleHeight:F2}, RelativeZ={relativeZ:F2}, SpanOK={spanOk}, ContainsHeight={containsHeight}, 原因：高度约束不满足");
|
||
return false;
|
||
}
|
||
|
||
return false;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"[高度约束检查] 检查点可通行性失败: {ex.Message}");
|
||
return false;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 根据网格高度区间调整路径点Z坐标
|
||
/// </summary>
|
||
/// <param name="path">原始路径</param>
|
||
/// <param name="gridMap">网格地图</param>
|
||
/// <param name="vehicleHeight">车辆高度（模型单位）</param>
|
||
/// <returns>调整后的路径</returns>
|
||
private List<Point3D> ApplyGridHeightConstraints(List<Point3D> path, GridMap gridMap, double vehicleHeight)
|
||
{
|
||
LogManager.Info($"[应用高度区间] 开始调整，路径点数: {path.Count}");
|
||
|
||
var adjustedPath = new List<Point3D>();
|
||
int adjustedCount = 0;
|
||
|
||
foreach (var point in path)
|
||
{
|
||
var gridPos = gridMap.WorldToGrid(point);
|
||
|
||
// 检查网格位置有效性
|
||
if (!gridMap.IsValidGridPosition(gridPos))
|
||
{
|
||
adjustedPath.Add(point);
|
||
continue;
|
||
}
|
||
|
||
var cell = gridMap.Cells[gridPos.X, gridPos.Y];
|
||
|
||
// 检查单个高度区间是否满足车辆高度要求
|
||
if (cell.PassableHeight.GetSpan() >= vehicleHeight)
|
||
{
|
||
// 使用高度区间的底部作为最优Z坐标
|
||
var optimalZ = cell.WorldPosition.Z + cell.PassableHeight.MinZ;
|
||
adjustedPath.Add(new Point3D(point.X, point.Y, optimalZ));
|
||
}
|
||
else
|
||
{
|
||
// 高度不足，保持原始点
|
||
adjustedPath.Add(point);
|
||
}
|
||
adjustedCount++;
|
||
}
|
||
|
||
LogManager.Info($"[应用高度区间] 完成，调整了 {adjustedCount}/{path.Count} 个点");
|
||
return adjustedPath;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将A*返回的米坐标转换为网格坐标（统一方法）
|
||
/// </summary>
|
||
/// <param name="astarPosition">A*位置（米坐标）</param>
|
||
/// <param name="gridMap">网格地图</param>
|
||
/// <returns>网格坐标</returns>
|
||
private GridPoint2D ConvertAStarPositionToGrid(Position astarPosition, GridMap gridMap)
|
||
{
|
||
LogManager.Info($"[A*原始坐标] A*返回: ({astarPosition.X:F3}, {astarPosition.Y:F3})");
|
||
|
||
double cellSizeInMeters = UnitsConverter.ConvertToMeters(gridMap.CellSize);
|
||
int gridX = (int)Math.Round(astarPosition.X / cellSizeInMeters);
|
||
int gridY = (int)Math.Round(astarPosition.Y / cellSizeInMeters);
|
||
|
||
LogManager.Info($"[A*转换坐标] 转换结果: ({gridX}, {gridY})");
|
||
|
||
return new GridPoint2D(gridX, gridY);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 执行A*算法的核心逻辑
|
||
/// </summary>
|
||
/// <param name="start">起点</param>
|
||
/// <param name="end">终点</param>
|
||
/// <param name="gridMap">网格地图</param>
|
||
/// <param name="astarGrid">A*网格</param>
|
||
/// <returns>路径查找结果</returns>
|
||
private PathFindingResult ExecuteAStarAlgorithm(Point3D start, Point3D end, GridMap gridMap, Grid astarGrid)
|
||
{
|
||
try
|
||
{
|
||
// 转换起点和终点到网格坐标
|
||
var startGrid = gridMap.WorldToGrid(start);
|
||
var endGrid = gridMap.WorldToGrid(end);
|
||
|
||
LogManager.Info($"[A*执行] 网格坐标转换 - 起点: ({startGrid.X}, {startGrid.Y}), 终点: ({endGrid.X}, {endGrid.Y})");
|
||
|
||
// 获取单位转换因子
|
||
double cellSizeInMeters = UnitsConverter.ConvertToMeters(gridMap.CellSize);
|
||
|
||
// 正确：直接使用网格索引（RoyT.AStar内部处理米坐标转换）
|
||
var startPos = new GridPosition(startGrid.X, startGrid.Y);
|
||
var endPos = new GridPosition(endGrid.X, endGrid.Y);
|
||
|
||
// 执行A*算法
|
||
var pathfinder = new PathFinder();
|
||
var astarPath = pathfinder.FindPath(startPos, endPos, astarGrid);
|
||
|
||
if (astarPath != null && astarPath.Edges.Count > 0)
|
||
{
|
||
// 转换回世界坐标
|
||
var worldPath = ConvertPathToWorldCoordinates(astarPath, gridMap);
|
||
|
||
// 创建结果对象
|
||
var result = new PathFindingResult
|
||
{
|
||
PathPoints = worldPath,
|
||
OriginalEndPoint = end
|
||
};
|
||
|
||
// 检查路径类型
|
||
if (astarPath.Type == PathType.ClosestApproach)
|
||
{
|
||
LogManager.Info($"[A*执行] 找到部分路径（最近接近），包含 {astarPath.Edges.Count + 1} 个网格点");
|
||
LogManager.Warning($"[A*执行] 无法完全到达目标点，已找到最接近的可达点");
|
||
|
||
// 计算完成百分比
|
||
var lastNode = astarPath.Edges.Last().End;
|
||
var actualEndGrid = ConvertAStarPositionToGrid(lastNode.Position, gridMap);
|
||
var actualEndCell = gridMap.GetCell(actualEndGrid);
|
||
var actualEndWorld = gridMap.GridToWorld3D(actualEndGrid);
|
||
|
||
var originalEndCell = gridMap.GetCell(endGrid);
|
||
var originalEndWorld = gridMap.GridToWorld3D(endGrid);
|
||
var startWorldCell = gridMap.GetCell(startGrid);
|
||
var startWorld = gridMap.GridToWorld3D(startGrid);
|
||
|
||
var totalDistance = CalculateDistance(startWorld, originalEndWorld);
|
||
var actualDistance = CalculateDistance(startWorld, actualEndWorld);
|
||
var completionPercentage = totalDistance > 0 ? (actualDistance / totalDistance) * 100 : 0;
|
||
|
||
// 设置部分路径信息
|
||
result.IsComplete = false;
|
||
result.ActualEndPoint = actualEndWorld;
|
||
result.CompletionPercentage = completionPercentage;
|
||
|
||
LogManager.Info($"[A*执行] 路径完成度: {completionPercentage:F1}% (实际到达点: {actualEndWorld.X:F2}, {actualEndWorld.Y:F2}, {actualEndWorld.Z:F2})");
|
||
}
|
||
else
|
||
{
|
||
LogManager.Info($"[A*执行] 找到完整路径，包含 {astarPath.Edges.Count + 1} 个网格点");
|
||
result.IsComplete = true;
|
||
result.ActualEndPoint = end;
|
||
result.CompletionPercentage = 100.0;
|
||
}
|
||
|
||
return result;
|
||
}
|
||
else
|
||
{
|
||
LogManager.Warning("[A*执行] 未找到路径");
|
||
return new PathFindingResult
|
||
{
|
||
PathPoints = new List<Point3D>(),
|
||
IsComplete = false,
|
||
OriginalEndPoint = end,
|
||
CompletionPercentage = 0.0
|
||
};
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"[A*执行] A*算法执行失败: {ex.Message}");
|
||
throw new AutoPathPlanningException($"A*算法执行失败: {ex.Message}", ex);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证路径中所有点的高度约束（特别检查门网格）
|
||
/// </summary>
|
||
/// <param name="path">路径点列表</param>
|
||
/// <param name="gridMap">网格地图</param>
|
||
/// <param name="vehicleHeight">车辆高度（模型单位）</param>
|
||
private void ValidatePathHeightConstraints(List<Point3D> path, GridMap gridMap, double vehicleHeight)
|
||
{
|
||
LogManager.Info($"[路径高度验证] 开始验证 {path.Count} 个路径点的高度约束");
|
||
|
||
int checkedPoints = 0;
|
||
int passedPoints = 0;
|
||
int failedPoints = 0;
|
||
int doorPoints = 0;
|
||
|
||
for (int i = 0; i < path.Count; i++)
|
||
{
|
||
var point = path[i];
|
||
var gridPos = gridMap.WorldToGrid(point);
|
||
|
||
if (gridMap.IsValidGridPosition(gridPos))
|
||
{
|
||
var cell = gridMap.Cells[gridPos.X, gridPos.Y];
|
||
checkedPoints++;
|
||
|
||
// 特别标记门网格点
|
||
if (cell.CellType == CategoryAttributeManager.LogisticsElementType.门)
|
||
{
|
||
doorPoints++;
|
||
LogManager.Info($"[路径高度验证] 检查路径点{i}（门网格）: ({point.X:F2}, {point.Y:F2}, {point.Z:F2}) -> 网格({gridPos.X},{gridPos.Y})");
|
||
}
|
||
|
||
// 进行高度约束检查
|
||
bool isPassable = IsPointPassableAtHeight(point, gridMap, vehicleHeight);
|
||
if (isPassable)
|
||
{
|
||
passedPoints++;
|
||
}
|
||
else
|
||
{
|
||
failedPoints++;
|
||
LogManager.Warning($"[路径高度验证] ❌ 路径点{i}高度约束检查失败！");
|
||
}
|
||
}
|
||
}
|
||
|
||
LogManager.Info($"[路径高度验证] 完成 - 检查点数: {checkedPoints}, 通过: {passedPoints}, 失败: {failedPoints}, 门网格点: {doorPoints}");
|
||
|
||
if (failedPoints > 0)
|
||
{
|
||
LogManager.Warning($"[路径高度验证] 警告：发现 {failedPoints} 个点不满足高度约束！");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算两点间的3D距离
|
||
/// </summary>
|
||
private double CalculateDistance(Point3D point1, Point3D point2)
|
||
{
|
||
var dx = point2.X - point1.X;
|
||
var dy = point2.Y - point1.Y;
|
||
var dz = point2.Z - point1.Z;
|
||
return Math.Sqrt(dx * dx + dy * dy + dz * dz);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 优化路径（去除共线点等）
|
||
/// </summary>
|
||
/// <param name="pathResult">原始路径查找结果</param>
|
||
/// <param name="gridMap">网格地图</param>
|
||
/// <returns>优化后的路径查找结果</returns>
|
||
private PathFindingResult OptimizePath(PathFindingResult pathResult, GridMap gridMap = null)
|
||
{
|
||
if (pathResult == null || pathResult.PathPoints.Count <= 2)
|
||
{
|
||
return pathResult;
|
||
}
|
||
|
||
try
|
||
{
|
||
// 创建临时路径用于优化
|
||
var tempRoute = new PathRoute("临时优化路径");
|
||
for (int i = 0; i < pathResult.PathPoints.Count; i++)
|
||
{
|
||
var point = pathResult.PathPoints[i];
|
||
var pathPoint = new PathPoint
|
||
{
|
||
Position = point,
|
||
Index = i,
|
||
Type = i == 0 ? PathPointType.StartPoint :
|
||
i == pathResult.PathPoints.Count - 1 ? PathPointType.EndPoint :
|
||
PathPointType.WayPoint,
|
||
Name = $"路径点{i}"
|
||
};
|
||
tempRoute.Points.Add(pathPoint);
|
||
}
|
||
|
||
// 使用基于网格的路径优化算法
|
||
if (gridMap != null)
|
||
{
|
||
// 创建路径优化器并执行优化
|
||
var optimizerConfig = new PathOptimizer.OptimizationConfig
|
||
{
|
||
EnableSimplification = true,
|
||
CollinearTolerance = 0.01
|
||
};
|
||
var optimizer = new PathOptimizer(optimizerConfig);
|
||
var optimizedRoute = optimizer.OptimizePath(tempRoute, gridMap);
|
||
|
||
if (optimizedRoute != null && optimizedRoute.Points.Count > 0)
|
||
{
|
||
// 提取优化后的点位置
|
||
var optimizedPoints = optimizedRoute.Points.Select(p => p.Position).ToList();
|
||
|
||
LogManager.Info($"[路径优化] 点数变化：{pathResult.PathPoints.Count} -> {optimizedPoints.Count}");
|
||
|
||
// 更新路径结果
|
||
pathResult.PathPoints = optimizedPoints;
|
||
|
||
// 🔥 步骤2：在现有优化基础上应用专门的斜线优化
|
||
var diagonalOptimizedPoints = ApplyDiagonalOptimization(optimizedPoints, gridMap);
|
||
|
||
if (diagonalOptimizedPoints.Count != optimizedPoints.Count)
|
||
{
|
||
LogManager.Info($"[斜线优化] 二次优化点数变化：{optimizedPoints.Count} -> {diagonalOptimizedPoints.Count}");
|
||
pathResult.PathPoints = diagonalOptimizedPoints;
|
||
}
|
||
else
|
||
{
|
||
LogManager.Info($"[斜线优化] 斜线优化未能进一步减少点数，保持现有结果");
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
LogManager.Info($"[路径生成] 未提供网格地图，跳过路径优化，点数: {pathResult.PathPoints.Count}");
|
||
}
|
||
|
||
return pathResult;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"[路径优化] 优化失败，使用原始路径: {ex.Message}", ex);
|
||
return pathResult; // 出错时返回原始路径
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 安全优先的A*网格转换方法
|
||
/// 🔥 重写：参照局部直线优先算法的成功模式，重新构建网格连接
|
||
/// </summary>
|
||
private Grid ConvertToAStarGridSafestCenter(GridMap gridMap, ChannelCoverage channelCoverage, double vehicleHeight)
|
||
{
|
||
try
|
||
{
|
||
LogManager.Info($"[安全优先] 开始实施安全优先算法，参照成功的直线优先模式");
|
||
|
||
// 清空日志记录集合，确保每次执行都能看到映射关系
|
||
_loggedDistances.Clear();
|
||
|
||
// 1. 创建网格基础结构（参照局部直线优先）
|
||
double cellSizeInMeters = UnitsConverter.ConvertToMeters(gridMap.CellSize);
|
||
var gridSize = new GridSize(gridMap.Width, gridMap.Height);
|
||
var cellSize = new Size(Distance.FromMeters((float)cellSizeInMeters), Distance.FromMeters((float)cellSizeInMeters));
|
||
|
||
// 使用基础速度创建网格，后续会动态调整
|
||
var baseVelocity = Velocity.FromKilometersPerHour(5); // 基础速度
|
||
var grid = Grid.CreateGridWithLateralConnections(gridSize, cellSize, baseVelocity);
|
||
|
||
LogManager.Info($"[安全优先] A*网格创建完成，尺寸: {gridMap.Width}x{gridMap.Height}");
|
||
|
||
// 2. 🔥 关键步骤：断开所有连接，准备重新按安全距离连接
|
||
LogManager.Info($"[安全优先断开诊断] 开始断开所有节点连接，网格尺寸: {gridMap.Width}x{gridMap.Height}");
|
||
for (int x = 0; x < gridMap.Width; x++)
|
||
{
|
||
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($"[安全优先断开诊断] 所有节点连接断开完成，准备重新构建");
|
||
|
||
// 3. 计算安全距离图
|
||
var safetyDistanceMap = CalculateSafetyDistanceFromGridMap(gridMap);
|
||
LogManager.Info($"[安全优先] 安全距离计算完成");
|
||
|
||
// 4. 智能重连：基于安全距离重新构建连接
|
||
int connectedCells = 0;
|
||
int heightConstrainedCells = 0;
|
||
int safetyConnections = 0;
|
||
var velocityStats = new Dictionary<float, int>();
|
||
|
||
for (int x = 0; x < gridMap.Width; x++)
|
||
{
|
||
for (int y = 0; y < gridMap.Height; y++)
|
||
{
|
||
var cell = gridMap.Cells[x, y];
|
||
|
||
// 检查基本可通行性和高度约束
|
||
bool isPassable = cell.IsWalkable;
|
||
if (isPassable && cell.PassableHeight.MaxZ > cell.PassableHeight.MinZ)
|
||
{
|
||
// vehicleHeight已经是模型单位，直接比较即可
|
||
bool heightOk = cell.PassableHeight.GetSpan() >= vehicleHeight;
|
||
if (!heightOk)
|
||
{
|
||
heightConstrainedCells++;
|
||
isPassable = false;
|
||
}
|
||
}
|
||
|
||
if (isPassable)
|
||
{
|
||
var pos = new GridPosition(x, y);
|
||
var currentSafetyDistance = safetyDistanceMap[x, y];
|
||
connectedCells++;
|
||
|
||
// 🔥 核心改动：右方向连接（基于安全距离）
|
||
if (x + 1 < gridMap.Width)
|
||
{
|
||
var rightCell = gridMap.Cells[x + 1, y];
|
||
if (IsPassableWithHeight(rightCell, vehicleHeight))
|
||
{
|
||
var rightPos = new GridPosition(x + 1, y);
|
||
var rightSafetyDistance = safetyDistanceMap[x + 1, y];
|
||
|
||
// 使用两个位置中更安全的距离
|
||
var maxSafetyDistance = Math.Max(currentSafetyDistance, rightSafetyDistance);
|
||
var speedValue = CalculateSpeedBySafetyDistance(maxSafetyDistance);
|
||
var rightVelocity = Velocity.FromKilometersPerHour(speedValue);
|
||
|
||
// 统计速度分布
|
||
if (velocityStats.ContainsKey(speedValue))
|
||
velocityStats[speedValue]++;
|
||
else
|
||
velocityStats[speedValue] = 1;
|
||
|
||
// 🔥 修复：只添加单向边，避免强制连接障碍物网格
|
||
// 反向边会在处理rightPos时自然添加（如果rightPos也可通行）
|
||
grid.AddEdge(pos, rightPos, rightVelocity);
|
||
safetyConnections++;
|
||
}
|
||
}
|
||
|
||
// 🔥 核心改动：下方向连接（基于安全距离）
|
||
if (y + 1 < gridMap.Height)
|
||
{
|
||
var bottomCell = gridMap.Cells[x, y + 1];
|
||
if (IsPassableWithHeight(bottomCell, vehicleHeight))
|
||
{
|
||
var bottomPos = new GridPosition(x, y + 1);
|
||
var bottomSafetyDistance = safetyDistanceMap[x, y + 1];
|
||
|
||
// 使用两个位置中更安全的距离
|
||
var maxSafetyDistance = Math.Max(currentSafetyDistance, bottomSafetyDistance);
|
||
var speedValue = CalculateSpeedBySafetyDistance(maxSafetyDistance);
|
||
var bottomVelocity = Velocity.FromKilometersPerHour(speedValue);
|
||
|
||
// 统计速度分布
|
||
if (velocityStats.ContainsKey(speedValue))
|
||
velocityStats[speedValue]++;
|
||
else
|
||
velocityStats[speedValue] = 1;
|
||
|
||
// 🔥 修复：只添加单向边，避免强制连接障碍物网格
|
||
// 反向边会在处理bottomPos时自然添加（如果bottomPos也可通行）
|
||
grid.AddEdge(pos, bottomPos, bottomVelocity);
|
||
safetyConnections++;
|
||
}
|
||
}
|
||
|
||
// 🔥 连通性修复：左方向连接（基于安全距离）
|
||
if (x - 1 >= 0)
|
||
{
|
||
var leftCell = gridMap.Cells[x - 1, y];
|
||
if (IsPassableWithHeight(leftCell, vehicleHeight))
|
||
{
|
||
var leftPos = new GridPosition(x - 1, y);
|
||
var leftSafetyDistance = safetyDistanceMap[x - 1, y];
|
||
|
||
// 使用两个位置中更安全的距离
|
||
var maxSafetyDistance = Math.Max(currentSafetyDistance, leftSafetyDistance);
|
||
var speedValue = CalculateSpeedBySafetyDistance(maxSafetyDistance);
|
||
var leftVelocity = Velocity.FromKilometersPerHour(speedValue);
|
||
|
||
// 统计速度分布
|
||
if (velocityStats.ContainsKey(speedValue))
|
||
velocityStats[speedValue]++;
|
||
else
|
||
velocityStats[speedValue] = 1;
|
||
|
||
// 🔥 修复：只添加单向边，避免强制连接障碍物网格
|
||
// 反向边会在处理leftPos时自然添加（如果leftPos也可通行）
|
||
grid.AddEdge(pos, leftPos, leftVelocity);
|
||
safetyConnections++;
|
||
}
|
||
}
|
||
|
||
// 🔥 连通性修复：上方向连接（基于安全距离）
|
||
if (y - 1 >= 0)
|
||
{
|
||
var topCell = gridMap.Cells[x, y - 1];
|
||
if (IsPassableWithHeight(topCell, vehicleHeight))
|
||
{
|
||
var topPos = new GridPosition(x, y - 1);
|
||
var topSafetyDistance = safetyDistanceMap[x, y - 1];
|
||
|
||
// 使用两个位置中更安全的距离
|
||
var maxSafetyDistance = Math.Max(currentSafetyDistance, topSafetyDistance);
|
||
var speedValue = CalculateSpeedBySafetyDistance(maxSafetyDistance);
|
||
var topVelocity = Velocity.FromKilometersPerHour(speedValue);
|
||
|
||
// 统计速度分布
|
||
if (velocityStats.ContainsKey(speedValue))
|
||
velocityStats[speedValue]++;
|
||
else
|
||
velocityStats[speedValue] = 1;
|
||
|
||
// 🔥 修复：只添加单向边，避免强制连接障碍物网格
|
||
// 反向边会在处理topPos时自然添加（如果topPos也可通行）
|
||
grid.AddEdge(pos, topPos, topVelocity);
|
||
safetyConnections++;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
// 输出统计信息
|
||
LogManager.Info($"[安全优先] 网格重建完成，连接统计:");
|
||
LogManager.Info($"[安全优先] - 连接的节点: {connectedCells}");
|
||
LogManager.Info($"[安全优先] - 被高度约束排除: {heightConstrainedCells}");
|
||
LogManager.Info($"[安全优先] - 安全连接数: {safetyConnections}");
|
||
LogManager.Info($"[安全优先] - A*可用率: {(double)connectedCells / (gridMap.Width * gridMap.Height) * 100:F1}%");
|
||
|
||
// 输出速度统计信息
|
||
if (velocityStats.Count > 0)
|
||
{
|
||
var sortedVelocityStats = velocityStats.OrderBy(kv => kv.Key);
|
||
var velocityStatsText = string.Join(", ", sortedVelocityStats.Select(kv => $"{kv.Key:F1}km/h:{kv.Value}条边"));
|
||
LogManager.Info($"[安全优先速度统计] {velocityStatsText}");
|
||
}
|
||
|
||
return grid;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"[安全优先] 网格转换失败: {ex.Message}");
|
||
throw new AutoPathPlanningException($"安全优先网格转换失败: {ex.Message}", ex);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算安全距离图
|
||
/// 直接使用GridMap的CellType，将障碍物和Unknown（边界）都视为不安全区域
|
||
/// </summary>
|
||
private int[,] CalculateSafetyDistanceFromGridMap(GridMap gridMap)
|
||
{
|
||
var distanceMap = new int[gridMap.Width, gridMap.Height];
|
||
|
||
// 统计网格类型分布
|
||
int obstacleCount = 0, unknownCount = 0, walkableCount = 0;
|
||
|
||
// 初始化距离矩阵：障碍物和Unknown=0，可通行=无穷大
|
||
for (int x = 0; x < gridMap.Width; x++)
|
||
{
|
||
for (int y = 0; y < gridMap.Height; y++)
|
||
{
|
||
var cell = gridMap.Cells[x, y];
|
||
// 🔥 核心修改：直接使用IsWalkable判断，所有不可通行的网格都视为障碍物
|
||
bool isUnsafeArea = !cell.IsWalkable;
|
||
|
||
distanceMap[x, y] = isUnsafeArea ? 0 : int.MaxValue;
|
||
|
||
// 统计网格类型
|
||
if (cell.CellType == CategoryAttributeManager.LogisticsElementType.障碍物)
|
||
obstacleCount++;
|
||
else if (cell.CellType == CategoryAttributeManager.LogisticsElementType.Unknown)
|
||
unknownCount++;
|
||
else if (cell.IsWalkable)
|
||
walkableCount++;
|
||
}
|
||
}
|
||
|
||
LogManager.Info($"[安全距离计算] 网格类型统计: 障碍物={obstacleCount}, Unknown(边界)={unknownCount}, 可通行={walkableCount}, 总计={gridMap.Width * gridMap.Height}");
|
||
LogManager.Info($"[安全距离计算] 不安全区域(障碍物+边界)={obstacleCount + unknownCount}, 占比={(double)(obstacleCount + unknownCount) / (gridMap.Width * gridMap.Height) * 100:F1}%");
|
||
|
||
// 正向扫描（从左上到右下）
|
||
for (int x = 0; x < gridMap.Width; x++)
|
||
{
|
||
for (int y = 0; y < gridMap.Height; y++)
|
||
{
|
||
if (distanceMap[x, y] == int.MaxValue)
|
||
{
|
||
int minDist = int.MaxValue;
|
||
|
||
if (x > 0 && distanceMap[x - 1, y] != int.MaxValue)
|
||
minDist = Math.Min(minDist, distanceMap[x - 1, y] + 1);
|
||
if (y > 0 && distanceMap[x, y - 1] != int.MaxValue)
|
||
minDist = Math.Min(minDist, distanceMap[x, y - 1] + 1);
|
||
if (x > 0 && y > 0 && distanceMap[x - 1, y - 1] != int.MaxValue)
|
||
minDist = Math.Min(minDist, distanceMap[x - 1, y - 1] + 1);
|
||
if (x > 0 && y < gridMap.Height - 1 && distanceMap[x - 1, y + 1] != int.MaxValue)
|
||
minDist = Math.Min(minDist, distanceMap[x - 1, y + 1] + 1);
|
||
|
||
if (minDist != int.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 && distanceMap[x, y] != int.MaxValue)
|
||
{
|
||
int minDist = distanceMap[x, y];
|
||
|
||
if (x < gridMap.Width - 1 && distanceMap[x + 1, y] != int.MaxValue)
|
||
minDist = Math.Min(minDist, distanceMap[x + 1, y] + 1);
|
||
if (y < gridMap.Height - 1 && distanceMap[x, y + 1] != int.MaxValue)
|
||
minDist = Math.Min(minDist, distanceMap[x, y + 1] + 1);
|
||
if (x < gridMap.Width - 1 && y < gridMap.Height - 1 && distanceMap[x + 1, y + 1] != int.MaxValue)
|
||
minDist = Math.Min(minDist, distanceMap[x + 1, y + 1] + 1);
|
||
if (x < gridMap.Width - 1 && y > 0 && distanceMap[x + 1, y - 1] != int.MaxValue)
|
||
minDist = Math.Min(minDist, distanceMap[x + 1, y - 1] + 1);
|
||
|
||
distanceMap[x, y] = minDist;
|
||
}
|
||
else if (distanceMap[x, y] == int.MaxValue)
|
||
{
|
||
int minDist = int.MaxValue;
|
||
|
||
if (x < gridMap.Width - 1 && distanceMap[x + 1, y] != int.MaxValue)
|
||
minDist = Math.Min(minDist, distanceMap[x + 1, y] + 1);
|
||
if (y < gridMap.Height - 1 && distanceMap[x, y + 1] != int.MaxValue)
|
||
minDist = Math.Min(minDist, distanceMap[x, y + 1] + 1);
|
||
if (x < gridMap.Width - 1 && y < gridMap.Height - 1 && distanceMap[x + 1, y + 1] != int.MaxValue)
|
||
minDist = Math.Min(minDist, distanceMap[x + 1, y + 1] + 1);
|
||
if (x < gridMap.Width - 1 && y > 0 && distanceMap[x + 1, y - 1] != int.MaxValue)
|
||
minDist = Math.Min(minDist, distanceMap[x + 1, y - 1] + 1);
|
||
|
||
if (minDist != int.MaxValue)
|
||
distanceMap[x, y] = minDist;
|
||
}
|
||
}
|
||
}
|
||
|
||
// 统计安全距离分布
|
||
var distanceHistogram = new Dictionary<int, int>();
|
||
int totalWalkableCells = 0;
|
||
int totalDistanceSum = 0;
|
||
|
||
for (int x = 0; x < gridMap.Width; x++)
|
||
{
|
||
for (int y = 0; y < gridMap.Height; y++)
|
||
{
|
||
var distance = distanceMap[x, y];
|
||
if (distance != int.MaxValue && distance > 0)
|
||
{
|
||
totalWalkableCells++;
|
||
totalDistanceSum += distance;
|
||
|
||
// 分组统计：1,2,3,4,5-9,10+
|
||
int distanceGroup;
|
||
if (distance <= 4)
|
||
distanceGroup = distance;
|
||
else if (distance <= 9)
|
||
distanceGroup = 5; // 代表5-9
|
||
else
|
||
distanceGroup = 10; // 代表10+
|
||
|
||
if (distanceHistogram.ContainsKey(distanceGroup))
|
||
distanceHistogram[distanceGroup]++;
|
||
else
|
||
distanceHistogram[distanceGroup] = 1;
|
||
}
|
||
}
|
||
}
|
||
|
||
if (totalWalkableCells > 0)
|
||
{
|
||
double averageDistance = (double)totalDistanceSum / totalWalkableCells;
|
||
var sortedStats = distanceHistogram.OrderBy(kv => kv.Key);
|
||
var statsText = string.Join(", ", sortedStats.Select(kv =>
|
||
kv.Key <= 4 ? $"距离{kv.Key}:{kv.Value}格" :
|
||
kv.Key == 5 ? $"距离5-9:{kv.Value}格" :
|
||
$"距离10+:{kv.Value}格"
|
||
));
|
||
|
||
LogManager.Info($"[安全距离分布] {statsText}");
|
||
LogManager.Info($"[安全距离分析] 平均距离: {averageDistance:F2}格, 可通行网格总数: {totalWalkableCells}");
|
||
}
|
||
|
||
return distanceMap;
|
||
}
|
||
|
||
|
||
// 静态集合用于记录已打印的速度映射，避免重复日志
|
||
private static readonly HashSet<int> _loggedDistances = new HashSet<int>();
|
||
|
||
/// <summary>
|
||
/// 根据障碍物距离计算安全速度值（浮点数）
|
||
/// </summary>
|
||
private float CalculateSpeedBySafetyDistance(int safetyDistance)
|
||
{
|
||
float finalSpeed;
|
||
|
||
// 🔧 温和映射：参考局部直线优先算法，使用5-35km/h范围，减少网格敏感性
|
||
if (safetyDistance >= 5)
|
||
finalSpeed = 35.0f; // 35 km/h（最高速度，安全区域）
|
||
else if (safetyDistance >= 3)
|
||
finalSpeed = 25.0f; // 25 km/h
|
||
else if (safetyDistance >= 2)
|
||
finalSpeed = 15.0f; // 15 km/h
|
||
else if (safetyDistance >= 1)
|
||
finalSpeed = 10.0f; // 10 km/h
|
||
else
|
||
finalSpeed = 5.0f; // 5 km/h（最低速度，但不是极端值）
|
||
|
||
// 只在每种距离第一次出现时记录速度映射
|
||
if (!_loggedDistances.Contains(safetyDistance))
|
||
{
|
||
_loggedDistances.Add(safetyDistance);
|
||
}
|
||
|
||
return finalSpeed;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取GridMap缓存统计信息
|
||
/// </summary>
|
||
/// <returns>缓存统计报告</returns>
|
||
public static string GetGridMapCacheStatistics()
|
||
{
|
||
return GridMapGenerator.GetCacheStatistics();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取GridMap缓存详细统计报告
|
||
/// </summary>
|
||
/// <returns>详细统计报告</returns>
|
||
public static string GetDetailedGridMapCacheReport()
|
||
{
|
||
return GridMapGenerator.GetDetailedCacheReport();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 清除GridMap缓存
|
||
/// </summary>
|
||
public static void ClearGridMapCache()
|
||
{
|
||
GridMapGenerator.ClearCache();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 记录路径规划完成时的缓存统计
|
||
/// </summary>
|
||
private void LogCacheStatistics()
|
||
{
|
||
var stats = GlobalGridMapCache.Instance.Statistics;
|
||
if (stats.HitCount + stats.MissCount > 0)
|
||
{
|
||
LogManager.Info($"[GridMap缓存统计] {stats.GenerateReport()}");
|
||
}
|
||
}
|
||
}
|
||
} |