3191 lines
153 KiB
C#
3191 lines
153 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;
|
||
using NavisworksTransport.Core.Config;
|
||
using NavisworksTransport.Utils.CoordinateSystem;
|
||
|
||
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>
|
||
/// 路径查找是否成功
|
||
/// </summary>
|
||
public bool IsSuccess => PathPoints.Count >= 2;
|
||
|
||
/// <summary>
|
||
/// 失败消息
|
||
/// </summary>
|
||
public string ErrorMessage { get; set; }
|
||
|
||
/// <summary>
|
||
/// 创建失败结果
|
||
/// </summary>
|
||
public static PathFindingResult Failure(string errorMessage)
|
||
{
|
||
return new PathFindingResult
|
||
{
|
||
PathPoints = new List<Point3D>(),
|
||
IsComplete = false,
|
||
CompletionPercentage = 0.0,
|
||
ErrorMessage = errorMessage
|
||
};
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 自动路径查找器
|
||
/// 基于A*算法的路径规划核心类
|
||
/// 支持传统模式和真3D图路径规划
|
||
/// </summary>
|
||
public class AutoPathFinder
|
||
{
|
||
/// <summary>
|
||
/// 缓坡速度系数 - 当坡度在 3cm 到 25cm 之间时使用
|
||
/// </summary>
|
||
private const float SLOPE_SPEED_FACTOR_GENTLE = 0.8f;
|
||
|
||
/// <summary>
|
||
/// 楼梯速度系数 - 当坡度在 25cm 到 50cm 之间时使用
|
||
/// </summary>
|
||
private const float SLOPE_SPEED_FACTOR_STAIR = 0.5f;
|
||
|
||
/// <summary>
|
||
/// 陡坡速度系数 - 当坡度超过 50cm 时使用
|
||
/// </summary>
|
||
private const float SLOPE_SPEED_FACTOR_STEEP = 0.1f;
|
||
|
||
/// <summary>
|
||
/// A*高度惩罚因子 - 用于计算距离目标高度的速度扣减
|
||
/// </summary>
|
||
private const double HEIGHT_PENALTY_FACTOR = 0.1;
|
||
|
||
/// <summary>
|
||
/// 转弯角度阈值（弧度）- 用于检测路径转弯
|
||
/// </summary>
|
||
private const double TURN_ANGLE_THRESHOLD_RADIANS = 0.1;
|
||
|
||
/// <summary>
|
||
/// 获取水平边连接允许的最大高度差（米）
|
||
/// 从配置文件读取，足以支持台阶连接（约0.15米）和地面到楼梯底部的连接（约0.3米）
|
||
/// 使用时需转换为模型单位
|
||
/// </summary>
|
||
private double GetMaxHeightDiffMeters()
|
||
{
|
||
return ConfigManager.Instance.Current.PathEditing.MaxHeightDiffMeters;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 3D节点信息（存储每个A*节点对应的3D坐标和类型）
|
||
/// </summary>
|
||
private class Node3DInfo
|
||
{
|
||
public int X { get; set; }
|
||
public int Y { get; set; }
|
||
public int LayerIndex { get; set; }
|
||
public double Z { get; set; }
|
||
public string CellType { get; set; }
|
||
}
|
||
|
||
/// <summary>
|
||
/// 3D图构建结果
|
||
/// </summary>
|
||
private class Graph3DResult
|
||
{
|
||
/// <summary>
|
||
/// 节点字典：(x, y, layerIndex) -> A*节点
|
||
/// </summary>
|
||
public Dictionary<(int x, int y, int layerIndex), Roy_T.AStar.Graphs.Node> Nodes { get; set; }
|
||
|
||
/// <summary>
|
||
/// 节点信息字典：A*节点 -> 3D信息
|
||
/// </summary>
|
||
public Dictionary<Roy_T.AStar.Graphs.Node, Node3DInfo> NodeInfo { get; set; }
|
||
|
||
/// <summary>
|
||
/// 所有节点列表（用于查找最近节点）
|
||
/// </summary>
|
||
public List<Roy_T.AStar.Graphs.Node> AllNodes { get; set; }
|
||
|
||
/// <summary>
|
||
/// 最大速度
|
||
/// </summary>
|
||
public Velocity MaxVelocity { get; set; }
|
||
}
|
||
|
||
/// <summary>
|
||
/// 真3D路径规划（使用通道覆盖数据、高度约束和路径策略）
|
||
/// </summary>
|
||
public PathFindingResult FindPath(Point3D start, Point3D end, GridMap gridMap, ChannelCoverage channelCoverage, double objectHeight, PathStrategy strategy = PathStrategy.Shortest)
|
||
{
|
||
try
|
||
{
|
||
LogManager.Info("[3D路径规划] 开始执行");
|
||
|
||
// 构建3D图
|
||
var graph3D = BuildGraphWithStrategy(gridMap, channelCoverage, objectHeight, start, end, strategy);
|
||
|
||
// 执行A*搜索
|
||
var pathResult = ExecuteAStarOnGraph(start, end, gridMap, graph3D);
|
||
|
||
// 优化路径
|
||
pathResult = OptimizePath(pathResult, gridMap);
|
||
|
||
return pathResult;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"[3D路径规划] 发生异常: {ex.Message}\n{ex.StackTrace}");
|
||
throw; // 直接抛出异常，不返回默认路径
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 速度计算委托：根据当前节点、邻居节点和其他上下文信息计算边的速度
|
||
/// </summary>
|
||
private delegate float SpeedCalculator(
|
||
int currentX, int currentY, int currentLayerIndex, double currentZ,
|
||
int neighborX, int neighborY, int neighborLayerIndex, double neighborZ,
|
||
double heightDiff, double baseSpeed, double maxHeightDiff,
|
||
Point3D startPos, Point3D endPos);
|
||
|
||
/// <summary>
|
||
/// 构建3D图（每个(x,y,layerIndex)作为独立节点）
|
||
/// </summary>
|
||
private Graph3DResult BuildGraph3D(GridMap gridMap, ChannelCoverage channelCoverage, double objectHeight, Point3D startPos, Point3D endPos, SpeedCalculator speedCalculator = null)
|
||
{
|
||
LogManager.Info("[3D图构建] 开始构建真3D图");
|
||
|
||
var nodes = new Dictionary<(int x, int y, int layerIndex), Roy_T.AStar.Graphs.Node>();
|
||
var nodeInfo = new Dictionary<Roy_T.AStar.Graphs.Node, Node3DInfo>();
|
||
var allNodes = new List<Roy_T.AStar.Graphs.Node>();
|
||
|
||
double baseSpeed = 5.0; // km/h
|
||
float maxSpeed = (float)baseSpeed;
|
||
|
||
// 使用配置的模型单位值
|
||
double maxHeightDiff = ConfigManager.Instance.Current.PathEditing.MaxHeightDiff;
|
||
LogManager.Info($"[3D图构建] 高度差阈值: {ConfigManager.Instance.Current.PathEditing.MaxHeightDiffMeters}米 = {maxHeightDiff:F2}模型单位");
|
||
|
||
// === 阶段1：创建所有3D节点 ===
|
||
int totalNodesCreated = 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.HasAnyWalkableLayer())
|
||
continue;
|
||
|
||
// 为每个高度层创建独立节点
|
||
for (int li = 0; li < cell.HeightLayers.Count; li++)
|
||
{
|
||
var layer = cell.HeightLayers[li];
|
||
|
||
// 检查该层是否可通行（膨胀影响）
|
||
if (!layer.IsWalkable)
|
||
continue;
|
||
|
||
// 楼梯/电梯区域跳过层0（下方楼板，不是可行走表面）
|
||
if (li == 0 && (layer.Type == "楼梯" ||
|
||
layer.Type == "电梯"))
|
||
{
|
||
continue;
|
||
}
|
||
|
||
if (layer.PassableHeight.GetSpan() < objectHeight)
|
||
continue;
|
||
|
||
// 使用2D位置（用于A*启发式）
|
||
var position = new Position(x, y);
|
||
var node = new Roy_T.AStar.Graphs.Node(position);
|
||
|
||
nodes[(x, y, li)] = node;
|
||
nodeInfo[node] = new Node3DInfo
|
||
{
|
||
X = x,
|
||
Y = y,
|
||
LayerIndex = li,
|
||
Z = layer.Z,
|
||
CellType = layer.Type
|
||
};
|
||
allNodes.Add(node);
|
||
totalNodesCreated++;
|
||
}
|
||
}
|
||
}
|
||
|
||
LogManager.Info($"[3D图构建] 阶段1完成：创建了 {totalNodesCreated} 个3D节点");
|
||
|
||
// === 阶段2：创建水平边（同层的相邻节点，高度差在阈值内）===
|
||
var directions = new[] { (1, 0), (-1, 0), (0, 1), (0, -1) };
|
||
int horizontalEdges = 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.HasAnyWalkableLayer())
|
||
continue;
|
||
|
||
for (int li = 0; li < cell.HeightLayers.Count; li++)
|
||
{
|
||
var currentLayer = cell.HeightLayers[li];
|
||
if (currentLayer.PassableHeight.GetSpan() < objectHeight)
|
||
continue;
|
||
|
||
if (!nodes.TryGetValue((x, y, li), out var currentNode))
|
||
continue;
|
||
|
||
double currentZ = currentLayer.Z;
|
||
|
||
foreach (var (dx, dy) in directions)
|
||
{
|
||
int nx = x + dx;
|
||
int ny = y + dy;
|
||
|
||
if (nx < 0 || nx >= gridMap.Width || ny < 0 || ny >= gridMap.Height)
|
||
continue;
|
||
|
||
var neighborCell = gridMap.Cells[nx, ny];
|
||
if (!neighborCell.HasAnyWalkableLayer())
|
||
continue;
|
||
|
||
// 查找邻居中所有满足高度约束的层
|
||
for (int nli = 0; nli < neighborCell.HeightLayers.Count; nli++)
|
||
{
|
||
var neighborLayer = neighborCell.HeightLayers[nli];
|
||
if (neighborLayer.PassableHeight.GetSpan() < objectHeight)
|
||
continue;
|
||
|
||
double heightDiff = Math.Abs(neighborLayer.Z - currentZ);
|
||
if (heightDiff > maxHeightDiff)
|
||
continue;
|
||
|
||
if (!nodes.TryGetValue((nx, ny, nli), out var neighborNode))
|
||
continue;
|
||
|
||
// 使用策略特定的速度计算器，如果未提供则使用默认的高度差计算
|
||
float speed;
|
||
if (speedCalculator != null)
|
||
{
|
||
speed = speedCalculator(x, y, li, currentZ, nx, ny, nli, neighborLayer.Z,
|
||
heightDiff, baseSpeed, maxHeightDiff, startPos, endPos);
|
||
}
|
||
else
|
||
{
|
||
speed = CalculateSpeedByHeightDiff(heightDiff, baseSpeed, maxHeightDiff);
|
||
}
|
||
|
||
maxSpeed = Math.Max(maxSpeed, speed);
|
||
var velocity = Velocity.FromKilometersPerHour(speed);
|
||
|
||
currentNode.Connect(neighborNode, velocity);
|
||
horizontalEdges++;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
LogManager.Info($"[3D图构建] 阶段2完成：创建了 {horizontalEdges} 条水平边");
|
||
|
||
// === 阶段3：创建垂直边（仅在楼梯/电梯内部的相邻层）===
|
||
int verticalEdges = 0;
|
||
|
||
for (int x2 = 0; x2 < gridMap.Width; x2++)
|
||
{
|
||
for (int y2 = 0; y2 < gridMap.Height; y2++)
|
||
{
|
||
var cell = gridMap.Cells[x2, y2];
|
||
|
||
// 检查该网格是否有任何楼梯或电梯层（允许垂直移动）
|
||
bool hasStairsOrElevator = cell.HeightLayers.Any(layer =>
|
||
layer.Type == "楼梯" ||
|
||
layer.Type == "电梯");
|
||
|
||
if (!hasStairsOrElevator || cell.HeightLayers.Count < 2)
|
||
continue;
|
||
|
||
// 连接同一(x,y)位置的不同高度层（相邻层之间）
|
||
for (int li = 0; li < cell.HeightLayers.Count - 1; li++)
|
||
{
|
||
var layer1 = cell.HeightLayers[li];
|
||
var layer2 = cell.HeightLayers[li + 1];
|
||
|
||
// 垂直边只连接相同类型的层（楼梯到楼梯，电梯到电梯）
|
||
// 不允许从通道跳到楼梯，必须通过水平边进入楼梯
|
||
if (layer1.Type != layer2.Type)
|
||
continue;
|
||
|
||
// 垂直边只在楼梯/电梯类型之间创建
|
||
if (layer1.Type != "楼梯" &&
|
||
layer1.Type != "电梯")
|
||
continue;
|
||
|
||
if (layer1.PassableHeight.GetSpan() < objectHeight ||
|
||
layer2.PassableHeight.GetSpan() < objectHeight)
|
||
continue;
|
||
|
||
double heightDiff = Math.Abs(layer2.Z - layer1.Z);
|
||
if (heightDiff > maxHeightDiff)
|
||
continue;
|
||
|
||
if (!nodes.TryGetValue((x2, y2, li), out var node1) ||
|
||
!nodes.TryGetValue((x2, y2, li + 1), out var node2))
|
||
continue;
|
||
|
||
// 垂直移动速度：电梯较快，楼梯较慢
|
||
float verticalSpeed = layer1.Type == "电梯"
|
||
? (float)(baseSpeed * 0.5)
|
||
: (float)(baseSpeed * 0.3);
|
||
maxSpeed = Math.Max(maxSpeed, verticalSpeed);
|
||
var velocity = Velocity.FromKilometersPerHour(verticalSpeed);
|
||
|
||
// 双向连接
|
||
node1.Connect(node2, velocity);
|
||
node2.Connect(node1, velocity);
|
||
verticalEdges += 2;
|
||
}
|
||
}
|
||
}
|
||
|
||
LogManager.Info($"[3D图构建] 阶段3完成：创建了 {verticalEdges} 条垂直边（双向）");
|
||
LogManager.Info($"[3D图构建] 总结：{totalNodesCreated}个节点，{horizontalEdges + verticalEdges}条边");
|
||
|
||
return new Graph3DResult
|
||
{
|
||
Nodes = nodes,
|
||
NodeInfo = nodeInfo,
|
||
AllNodes = allNodes,
|
||
MaxVelocity = Velocity.FromKilometersPerHour(maxSpeed)
|
||
};
|
||
}
|
||
|
||
/// <summary>
|
||
/// 根据高度差计算速度（高度差单位：模型单位）
|
||
/// </summary>
|
||
/// <param name="heightDiff">高度差（模型单位）</param>
|
||
/// <param name="baseSpeed">基础速度（km/h）</param>
|
||
/// <param name="maxHeightDiff">最大允许高度差（模型单位，对应0.5米）</param>
|
||
private float CalculateSpeedByHeightDiff(double heightDiff, double baseSpeed, double maxHeightDiff)
|
||
{
|
||
// 计算相对高度差比例
|
||
double heightRatio = heightDiff / maxHeightDiff;
|
||
|
||
if (heightRatio < 0.06) return (float)baseSpeed; // 平地 (<3cm，即<0.06*0.5m)
|
||
if (heightRatio <= 0.5) return (float)(baseSpeed * SLOPE_SPEED_FACTOR_GENTLE); // 缓坡 (≤0.25m)
|
||
if (heightRatio <= 1.0) return (float)(baseSpeed * SLOPE_SPEED_FACTOR_STAIR); // 楼梯 (≤0.5m)
|
||
return (float)(baseSpeed * SLOPE_SPEED_FACTOR_STEEP); // 过陡 (>0.5m)
|
||
}
|
||
|
||
/// <summary>
|
||
/// 根据策略构建图
|
||
/// </summary>
|
||
private Graph3DResult BuildGraphWithStrategy(GridMap gridMap, ChannelCoverage channelCoverage, double objectHeight, Point3D startPos, Point3D endPos, PathStrategy strategy)
|
||
{
|
||
SpeedCalculator speedCalc = null;
|
||
|
||
switch (strategy)
|
||
{
|
||
case PathStrategy.Shortest:
|
||
LogManager.Info($"[3D图构建] 使用最短路径策略");
|
||
// 默认策略，speedCalc = null，使用高度差计算
|
||
break;
|
||
|
||
case PathStrategy.Straightest:
|
||
LogManager.Info($"[3D图构建] 使用直线优先策略");
|
||
// 直线优先：计算沿边方向能够直走的距离，距离越长速度越高
|
||
{
|
||
speedCalc = (cx, cy, cli, cz, nx, ny, nli, nz, heightDiff, baseSpeed, maxHeightDiff, start, end) =>
|
||
{
|
||
// 基础速度（根据高度差）
|
||
float baseSpeedValue = CalculateSpeedByHeightDiff(heightDiff, baseSpeed, maxHeightDiff);
|
||
|
||
// 计算边的方向
|
||
var direction = new GridPoint2D(nx - cx, ny - cy);
|
||
var currentPos = new GridPoint2D(cx, cy);
|
||
|
||
// 计算沿该方向能够直走的距离
|
||
int straightDistance = CalculateStraightDistance(currentPos, direction, gridMap, objectHeight);
|
||
|
||
// 直走距离越长，速度加成越高（最多+30 km/h）
|
||
float bonusSpeed = Math.Min(straightDistance * 0.5f, 30.0f);
|
||
float finalSpeed = baseSpeedValue + bonusSpeed;
|
||
|
||
return finalSpeed;
|
||
};
|
||
}
|
||
break;
|
||
|
||
case PathStrategy.SafestCenter:
|
||
LogManager.Info($"[3D图构建] 使用安全优先策略");
|
||
// 安全优先：计算安全距离图，基于到障碍物的距离调整速度
|
||
{
|
||
var safetyDistanceMap = CalculateSafetyDistanceFromGridMap(gridMap);
|
||
LogManager.Info($"[3D图构建] 安全距离图计算完成");
|
||
|
||
speedCalc = (cx, cy, cli, cz, nx, ny, nli, nz, heightDiff, baseSpeed, maxHeightDiff, start, end) =>
|
||
{
|
||
// 基础速度（根据高度差）
|
||
float baseSpeedValue = CalculateSpeedByHeightDiff(heightDiff, baseSpeed, maxHeightDiff);
|
||
|
||
// 获取两个节点的安全距离，取较大值（更安全的距离）
|
||
int currentSafety = safetyDistanceMap[cx, cy];
|
||
int neighborSafety = safetyDistanceMap[nx, ny];
|
||
int maxSafety = Math.Max(currentSafety, neighborSafety);
|
||
|
||
// 根据安全距离计算速度系数
|
||
float safetySpeed = CalculateSpeedBySafetyDistance(maxSafety);
|
||
|
||
// 返回基础速度和安全速度的加权组合
|
||
// 高度差影响基础速度，安全距离影响额外速度
|
||
return baseSpeedValue * 0.3f + safetySpeed * 0.7f;
|
||
};
|
||
}
|
||
break;
|
||
|
||
default:
|
||
LogManager.Warning($"[3D图构建] 未知策略 {strategy}，使用默认最短路径");
|
||
break;
|
||
}
|
||
|
||
return BuildGraph3D(gridMap, channelCoverage, objectHeight, startPos, endPos, speedCalc);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 在3D图上执行A*搜索
|
||
/// </summary>
|
||
private PathFindingResult ExecuteAStarOnGraph(Point3D start, Point3D end, GridMap gridMap, Graph3DResult graph3D)
|
||
{
|
||
var startGridPos = gridMap.WorldToGrid(start);
|
||
var endGridPos = gridMap.WorldToGrid(end);
|
||
|
||
// 找到起点和终点最近的3D节点
|
||
double startElevation = gridMap.GetWorldElevation(start);
|
||
double endElevation = gridMap.GetWorldElevation(end);
|
||
|
||
var startNode = FindClosest3DNode(graph3D, startGridPos, startElevation);
|
||
var endNode = FindClosest3DNode(graph3D, endGridPos, endElevation);
|
||
|
||
// 起点和终点必须检查可达性
|
||
bool startOnObstacle = (startNode == null);
|
||
bool endOnObstacle = (endNode == null);
|
||
|
||
// 如果起点在障碍物上，返回失败结果（正常业务流程，不记录错误）
|
||
if (startOnObstacle && endOnObstacle)
|
||
{
|
||
LogManager.Info($"[A*执行-3D] 起点网格({startGridPos.X},{startGridPos.Y})和终点网格({endGridPos.X},{endGridPos.Y})都在不可通行区域");
|
||
return PathFindingResult.Failure("起点和终点都在不可通行区域，请选择可通行的位置");
|
||
}
|
||
else if (startOnObstacle)
|
||
{
|
||
LogManager.Info($"[A*执行-3D] 起点网格({startGridPos.X},{startGridPos.Y})在不可通行区域");
|
||
return PathFindingResult.Failure("起点在不可通行区域，请选择可通行的位置");
|
||
}
|
||
|
||
// 如果终点不可达，寻找最接近终点的可达节点（类似2.5D的ClosestApproach逻辑）
|
||
if (endOnObstacle)
|
||
{
|
||
LogManager.Warning($"[A*执行-3D] 终点网格({endGridPos.X},{endGridPos.Y})不可达，寻找最接近的可达节点");
|
||
|
||
// 从所有节点中找到最接近终点的节点
|
||
double minDistance = double.MaxValue;
|
||
foreach (var node in graph3D.AllNodes)
|
||
{
|
||
var info = graph3D.NodeInfo[node];
|
||
var nodeWorldPos = gridMap.CreateWorldPoint(new GridPoint2D(info.X, info.Y), info.Z);
|
||
|
||
double distance = Math.Sqrt(
|
||
Math.Pow(nodeWorldPos.X - end.X, 2) +
|
||
Math.Pow(nodeWorldPos.Y - end.Y, 2) +
|
||
Math.Pow(nodeWorldPos.Z - end.Z, 2)
|
||
);
|
||
|
||
if (distance < minDistance)
|
||
{
|
||
minDistance = distance;
|
||
endNode = node;
|
||
}
|
||
}
|
||
|
||
if (endNode != null)
|
||
{
|
||
var closestInfo = graph3D.NodeInfo[endNode];
|
||
LogManager.Info($"[A*执行-3D] 找到最接近终点的可达节点: ({closestInfo.X},{closestInfo.Y},层{closestInfo.LayerIndex},Z={closestInfo.Z:F2})，距离原终点 {minDistance:F2}模型单位");
|
||
}
|
||
else
|
||
{
|
||
LogManager.Warning($"[A*执行-3D] 无法找到任何可达节点（图中没有任何节点）");
|
||
return PathFindingResult.Failure("无法找到任何可达节点，网格地图可能完全被障碍物覆盖");
|
||
}
|
||
}
|
||
|
||
var startInfo = graph3D.NodeInfo[startNode];
|
||
var endInfo = graph3D.NodeInfo[endNode];
|
||
LogManager.Info($"[A*执行-3D] 起点映射: ({startGridPos.X},{startGridPos.Y},高程={startElevation:F2}) -> 节点({startInfo.X},{startInfo.Y},层{startInfo.LayerIndex},高程={startInfo.Z:F2})");
|
||
LogManager.Info($"[A*执行-3D] 终点映射: ({endGridPos.X},{endGridPos.Y},高程={endElevation:F2}) -> 节点({endInfo.X},{endInfo.Y},层{endInfo.LayerIndex},高程={endInfo.Z:F2})");
|
||
|
||
// 执行A*
|
||
var pathfinder = new Roy_T.AStar.Paths.PathFinder();
|
||
var astarPath = pathfinder.FindPath(startNode, endNode, graph3D.MaxVelocity);
|
||
|
||
if (astarPath.Edges.Count == 0)
|
||
{
|
||
LogManager.Warning($"[A*执行-3D] 未找到路径，起点({startInfo.X},{startInfo.Y})到终点({endInfo.X},{endInfo.Y})之间被障碍物完全阻断");
|
||
return PathFindingResult.Failure("起点和终点之间被障碍物完全阻断，无法找到可行路径");
|
||
}
|
||
|
||
// 转换为世界坐标路径
|
||
var path3D = ConvertGraphPathToWorld(astarPath, graph3D, gridMap, start, end);
|
||
|
||
// 计算路径完成度，检查是否到达终点
|
||
double completionPercentage = 100.0;
|
||
bool isComplete = true;
|
||
Point3D actualEnd = path3D.Count > 0 ? path3D[path3D.Count - 1] : start;
|
||
|
||
// 如果终点在障碍物上，路径肯定不完整
|
||
if (endOnObstacle)
|
||
{
|
||
isComplete = false;
|
||
actualEnd = path3D[path3D.Count - 1];
|
||
|
||
// 计算实际距离和总距离
|
||
double totalDistance = Math.Sqrt(
|
||
Math.Pow(end.X - start.X, 2) +
|
||
Math.Pow(end.Y - start.Y, 2) +
|
||
Math.Pow(end.Z - start.Z, 2)
|
||
);
|
||
double actualDistance = Math.Sqrt(
|
||
Math.Pow(actualEnd.X - start.X, 2) +
|
||
Math.Pow(actualEnd.Y - start.Y, 2) +
|
||
Math.Pow(actualEnd.Z - start.Z, 2)
|
||
);
|
||
|
||
completionPercentage = totalDistance > 0 ? (actualDistance / totalDistance) * 100 : 0;
|
||
|
||
LogManager.Warning($"[3D路径规划] 终点在障碍物上，使用最接近的可达节点，路径不完整：完成度 {completionPercentage:F1}%");
|
||
}
|
||
else
|
||
{
|
||
// 检查A*路径最后一个节点是否到达终点网格
|
||
if (astarPath.Edges.Count > 0)
|
||
{
|
||
var lastEdge = astarPath.Edges[astarPath.Edges.Count - 1];
|
||
var lastNode = lastEdge.End as Roy_T.AStar.Graphs.Node;
|
||
|
||
if (lastNode != null && graph3D.NodeInfo.ContainsKey(lastNode))
|
||
{
|
||
var lastInfo = graph3D.NodeInfo[lastNode];
|
||
|
||
// 检查最后节点是否到达终点（网格坐标和高度层都要匹配）
|
||
if (lastInfo.X == endGridPos.X && lastInfo.Y == endGridPos.Y && lastInfo.LayerIndex == endInfo.LayerIndex)
|
||
{
|
||
// 到达终点网格和高度层，路径完整
|
||
isComplete = true;
|
||
actualEnd = path3D[path3D.Count - 1];
|
||
completionPercentage = 100.0;
|
||
LogManager.Info($"[3D路径规划] 找到完整路径，到达终点网格({endGridPos.X},{endGridPos.Y}) Layer{endInfo.LayerIndex}");
|
||
}
|
||
else
|
||
{
|
||
// 未到达终点，计算完成度
|
||
isComplete = false;
|
||
actualEnd = path3D[path3D.Count - 1];
|
||
|
||
// 计算实际距离和总距离
|
||
double totalDistance = Math.Sqrt(
|
||
Math.Pow(end.X - start.X, 2) +
|
||
Math.Pow(end.Y - start.Y, 2) +
|
||
Math.Pow(end.Z - start.Z, 2)
|
||
);
|
||
double actualDistance = Math.Sqrt(
|
||
Math.Pow(actualEnd.X - start.X, 2) +
|
||
Math.Pow(actualEnd.Y - start.Y, 2) +
|
||
Math.Pow(actualEnd.Z - start.Z, 2)
|
||
);
|
||
|
||
completionPercentage = totalDistance > 0 ? (actualDistance / totalDistance) * 100 : 0;
|
||
|
||
// 判断是否到达了目标网格但高度层不对
|
||
if (lastInfo.X == endGridPos.X && lastInfo.Y == endGridPos.Y)
|
||
{
|
||
LogManager.Warning($"[3D路径规划] 部分路径：到达目标网格({lastInfo.X},{lastInfo.Y})但高度层不匹配 - 实际Layer{lastInfo.LayerIndex}, 目标Layer{endInfo.LayerIndex}");
|
||
}
|
||
else
|
||
{
|
||
LogManager.Warning($"[3D路径规划] 部分路径：到达网格({lastInfo.X},{lastInfo.Y}) Layer{lastInfo.LayerIndex}，距离终点网格({endGridPos.X},{endGridPos.Y}) Layer{endInfo.LayerIndex}还有距离");
|
||
}
|
||
LogManager.Info($"[3D路径规划] 完成度: {completionPercentage:F1}%，实际终点: ({actualEnd.X:F2}, {actualEnd.Y:F2}, Z={actualEnd.Z:F2})");
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
LogManager.Info($"[3D路径规划] 路径生成完成，包含 {path3D.Count} 个路径点，完成度: {completionPercentage:F1}%");
|
||
|
||
return new PathFindingResult
|
||
{
|
||
PathPoints = path3D,
|
||
IsComplete = isComplete,
|
||
OriginalEndPoint = end,
|
||
ActualEndPoint = actualEnd,
|
||
CompletionPercentage = completionPercentage
|
||
};
|
||
}
|
||
|
||
/// <summary>
|
||
/// 找到最接近指定位置和高度的3D节点
|
||
/// </summary>
|
||
private Roy_T.AStar.Graphs.Node FindClosest3DNode(Graph3DResult graph3D, GridPoint2D gridPos, double targetZ)
|
||
{
|
||
Roy_T.AStar.Graphs.Node closestNode = null;
|
||
double minDist = double.MaxValue;
|
||
|
||
foreach (var node in graph3D.AllNodes)
|
||
{
|
||
var info = graph3D.NodeInfo[node];
|
||
if (info.X == gridPos.X && info.Y == gridPos.Y)
|
||
{
|
||
double heightDiff = Math.Abs(info.Z - targetZ);
|
||
if (heightDiff < minDist)
|
||
{
|
||
minDist = heightDiff;
|
||
closestNode = node;
|
||
}
|
||
}
|
||
}
|
||
|
||
return closestNode;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将图路径转换为世界坐标路径
|
||
/// </summary>
|
||
private List<Point3D> ConvertGraphPathToWorld(Roy_T.AStar.Paths.Path astarPath, Graph3DResult graph3D, GridMap gridMap, Point3D originalStart, Point3D originalEnd)
|
||
{
|
||
var path3D = new List<Point3D>();
|
||
path3D.Add(originalStart);
|
||
|
||
int pointIndex = 0;
|
||
LogManager.Debug($"[路径点{pointIndex}] 原始起点: ({originalStart.X:F2}, {originalStart.Y:F2}, {originalStart.Z:F2}), 高程={gridMap.GetWorldElevation(originalStart):F2}");
|
||
pointIndex++;
|
||
|
||
// 添加第一条边的起点（如果与originalStart不同）
|
||
if (astarPath.Edges.Count > 0)
|
||
{
|
||
var firstEdge = astarPath.Edges[0];
|
||
var firstNode = firstEdge.Start as Roy_T.AStar.Graphs.Node;
|
||
if (firstNode != null && graph3D.NodeInfo.ContainsKey(firstNode))
|
||
{
|
||
var firstInfo = graph3D.NodeInfo[firstNode];
|
||
var firstPoint = gridMap.CreateWorldPoint(new GridPoint2D(firstInfo.X, firstInfo.Y), firstInfo.Z);
|
||
|
||
// 如果与起点高度不同，添加这个节点（避免直接跳跃）
|
||
if (Math.Abs(gridMap.GetWorldElevation(firstPoint) - gridMap.GetWorldElevation(originalStart)) > 0.01)
|
||
{
|
||
path3D.Add(firstPoint);
|
||
LogManager.Debug($"[路径点{pointIndex}] A*起点: 网格({firstInfo.X},{firstInfo.Y}), 层{firstInfo.LayerIndex}, 高程={firstInfo.Z:F2}, 类型={firstInfo.CellType}");
|
||
pointIndex++;
|
||
}
|
||
}
|
||
}
|
||
|
||
// 添加所有A*路径节点
|
||
foreach (var edge in astarPath.Edges)
|
||
{
|
||
var endNode = edge.End as Roy_T.AStar.Graphs.Node;
|
||
if (endNode == null || !graph3D.NodeInfo.ContainsKey(endNode))
|
||
continue;
|
||
|
||
var nodeInfo = graph3D.NodeInfo[endNode];
|
||
path3D.Add(gridMap.CreateWorldPoint(new GridPoint2D(nodeInfo.X, nodeInfo.Y), nodeInfo.Z));
|
||
|
||
LogManager.Debug($"[路径点{pointIndex}] 网格({nodeInfo.X},{nodeInfo.Y}), 层{nodeInfo.LayerIndex}, 高程={nodeInfo.Z:F2}, 类型={nodeInfo.CellType}");
|
||
pointIndex++;
|
||
}
|
||
|
||
// 🔥 修复：不再强制添加原始终点，由ExecuteAStarOnGraph根据完成度决定
|
||
LogManager.Info($"[路径转换完成] 总共 {path3D.Count} 个路径点");
|
||
|
||
return path3D;
|
||
}
|
||
|
||
|
||
/// <summary>
|
||
/// 2.5D路径规划（使用通道覆盖数据、高度约束和路径策略）[已废弃，保留作为备份]
|
||
/// </summary>
|
||
private PathFindingResult FindPath_Legacy_2_5D(Point3D start, Point3D end, GridMap gridMap, ChannelCoverage channelCoverage, double objectHeight, PathStrategy strategy = PathStrategy.Shortest)
|
||
{
|
||
try
|
||
{
|
||
LogManager.Info("[2.5D路径规划] 开始执行");
|
||
|
||
// 保存原始起点和终点，用于最终路径修正
|
||
var originalStart = start;
|
||
var originalEnd = end;
|
||
|
||
// 1. 检查起点和终点的高度约束
|
||
var startGridPos = gridMap.WorldToGrid(start);
|
||
if (!IsPointPassableAtHeight(startGridPos, start, gridMap, objectHeight))
|
||
{
|
||
LogManager.Warning("[2.5D路径规划] 起点位置不满足高度约束");
|
||
}
|
||
|
||
var endGridPos = gridMap.WorldToGrid(end);
|
||
if (!IsPointPassableAtHeight(endGridPos, end, gridMap, objectHeight))
|
||
{
|
||
LogManager.Warning("[2.5D路径规划] 终点位置不满足高度约束");
|
||
}
|
||
|
||
// 2. 根据策略选择合适的网格转换方法（返回A*网格和激活层字典）
|
||
var (astarGrid, activeLayerZ) = ConvertToAStarGridWithStrategy(gridMap, channelCoverage, objectHeight, start, end, strategy);
|
||
|
||
// 3. 执行A*算法
|
||
var pathResult = ExecuteAStarAlgorithm(start, end, gridMap, astarGrid);
|
||
|
||
if (pathResult == null || !pathResult.PathPoints.Any())
|
||
{
|
||
LogManager.Warning("[2.5D路径规划] 未找到有效路径");
|
||
return PathFindingResult.Failure("未找到有效路径，起点和终点之间可能被障碍物阻断");
|
||
}
|
||
|
||
LogManager.Info($"[2.5D路径规划] A*算法找到原始路径，包含 {pathResult.PathPoints.Count} 个点");
|
||
|
||
// 4. 提取网格路径
|
||
var gridPath = pathResult.PathPoints
|
||
.Select(point => gridMap.WorldToGrid(point))
|
||
.ToList();
|
||
|
||
// 5. 使用激活层字典还原3D路径
|
||
var enhancedPath = ApplyActiveLayers(gridPath, gridMap, activeLayerZ, originalStart, originalEnd);
|
||
|
||
// 更新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}");
|
||
throw; // 直接抛出异常，不返回默认路径
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将网格地图转换为A*算法使用的格式（2.5D模式）
|
||
/// </summary>
|
||
/// <param name="gridMap">网格地图</param>
|
||
/// <param name="channelCoverage">通道覆盖数据</param>
|
||
/// <param name="objectHeight">物体高度（模型单位）</param>
|
||
/// <returns>A*算法网格</returns>
|
||
private (Grid grid, Dictionary<GridPoint2D, double> activeLayerZ) ConvertToAStarGridWith2_5D(GridMap gridMap, ChannelCoverage channelCoverage, double objectHeight, Point3D startPos, Point3D endPos)
|
||
{
|
||
try
|
||
{
|
||
LogManager.Info($"[A*转换-2.5D] 开始转换网格格式: {gridMap.Width}x{gridMap.Height}，物体高度: {objectHeight}m");
|
||
|
||
// 初始化激活层字典
|
||
var activeLayerZ = new Dictionary<GridPoint2D, double>();
|
||
double targetZ = gridMap.GetWorldElevation(endPos); // 使用宿主坐标语义下的终点高程作为目标高度
|
||
double baseSpeed = 5.0; // km/h
|
||
|
||
LogManager.Info($"[A*转换-2.5D] 目标高程: {targetZ:F2}m，基础速度: {baseSpeed}km/h");
|
||
|
||
// 输出详细的网格统计信息
|
||
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((float)baseSpeed);
|
||
|
||
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.HasAnyWalkableLayer())
|
||
{
|
||
totalWalkableCells++;
|
||
}
|
||
else
|
||
{
|
||
totalNonWalkableCells++;
|
||
}
|
||
|
||
// 检查是否有满足物体高度的层
|
||
if (!HasCompatibleLayer(cell, objectHeight))
|
||
{
|
||
if (cell.HasAnyWalkableLayer())
|
||
{
|
||
heightConstrainedCells++;
|
||
if (heightConstrainedCells <= 10)
|
||
{
|
||
var layerInfo = cell.HeightLayers != null && cell.HeightLayers.Count > 0
|
||
? $"有{cell.HeightLayers.Count}个高度层"
|
||
: "无高度层";
|
||
LogManager.Info($"[A*高度约束] 单元格({x},{y})被高度约束排除：物体高度{objectHeight:F2}模型单位，{layerInfo}");
|
||
}
|
||
else if (heightConstrainedCells == 11)
|
||
{
|
||
LogManager.Info($"[A*高度约束] 还有更多单元格被高度约束排除，不再详细记录...");
|
||
}
|
||
}
|
||
continue;
|
||
}
|
||
|
||
// 为当前单元格选择最优高度层
|
||
var currentZ = SelectBestLayerForTarget(cell.HeightLayers, targetZ, objectHeight);
|
||
if (!currentZ.HasValue)
|
||
continue;
|
||
|
||
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];
|
||
var rightZ = SelectBestLayerForTarget(rightCell.HeightLayers, targetZ, objectHeight);
|
||
if (rightZ.HasValue)
|
||
{
|
||
var rightPos = new GridPosition(x + 1, y);
|
||
var rightGridPos = new GridPoint2D(x + 1, y);
|
||
|
||
// 计算高度差和速度系数
|
||
double heightDiff = Math.Abs(rightZ.Value - currentZ.Value);
|
||
double distanceToTarget = Math.Abs(rightZ.Value - targetZ);
|
||
float speedMultiplier = CalculateSpeedMultiplier(distanceToTarget);
|
||
var adjustedVelocity = Velocity.FromKilometersPerHour((float)(baseSpeed * speedMultiplier));
|
||
|
||
grid.AddEdge(pos, rightPos, adjustedVelocity);
|
||
activeLayerZ[rightGridPos] = rightZ.Value;
|
||
|
||
if ((x >= 49 && x <= 62 && y == 25) || (x == 71 && y == 23))
|
||
{
|
||
LogManager.Debug($"[A*构建] 边 ({x},{y})Z={currentZ.Value:F2} → ({x+1},{y})Z={rightZ.Value:F2}, " +
|
||
$"高度差={heightDiff:F2}m, 速度系数={speedMultiplier:F2}");
|
||
}
|
||
}
|
||
}
|
||
|
||
// 连接下方邻居
|
||
if (y + 1 < gridMap.Height)
|
||
{
|
||
var bottomCell = gridMap.Cells[x, y + 1];
|
||
var bottomZ = SelectBestLayerForTarget(bottomCell.HeightLayers, targetZ, objectHeight);
|
||
if (bottomZ.HasValue)
|
||
{
|
||
var bottomPos = new GridPosition(x, y + 1);
|
||
var bottomGridPos = new GridPoint2D(x, y + 1);
|
||
|
||
double distanceToTarget = Math.Abs(bottomZ.Value - targetZ);
|
||
float speedMultiplier = CalculateSpeedMultiplier(distanceToTarget);
|
||
var adjustedVelocity = Velocity.FromKilometersPerHour((float)(baseSpeed * speedMultiplier));
|
||
|
||
grid.AddEdge(pos, bottomPos, adjustedVelocity);
|
||
activeLayerZ[bottomGridPos] = bottomZ.Value;
|
||
}
|
||
}
|
||
|
||
// 连接左侧邻居
|
||
if (x - 1 >= 0)
|
||
{
|
||
var leftCell = gridMap.Cells[x - 1, y];
|
||
var leftZ = SelectBestLayerForTarget(leftCell.HeightLayers, targetZ, objectHeight);
|
||
if (leftZ.HasValue)
|
||
{
|
||
var leftPos = new GridPosition(x - 1, y);
|
||
var leftGridPos = new GridPoint2D(x - 1, y);
|
||
|
||
double distanceToTarget = Math.Abs(leftZ.Value - targetZ);
|
||
float speedMultiplier = CalculateSpeedMultiplier(distanceToTarget);
|
||
var adjustedVelocity = Velocity.FromKilometersPerHour((float)(baseSpeed * speedMultiplier));
|
||
|
||
grid.AddEdge(pos, leftPos, adjustedVelocity);
|
||
activeLayerZ[leftGridPos] = leftZ.Value;
|
||
}
|
||
}
|
||
|
||
// 连接上方邻居
|
||
if (y - 1 >= 0)
|
||
{
|
||
var topCell = gridMap.Cells[x, y - 1];
|
||
var topZ = SelectBestLayerForTarget(topCell.HeightLayers, targetZ, objectHeight);
|
||
if (topZ.HasValue)
|
||
{
|
||
var topPos = new GridPosition(x, y - 1);
|
||
var topGridPos = new GridPoint2D(x, y - 1);
|
||
|
||
double distanceToTarget = Math.Abs(topZ.Value - targetZ);
|
||
float speedMultiplier = CalculateSpeedMultiplier(distanceToTarget);
|
||
var adjustedVelocity = Velocity.FromKilometersPerHour((float)(baseSpeed * speedMultiplier));
|
||
|
||
grid.AddEdge(pos, topPos, adjustedVelocity);
|
||
activeLayerZ[topGridPos] = topZ.Value;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
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}%");
|
||
LogManager.Info($"[A*转换-2.5D] 记录了 {activeLayerZ.Count} 个网格的激活层");
|
||
|
||
return (grid, activeLayerZ);
|
||
}
|
||
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="objectHeight">物体高度（模型单位）</param>
|
||
/// <param name="startPos">起点坐标</param>
|
||
/// <param name="endPos">终点坐标</param>
|
||
/// <param name="strategy">路径规划策略</param>
|
||
/// <returns>A*网格和激活层字典的元组</returns>
|
||
private (Grid grid, Dictionary<GridPoint2D, double> activeLayerZ) ConvertToAStarGridWithStrategy(GridMap gridMap, ChannelCoverage channelCoverage, double objectHeight, Point3D startPos, Point3D endPos, PathStrategy strategy)
|
||
{
|
||
switch (strategy)
|
||
{
|
||
case PathStrategy.Shortest:
|
||
LogManager.Info($"[策略路由] 使用最短路径策略");
|
||
return ConvertToAStarGridWith2_5D(gridMap, channelCoverage, objectHeight, startPos, endPos);
|
||
|
||
case PathStrategy.Straightest:
|
||
LogManager.Info($"[策略路由] 使用直线优先策略");
|
||
return ConvertToAStarGridStraightest(gridMap, channelCoverage, objectHeight, startPos, endPos);
|
||
|
||
case PathStrategy.SafestCenter:
|
||
LogManager.Info($"[策略路由] 使用安全优先策略");
|
||
return ConvertToAStarGridSafestCenter(gridMap, channelCoverage, objectHeight, startPos, endPos);
|
||
|
||
default:
|
||
LogManager.Warning($"[策略路由] 未知策略 {strategy}，使用默认最短路径");
|
||
return ConvertToAStarGridWith2_5D(gridMap, channelCoverage, objectHeight, startPos, endPos);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 直线优先的A*网格转换方法
|
||
/// 通过调整不同方向边的速度来引导算法优先选择主方向路径
|
||
/// </summary>
|
||
/// <param name="gridMap">网格地图</param>
|
||
/// <param name="channelCoverage">通道覆盖数据</param>
|
||
/// <param name="objectHeight">物体高度（模型单位）</param>
|
||
/// <param name="startPos">起点坐标</param>
|
||
/// <param name="endPos">终点坐标</param>
|
||
/// <returns>A*网格</returns>
|
||
private (Grid grid, Dictionary<GridPoint2D, double> activeLayerZ) ConvertToAStarGridStraightest(GridMap gridMap, ChannelCoverage channelCoverage, double objectHeight, Point3D startPos, Point3D endPos)
|
||
{
|
||
try
|
||
{
|
||
LogManager.Info($"[局部直线优先] 开始实施局部直线优先算法");
|
||
|
||
// 初始化激活高度层字典和目标高度
|
||
var activeLayerZ = new Dictionary<GridPoint2D, double>();
|
||
double targetZ = gridMap.GetWorldElevation(endPos);
|
||
|
||
// 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);
|
||
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];
|
||
|
||
// 检查是否有兼容的高度层
|
||
if (!HasCompatibleLayer(cell, objectHeight))
|
||
{
|
||
if (cell.HasAnyWalkableLayer())
|
||
{
|
||
heightConstrainedCells++;
|
||
}
|
||
continue;
|
||
}
|
||
|
||
// 选择当前网格的最佳高度层
|
||
var currentZ = SelectBestLayerForTarget(cell.HeightLayers, targetZ, objectHeight);
|
||
if (!currentZ.HasValue)
|
||
{
|
||
continue;
|
||
}
|
||
|
||
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, objectHeight))
|
||
{
|
||
// 选择右侧网格的最佳高度层
|
||
var rightZ = SelectBestLayerForTarget(rightCell.HeightLayers, targetZ, objectHeight);
|
||
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, objectHeight);
|
||
|
||
// 计算基础速度（基于直线距离）
|
||
float baseSpeed = 5.0f;
|
||
float bonusSpeed = Math.Min(rightDistance * 0.5f, 30.0f);
|
||
float straightSpeed = baseSpeed + bonusSpeed;
|
||
|
||
// 添加高度惩罚：离目标高度越远，速度越慢
|
||
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++;
|
||
}
|
||
}
|
||
}
|
||
|
||
// 🔥 核心改动：下方向连接（基于局部直线距离）
|
||
if (y + 1 < gridMap.Height)
|
||
{
|
||
var bottomCell = gridMap.Cells[x, y + 1];
|
||
|
||
// 检查下方网格是否有兼容的高度层
|
||
if (HasCompatibleLayer(bottomCell, objectHeight))
|
||
{
|
||
// 选择下方网格的最佳高度层
|
||
var bottomZ = SelectBestLayerForTarget(bottomCell.HeightLayers, targetZ, objectHeight);
|
||
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, objectHeight);
|
||
|
||
// 计算基础速度（基于直线距离）
|
||
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)
|
||
{
|
||
var leftCell = gridMap.Cells[x - 1, y];
|
||
|
||
// 检查左侧网格是否有兼容的高度层
|
||
if (HasCompatibleLayer(leftCell, objectHeight))
|
||
{
|
||
// 选择左侧网格的最佳高度层
|
||
var leftZ = SelectBestLayerForTarget(leftCell.HeightLayers, targetZ, objectHeight);
|
||
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, objectHeight);
|
||
|
||
// 计算基础速度（基于直线距离）
|
||
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)
|
||
{
|
||
var topCell = gridMap.Cells[x, y - 1];
|
||
|
||
// 检查上方网格是否有兼容的高度层
|
||
if (HasCompatibleLayer(topCell, objectHeight))
|
||
{
|
||
// 选择上方网格的最佳高度层
|
||
var topZ = SelectBestLayerForTarget(topCell.HeightLayers, targetZ, objectHeight);
|
||
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, objectHeight);
|
||
|
||
// 计算基础速度（基于直线距离）
|
||
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)
|
||
{
|
||
var diagonalCell = gridMap.Cells[x + 1, y + 1];
|
||
if (HasCompatibleLayer(diagonalCell, objectHeight))
|
||
{
|
||
var diagonalZ = SelectBestLayerForTarget(diagonalCell.HeightLayers, targetZ, objectHeight);
|
||
if (diagonalZ.HasValue)
|
||
{
|
||
var diagonalPos = new GridPosition(x + 1, y + 1);
|
||
var diagonalGridPos = new GridPoint2D(x + 1, y + 1);
|
||
activeLayerZ[diagonalGridPos] = diagonalZ.Value;
|
||
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}%");
|
||
LogManager.Info($"[局部直线优先] - 激活层记录数: {activeLayerZ.Count}");
|
||
|
||
return (grid, activeLayerZ);
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"[局部直线优先] 网格转换失败: {ex.Message}");
|
||
throw new AutoPathPlanningException($"局部直线优先网格转换失败: {ex.Message}", ex);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 检查单元格是否满足高度约束
|
||
/// </summary>
|
||
/// <param name="cell">网格单元格</param>
|
||
/// <param name="objectHeight">物体高度（模型单位）</param>
|
||
/// <returns>是否可通行</returns>
|
||
private bool IsPassableWithHeight(GridCell cell, double objectHeight)
|
||
{
|
||
if (!cell.HasAnyWalkableLayer())
|
||
return false;
|
||
|
||
// 检查是否有任何高度层满足物体高度要求
|
||
if (cell.HeightLayers != null && cell.HeightLayers.Count > 0)
|
||
{
|
||
foreach (var layer in cell.HeightLayers)
|
||
{
|
||
if (layer.IsWalkable && layer.PassableHeight.GetSpan() >= objectHeight)
|
||
return true;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
return false;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 检查单元格是否有满足物体高度的高度层
|
||
/// </summary>
|
||
/// <param name="cell">网格单元格</param>
|
||
/// <param name="objectHeight">物体高度（模型单位）</param>
|
||
/// <returns>是否有兼容的高度层</returns>
|
||
private bool HasCompatibleLayer(GridCell cell, double objectHeight)
|
||
{
|
||
if (!cell.HasAnyWalkableLayer())
|
||
return false;
|
||
|
||
if (cell.HeightLayers == null || cell.HeightLayers.Count == 0)
|
||
return false;
|
||
|
||
foreach (var layer in cell.HeightLayers)
|
||
{
|
||
if (layer.IsWalkable && layer.PassableHeight.GetSpan() >= objectHeight)
|
||
return true;
|
||
}
|
||
|
||
return false;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 从高度层列表中选择最接近目标高度且满足物体高度的层
|
||
/// </summary>
|
||
/// <param name="layers">高度层列表</param>
|
||
/// <param name="targetZ">目标高度（米）</param>
|
||
/// <param name="objectHeight">物体高度（模型单位）</param>
|
||
/// <returns>选中的高度层Z坐标，如果没有合适的返回null</returns>
|
||
private double? SelectBestLayerForTarget(List<HeightLayer> layers, double targetZ, double objectHeight)
|
||
{
|
||
if (layers == null || layers.Count == 0)
|
||
return null;
|
||
|
||
HeightLayer? bestLayer = null;
|
||
double minDistance = double.MaxValue;
|
||
|
||
foreach (var layer in layers)
|
||
{
|
||
if (!layer.IsWalkable)
|
||
continue;
|
||
|
||
// 必须满足物体高度
|
||
if (layer.PassableHeight.GetSpan() < objectHeight)
|
||
continue;
|
||
|
||
// 选择最接近目标高度的层
|
||
double distance = Math.Abs(layer.Z - targetZ);
|
||
if (distance < minDistance)
|
||
{
|
||
minDistance = distance;
|
||
bestLayer = layer;
|
||
}
|
||
}
|
||
|
||
return bestLayer?.Z;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 根据高度层与目标高度的距离计算速度系数
|
||
/// </summary>
|
||
/// <param name="distanceToTarget">与目标高度的距离（米）</param>
|
||
/// <returns>速度系数（0-1之间）</returns>
|
||
private float CalculateSpeedMultiplier(double distanceToTarget)
|
||
{
|
||
// 距离目标高度越近，速度越快（A*成本越低）
|
||
// 公式：速度 = 1 / (1 + 距离 * 惩罚系数)
|
||
// 例如：距离0m → 速度1.0x, 距离3m → 速度0.77x, 距离6m → 速度0.625x
|
||
return (float)(1.0 / (1.0 + distanceToTarget * HEIGHT_PENALTY_FACTOR));
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算从指定位置沿指定方向能走的最大直线距离
|
||
/// </summary>
|
||
/// <param name="startPos">起始网格位置</param>
|
||
/// <param name="direction">方向 (1,0)=右, (0,1)=下, (-1,0)=左, (0,-1)=上</param>
|
||
/// <param name="gridMap">网格地图</param>
|
||
/// <param name="objectHeight">物体高度（模型单位）</param>
|
||
/// <returns>直线距离（网格单位）</returns>
|
||
private int CalculateStraightDistance(GridPoint2D startPos, GridPoint2D direction,
|
||
GridMap gridMap, double objectHeight)
|
||
{
|
||
int distance = 0;
|
||
var currentPos = new GridPoint2D(startPos.X + direction.X, startPos.Y + direction.Y);
|
||
|
||
// 沿方向前进，直到遇到障碍
|
||
while (IsValidAndPassable(currentPos, gridMap, objectHeight))
|
||
{
|
||
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="objectHeight">物体高度（模型单位）</param>
|
||
/// <returns>是否有效且可通行</returns>
|
||
private bool IsValidAndPassable(GridPoint2D pos, GridMap gridMap, double objectHeight)
|
||
{
|
||
// 检查边界
|
||
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, objectHeight);
|
||
}
|
||
|
||
/// <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坐标
|
||
double endElevation = endCell.Value.HeightLayers != null && endCell.Value.HeightLayers.Count > 0
|
||
? endCell.Value.HeightLayers[0].Z
|
||
: 0;
|
||
|
||
if (endCell.Value.CellType == "门")
|
||
{
|
||
LogManager.Info($"[路径转换-门] 门网格({endGridPos.X},{endGridPos.Y}) HeightLayer[0].高程={endElevation:F3}, 位置({endWorldPos.X:F2},{endWorldPos.Y:F2})");
|
||
}
|
||
endWorldPos = gridMap.SetWorldElevation(endWorldPos, endElevation);
|
||
}
|
||
else
|
||
{
|
||
LogManager.Warning($"[路径转换] 网格({endGridPos.X},{endGridPos.Y})获取失败，位置({endWorldPos.X:F2},{endWorldPos.Y:F2})，高程保持为默认值");
|
||
}
|
||
|
||
// 检查是否与上一个点重复（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 = CalculateHostHorizontalDistance(startPoint, endPoint, gridMap);
|
||
|
||
// 🔥 修复：检查高度变化，有高度变化就不能优化掉
|
||
// 1. 起终点高度差不能超过阈值（原有逻辑：防止跨越大高度差，例如楼梯）
|
||
// 2. 中间点高度必须与起点和终点一致（新增逻辑：任何高度变化都不能优化掉）
|
||
bool canSkip = false;
|
||
double maxAllowedHeightDiff = ConfigManager.Instance.Current.PathEditing.MaxHeightDiff; // 从配置文件读取高度差阈值（模型单位）
|
||
|
||
double startZ = gridMap.GetWorldElevation(startPoint);
|
||
double endZ = gridMap.GetWorldElevation(endPoint);
|
||
double totalHeightDiff = Math.Abs(endZ - startZ);
|
||
|
||
// 检查1：起终点高度差必须在阈值内
|
||
if (totalHeightDiff <= maxAllowedHeightDiff)
|
||
{
|
||
// 检查2：所有中间点的高度必须与起点和终点一致（不能有高度变化）
|
||
bool hasMiddleHeightChange = false;
|
||
const double heightTolerance = 1e-6; // 浮点精度容差
|
||
|
||
for (int midIndex = currentIndex + 1; midIndex < testIndex; midIndex++)
|
||
{
|
||
double midZ = gridMap.GetWorldElevation(optimizedPath[midIndex]);
|
||
// 如果中间点高度与起点或终点不同，说明有高度变化，不能优化掉
|
||
if (Math.Abs(midZ - startZ) > heightTolerance ||
|
||
Math.Abs(midZ - endZ) > heightTolerance)
|
||
{
|
||
hasMiddleHeightChange = true;
|
||
break;
|
||
}
|
||
}
|
||
|
||
canSkip = !hasMiddleHeightChange;
|
||
}
|
||
|
||
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}个点)");
|
||
|
||
// 打印斜线优化后的路径点详情
|
||
LogManager.Info($"[斜线优化后路径详情] 共 {diagonalOptimizedPath.Count} 个点：");
|
||
for (int i = 0; i < diagonalOptimizedPath.Count; i++)
|
||
{
|
||
var pt = diagonalOptimizedPath[i];
|
||
LogManager.Info($" [斜线优化后点{i}] 位置=({pt.X:F2}, {pt.Y:F2}, Z={pt.Z:F2})");
|
||
}
|
||
|
||
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 = CalculateHostHorizontalDistance(start, end, gridMap);
|
||
|
||
// 如果距离太小，直接检查起点和终点
|
||
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;
|
||
double interpolatedElevation =
|
||
gridMap.GetWorldElevation(start) +
|
||
t * (gridMap.GetWorldElevation(end) - gridMap.GetWorldElevation(start));
|
||
|
||
var samplePoint = gridMap.SetWorldElevation(
|
||
new Point3D(
|
||
start.X + t * (end.X - start.X),
|
||
start.Y + t * (end.Y - start.Y),
|
||
start.Z + t * (end.Z - start.Z)),
|
||
interpolatedElevation);
|
||
|
||
// 🔥 修复：检查采样点的具体Z高度是否在可通行层范围内
|
||
// 使用IsPassableAt3DPoint而不是IsWalkable，确保不穿过障碍物层
|
||
var gridPos = gridMap.WorldToGrid(samplePoint);
|
||
double tolerance = 0.1; // Z坐标容差（模型单位）
|
||
|
||
if (!gridMap.IsValidGridPosition(gridPos) || !gridMap.IsPassableAtElevation(gridPos, gridMap.GetWorldElevation(samplePoint), tolerance))
|
||
{
|
||
// 🔥 修复：使用Origin计算网格左下角，而不是Bounds.Min
|
||
var (gridMinH1, gridMinH2) = GetGridCellMinHorizontalCoords(gridMap, gridPos);
|
||
var gridCenterH1 = gridMinH1 + 0.5 * gridMap.CellSize;
|
||
var gridCenterH2 = gridMinH2 + 0.5 * gridMap.CellSize;
|
||
var gridCenterZ = gridMap.GetWorldElevation(samplePoint);
|
||
|
||
// 计算偏差
|
||
var sampleHorizontal = GetHostHorizontalCoords(samplePoint, gridMap);
|
||
var deltaH1 = sampleHorizontal.h1 - gridCenterH1;
|
||
var deltaH2 = sampleHorizontal.h2 - gridCenterH2;
|
||
|
||
// 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) ? "网格无效" : "Z坐标不在可通行层范围")}");
|
||
return false;
|
||
}
|
||
|
||
// 🔥 新增：邻居障碍位置检查
|
||
if (!IsSamplePointSafeFromNeighborObstacles(samplePoint, gridPos, gridMap))
|
||
{
|
||
// 🔥 修复：使用Origin计算网格左下角
|
||
var (gridMinH1, gridMinH2) = GetGridCellMinHorizontalCoords(gridMap, gridPos);
|
||
var gridCenterH1 = gridMinH1 + 0.5 * gridMap.CellSize;
|
||
var gridCenterH2 = gridMinH2 + 0.5 * gridMap.CellSize;
|
||
var sampleHorizontal = GetHostHorizontalCoords(samplePoint, gridMap);
|
||
var deltaH1 = sampleHorizontal.h1 - gridCenterH1;
|
||
var deltaH2 = sampleHorizontal.h2 - gridCenterH2;
|
||
|
||
// 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;
|
||
}
|
||
}
|
||
|
||
if (SegmentIntersectsBlockedGridCellInterior(start, end, gridMap))
|
||
{
|
||
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 (gridMinH1, gridMinH2) = GetGridCellMinHorizontalCoords(gridMap, gridPos);
|
||
var sampleHorizontal = GetHostHorizontalCoords(samplePoint, gridMap);
|
||
|
||
var relativeX = (sampleHorizontal.h1 - gridMinH1) / gridMap.CellSize;
|
||
var relativeY = (sampleHorizontal.h2 - gridMinH2) / 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 gridCenterH1 = gridMinH1 + 0.5 * gridMap.CellSize;
|
||
var gridCenterH2 = gridMinH2 + 0.5 * gridMap.CellSize;
|
||
var deltaH1 = sampleHorizontal.h1 - gridCenterH1;
|
||
var deltaH2 = sampleHorizontal.h2 - gridCenterH2;
|
||
|
||
LogManager.Debug($"[邻居障碍] 位置约束失败：" +
|
||
$"{directionNames[i]}方向有障碍，" +
|
||
$"网格({gridPos.X},{gridPos.Y})，" +
|
||
$"采样点({samplePoint.X:F3},{samplePoint.Y:F3})，" +
|
||
$"网格左下角H=({gridMinH1:F3},{gridMinH2:F3})，" +
|
||
$"网格中心H=({gridCenterH1:F3},{gridCenterH2:F3})，" +
|
||
$"偏差(ΔH1={deltaH1:F3}, ΔH2={deltaH2: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;
|
||
}
|
||
|
||
private static double CalculateHostHorizontalDistance(Point3D start, Point3D end, GridMap gridMap)
|
||
{
|
||
var startHorizontal = GetHostHorizontalCoords(start, gridMap);
|
||
var endHorizontal = GetHostHorizontalCoords(end, gridMap);
|
||
double dh1 = endHorizontal.h1 - startHorizontal.h1;
|
||
double dh2 = endHorizontal.h2 - startHorizontal.h2;
|
||
return Math.Sqrt(dh1 * dh1 + dh2 * dh2);
|
||
}
|
||
|
||
private static (double h1, double h2) GetGridCellMinHorizontalCoords(GridMap gridMap, GridPoint2D gridPosition)
|
||
{
|
||
var (minH1, _, minH2, _) = gridMap.GetGridCellPlanarBounds(gridPosition);
|
||
return (minH1, minH2);
|
||
}
|
||
|
||
private static (double h1, double h2) GetHostHorizontalCoords(Point3D point, GridMap gridMap)
|
||
{
|
||
var adapter = new HostCoordinateAdapter(gridMap.CoordinateSystemType);
|
||
return HostPlanarGridHelper.GetHorizontalCoords3(
|
||
new System.Numerics.Vector3((float)point.X, (float)point.Y, (float)point.Z),
|
||
adapter);
|
||
}
|
||
|
||
private bool SegmentIntersectsBlockedGridCellInterior(Point3D start, Point3D end, GridMap gridMap)
|
||
{
|
||
var startHorizontal = GetHostHorizontalCoords(start, gridMap);
|
||
var endHorizontal = GetHostHorizontalCoords(end, gridMap);
|
||
var originHorizontal = GetHostHorizontalCoords(gridMap.Origin, gridMap);
|
||
|
||
int minGridX = (int)Math.Round((Math.Min(startHorizontal.h1, endHorizontal.h1) - originHorizontal.h1) / gridMap.CellSize) - 1;
|
||
int maxGridX = (int)Math.Round((Math.Max(startHorizontal.h1, endHorizontal.h1) - originHorizontal.h1) / gridMap.CellSize) + 1;
|
||
int minGridY = (int)Math.Round((Math.Min(startHorizontal.h2, endHorizontal.h2) - originHorizontal.h2) / gridMap.CellSize) - 1;
|
||
int maxGridY = (int)Math.Round((Math.Max(startHorizontal.h2, endHorizontal.h2) - originHorizontal.h2) / gridMap.CellSize) + 1;
|
||
|
||
minGridX = Math.Max(0, minGridX);
|
||
minGridY = Math.Max(0, minGridY);
|
||
maxGridX = Math.Min(gridMap.Width - 1, maxGridX);
|
||
maxGridY = Math.Min(gridMap.Height - 1, maxGridY);
|
||
|
||
for (int x = minGridX; x <= maxGridX; x++)
|
||
{
|
||
for (int y = minGridY; y <= maxGridY; y++)
|
||
{
|
||
var gridPosition = new GridPoint2D(x, y);
|
||
|
||
if (!gridMap.TryGetSegmentGridCellInteriorIntersection(start, end, gridPosition, out double enterT, out double exitT))
|
||
{
|
||
continue;
|
||
}
|
||
|
||
double sampleT = Math.Max(0.0, Math.Min(1.0, (enterT + exitT) * 0.5));
|
||
Point3D samplePoint = new Point3D(
|
||
start.X + sampleT * (end.X - start.X),
|
||
start.Y + sampleT * (end.Y - start.Y),
|
||
start.Z + sampleT * (end.Z - start.Z));
|
||
double sampleElevation = gridMap.GetWorldElevation(samplePoint);
|
||
|
||
if (!gridMap.IsPassableAtElevation(gridPosition, sampleElevation, 0.1))
|
||
{
|
||
LogManager.Debug(
|
||
$"[斜线检查] 线段穿过不可通行网格内部: 网格({x},{y}), " +
|
||
$"t=[{enterT:F3},{exitT:F3}], sampleT={sampleT:F3}");
|
||
return true;
|
||
}
|
||
}
|
||
}
|
||
|
||
return false;
|
||
}
|
||
|
||
|
||
/// <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 = TURN_ANGLE_THRESHOLD_RADIANS; // 弧度
|
||
|
||
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, GridMap gridMap)
|
||
{
|
||
if (path == null || path.Count == 0)
|
||
return path;
|
||
|
||
try
|
||
{
|
||
LogManager.Info($"[坐标修正] 开始修正路径起终点坐标");
|
||
var correctedPath = new List<Point3D>(path);
|
||
|
||
// 修正起点：保持原始宿主水平位置，复用路径求得的高程
|
||
if (correctedPath.Count > 0)
|
||
{
|
||
double startElevation = gridMap.GetWorldElevation(correctedPath[0]);
|
||
var correctedStart = gridMap.SetWorldElevation(originalStart, startElevation);
|
||
LogManager.Info($"[坐标修正] 起点: ({correctedPath[0].X:F3}, {correctedPath[0].Y:F3}, {correctedPath[0].Z:F3}) -> ({correctedStart.X:F3}, {correctedStart.Y:F3}, {correctedStart.Z:F3}), 高程={startElevation:F3}");
|
||
correctedPath[0] = correctedStart;
|
||
}
|
||
|
||
// 修正终点：保持原始宿主水平位置，复用路径求得的高程
|
||
if (correctedPath.Count > 1)
|
||
{
|
||
var lastIndex = correctedPath.Count - 1;
|
||
double endElevation = gridMap.GetWorldElevation(correctedPath[lastIndex]);
|
||
var correctedEnd = gridMap.SetWorldElevation(originalEnd, endElevation);
|
||
LogManager.Info($"[坐标修正] 终点: ({correctedPath[lastIndex].X:F3}, {correctedPath[lastIndex].Y:F3}, {correctedPath[lastIndex].Z:F3}) -> ({correctedEnd.X:F3}, {correctedEnd.Y:F3}, {correctedEnd.Z:F3}), 高程={endElevation:F3}");
|
||
correctedPath[lastIndex] = correctedEnd;
|
||
}
|
||
|
||
LogManager.Info($"[坐标修正] 路径起终点坐标修正完成");
|
||
return correctedPath;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"[坐标修正] 修正路径起终点坐标失败: {ex.Message}，使用原始路径");
|
||
return path;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 检查指定点在给定高度约束下是否可通行
|
||
/// </summary>
|
||
/// <param name="gridPos">网格坐标</param>
|
||
/// <param name="point">检查点（世界坐标）</param>
|
||
/// <param name="gridMap">网格地图</param>
|
||
/// <param name="objectHeight">物体高度（模型单位）</param>
|
||
/// <returns>是否可通行</returns>
|
||
private bool IsPointPassableAtHeight(GridPoint2D gridPos, Point3D point, GridMap gridMap, double objectHeight)
|
||
{
|
||
try
|
||
{
|
||
// 使用传入的网格坐标，避免重复转换
|
||
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.HasAnyWalkableLayer())
|
||
{
|
||
LogManager.Info($"[高度检查] 单元格不可通行：({gridX}, {gridY}) - HasWalkableLayer={cell.HasAnyWalkableLayer()}, CellType={cell.CellType}, SpeedLimit={cell.SpeedLimit:F2}m/s, 原因：网格标记为不可通行");
|
||
return false;
|
||
}
|
||
|
||
// 检查高度层约束
|
||
if (cell.HeightLayers != null && cell.HeightLayers.Count > 0)
|
||
{
|
||
// 查找包含指定Z坐标且满足物体高度的层
|
||
var matchingLayer = gridMap.FindLayerContainingElevation(gridPos, gridMap.GetWorldElevation(point), tolerance: 0.5);
|
||
|
||
if (matchingLayer.HasValue)
|
||
{
|
||
bool heightOk = matchingLayer.Value.PassableHeight.GetSpan() >= objectHeight;
|
||
if (heightOk)
|
||
{
|
||
return true;
|
||
}
|
||
else
|
||
{
|
||
LogManager.Warning($"[高度检查] ❌ 找到匹配高程={gridMap.GetWorldElevation(point):F2}的层高程={matchingLayer.Value.Z:F2}，但高度不足：物体高度{objectHeight:F2}，层高度跨度{matchingLayer.Value.PassableHeight.GetSpan():F2}");
|
||
}
|
||
}
|
||
else
|
||
{
|
||
LogManager.Warning($"[高度检查] ❌ 单元格({gridX}, {gridY})有{cell.HeightLayers.Count}个高度层，但无法找到包含高程={gridMap.GetWorldElevation(point):F2}的层");
|
||
}
|
||
return false;
|
||
}
|
||
|
||
LogManager.Warning($"[高度检查] ❌ 单元格({gridX}, {gridY})没有高度层信息");
|
||
return false;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"[高度约束检查] 检查点可通行性失败: {ex.Message}");
|
||
return false;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 根据网格高度区间调整路径点Z坐标
|
||
/// </summary>
|
||
/// <param name="pathWithGridCoords">路径点和对应的网格坐标</param>
|
||
/// <param name="gridMap">网格地图</param>
|
||
/// <param name="objectHeight">物体高度（模型单位）</param>
|
||
/// <param name="originalStart">用户指定的原始起点（保留原始Z坐标）</param>
|
||
/// <param name="originalEnd">用户指定的原始终点（保留原始Z坐标）</param>
|
||
/// <returns>调整后的路径</returns>
|
||
private List<Point3D> ApplyGridHeightConstraints(List<(Point3D point, GridPoint2D gridPos)> pathWithGridCoords, GridMap gridMap, double objectHeight, Point3D originalStart, Point3D originalEnd)
|
||
{
|
||
LogManager.Info($"[智能选层] 开始为路径点选择最优高度层，路径点数: {pathWithGridCoords.Count}");
|
||
|
||
if (pathWithGridCoords.Count == 0)
|
||
return new List<Point3D>();
|
||
|
||
var adjustedPath = new List<Point3D>();
|
||
|
||
// 1. 使用用户指定的原始起点和终点高程（而不是从转换后的路径中提取）
|
||
double startZ = gridMap.GetWorldElevation(originalStart);
|
||
double endZ = gridMap.GetWorldElevation(originalEnd);
|
||
|
||
// 对比日志：显示原始坐标与路径坐标的差异
|
||
var pathStartZ = gridMap.GetWorldElevation(pathWithGridCoords[0].point);
|
||
var pathEndZ = gridMap.GetWorldElevation(pathWithGridCoords[pathWithGridCoords.Count - 1].point);
|
||
|
||
LogManager.Info($"[智能选层] 原始起点高程={startZ:F3}, 原始终点高程={endZ:F3}");
|
||
LogManager.Info($"[智能选层] 路径起点高程={pathStartZ:F3}, 路径终点高程={pathEndZ:F3}");
|
||
LogManager.Info($"[智能选层] 使用原始高程进行智能选层，高程变化={endZ - startZ:F3}");
|
||
|
||
// 2. 为每个路径点选择高度层
|
||
for (int i = 0; i < pathWithGridCoords.Count; i++)
|
||
{
|
||
var (point, gridPos) = pathWithGridCoords[i];
|
||
|
||
// 检查网格位置有效性
|
||
if (!gridMap.IsValidGridPosition(gridPos))
|
||
{
|
||
adjustedPath.Add(point);
|
||
continue;
|
||
}
|
||
|
||
var cell = gridMap.Cells[gridPos.X, gridPos.Y];
|
||
|
||
// 如果没有高度层，使用原始点
|
||
if (cell.HeightLayers == null || cell.HeightLayers.Count == 0)
|
||
{
|
||
adjustedPath.Add(point);
|
||
continue;
|
||
}
|
||
|
||
// 起点和终点：锁定到包含其Z坐标的层
|
||
if (i == 0)
|
||
{
|
||
var selectedLayer = SelectBestLayer(cell.HeightLayers, startZ, objectHeight, startZ, endZ, i, pathWithGridCoords.Count, true);
|
||
if (selectedLayer.HasValue)
|
||
{
|
||
adjustedPath.Add(gridMap.SetWorldElevation(point, selectedLayer.Value.Z));
|
||
LogManager.Debug($"[智能选层] 起点({gridPos.X},{gridPos.Y}) 高程={selectedLayer.Value.Z:F3}");
|
||
}
|
||
else
|
||
{
|
||
adjustedPath.Add(point);
|
||
}
|
||
}
|
||
else if (i == pathWithGridCoords.Count - 1)
|
||
{
|
||
var selectedLayer = SelectBestLayer(cell.HeightLayers, endZ, objectHeight, startZ, endZ, i, pathWithGridCoords.Count, true);
|
||
if (selectedLayer.HasValue)
|
||
{
|
||
adjustedPath.Add(gridMap.SetWorldElevation(point, selectedLayer.Value.Z));
|
||
LogManager.Debug($"[智能选层] 终点({gridPos.X},{gridPos.Y}) 高程={selectedLayer.Value.Z:F3}");
|
||
}
|
||
else
|
||
{
|
||
adjustedPath.Add(point);
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// 中间点：根据高度趋势选择最佳层
|
||
var prevZ = gridMap.GetWorldElevation(adjustedPath[i - 1]);
|
||
var selectedLayer = SelectBestLayer(cell.HeightLayers, prevZ, objectHeight, startZ, endZ, i, pathWithGridCoords.Count, false);
|
||
|
||
if (selectedLayer.HasValue)
|
||
{
|
||
adjustedPath.Add(gridMap.SetWorldElevation(point, selectedLayer.Value.Z));
|
||
}
|
||
else
|
||
{
|
||
adjustedPath.Add(point);
|
||
}
|
||
}
|
||
}
|
||
|
||
LogManager.Info($"[智能选层] 完成，调整了 {adjustedPath.Count}/{pathWithGridCoords.Count} 个点");
|
||
return adjustedPath;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 使用激活层字典还原3D路径
|
||
/// </summary>
|
||
/// <param name="gridPath">网格路径</param>
|
||
/// <param name="gridMap">网格地图</param>
|
||
/// <param name="activeLayerZ">激活层字典（网格坐标→Z坐标）</param>
|
||
/// <param name="originalStart">原始起点</param>
|
||
/// <param name="originalEnd">原始终点</param>
|
||
/// <returns>3D路径点列表</returns>
|
||
private List<Point3D> ApplyActiveLayers(
|
||
List<GridPoint2D> gridPath,
|
||
GridMap gridMap,
|
||
Dictionary<GridPoint2D, double> activeLayerZ,
|
||
Point3D originalStart,
|
||
Point3D originalEnd)
|
||
{
|
||
LogManager.Info($"[3D还原] 开始使用激活层还原3D路径，路径点数: {gridPath.Count}，激活层记录数: {activeLayerZ.Count}");
|
||
|
||
var path3D = new List<Point3D>();
|
||
|
||
if (gridPath.Count == 0)
|
||
return path3D;
|
||
|
||
for (int i = 0; i < gridPath.Count; i++)
|
||
{
|
||
var gridPos = gridPath[i];
|
||
double elevation;
|
||
|
||
// 起点：使用原始起点Z
|
||
if (i == 0)
|
||
{
|
||
elevation = gridMap.GetWorldElevation(originalStart);
|
||
LogManager.Info($"[3D还原] 起点 ({gridPos.X},{gridPos.Y}) 高程={elevation:F2}m (原始起点)");
|
||
}
|
||
// 终点：使用原始终点Z
|
||
else if (i == gridPath.Count - 1)
|
||
{
|
||
elevation = gridMap.GetWorldElevation(originalEnd);
|
||
LogManager.Info($"[3D还原] 终点 ({gridPos.X},{gridPos.Y}) 高程={elevation:F2}m (原始终点)");
|
||
}
|
||
// 中间点：使用A*阶段记录的激活层
|
||
else if (activeLayerZ.TryGetValue(gridPos, out double activeZ))
|
||
{
|
||
elevation = activeZ;
|
||
LogManager.Debug($"[3D还原] 点{i} ({gridPos.X},{gridPos.Y}) 高程={elevation:F2}m (激活层)");
|
||
}
|
||
else
|
||
{
|
||
// 降级：无激活层记录，使用前一点高度
|
||
elevation = gridMap.GetWorldElevation(path3D[i - 1]);
|
||
LogManager.Warning($"[3D还原] 点{i} ({gridPos.X},{gridPos.Y}) 无激活层记录，使用前点高程={elevation:F2}m");
|
||
}
|
||
|
||
path3D.Add(gridMap.CreateWorldPoint(gridPos, elevation));
|
||
}
|
||
|
||
LogManager.Info($"[3D还原] 完成，生成了 {path3D.Count} 个3D路径点");
|
||
return path3D;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 为路径点选择最佳高度层
|
||
/// </summary>
|
||
/// <param name="layers">可用的高度层列表</param>
|
||
/// <param name="referenceZ">参考Z坐标（起点/终点为目标Z，中间点为前一点Z）</param>
|
||
/// <param name="objectHeight">物体高度</param>
|
||
/// <param name="startZ">路径起点Z</param>
|
||
/// <param name="endZ">路径终点Z</param>
|
||
/// <param name="currentIndex">当前点索引</param>
|
||
/// <param name="totalPoints">总点数</param>
|
||
/// <param name="exactMatch">是否精确匹配（起点/终点使用）</param>
|
||
/// <returns>选中的高度层，如果没有合适的返回null</returns>
|
||
private HeightLayer? SelectBestLayer(List<HeightLayer> layers, double referenceZ, double objectHeight,
|
||
double startZ, double endZ, int currentIndex, int totalPoints, bool exactMatch)
|
||
{
|
||
if (layers == null || layers.Count == 0)
|
||
return null;
|
||
|
||
// 只有一层，直接返回
|
||
if (layers.Count == 1)
|
||
{
|
||
var layer = layers[0];
|
||
if (layer.IsWalkable && layer.PassableHeight.GetSpan() >= objectHeight)
|
||
return layer;
|
||
return null;
|
||
}
|
||
|
||
// 确定目标Z坐标
|
||
double targetZ;
|
||
if (exactMatch)
|
||
{
|
||
// 精确匹配：直接使用参考Z
|
||
targetZ = referenceZ;
|
||
}
|
||
else
|
||
{
|
||
// 中间点：根据高度趋势计算目标Z
|
||
int remainingSteps = totalPoints - currentIndex - 1;
|
||
if (remainingSteps <= 0)
|
||
{
|
||
// 没有剩余步骤，使用参考Z
|
||
targetZ = referenceZ;
|
||
}
|
||
else
|
||
{
|
||
// 计算期望的高度变化率
|
||
double remainingHeight = endZ - referenceZ;
|
||
double desiredChange = remainingHeight / remainingSteps;
|
||
targetZ = referenceZ + desiredChange;
|
||
}
|
||
}
|
||
|
||
// 选择最接近目标Z且满足物体高度要求的层
|
||
HeightLayer? bestLayer = null;
|
||
double minDistance = double.MaxValue;
|
||
|
||
foreach (var layer in layers)
|
||
{
|
||
if (!layer.IsWalkable)
|
||
continue;
|
||
|
||
if (layer.PassableHeight.GetSpan() < objectHeight)
|
||
continue;
|
||
|
||
double distance = Math.Abs(layer.Z - targetZ);
|
||
if (distance < minDistance)
|
||
{
|
||
minDistance = distance;
|
||
bestLayer = layer;
|
||
}
|
||
}
|
||
|
||
return bestLayer;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将A*返回的米坐标转换为网格坐标（统一方法）
|
||
/// </summary>
|
||
/// <param name="astarPosition">A*位置（米坐标）</param>
|
||
/// <param name="gridMap">网格地图</param>
|
||
/// <returns>网格坐标</returns>
|
||
private GridPoint2D ConvertAStarPositionToGrid(Position astarPosition, GridMap gridMap)
|
||
{
|
||
double cellSizeInMeters = UnitsConverter.ConvertToMeters(gridMap.CellSize);
|
||
int gridX = (int)Math.Round(astarPosition.X / cellSizeInMeters);
|
||
int gridY = (int)Math.Round(astarPosition.Y / cellSizeInMeters);
|
||
|
||
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 == Roy_T.AStar.Paths.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 PathFindingResult.Failure("A*算法未找到路径，起点和终点之间被障碍物阻断");
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"[A*执行] A*算法执行失败: {ex.Message}");
|
||
throw new AutoPathPlanningException($"A*算法执行失败: {ex.Message}", ex);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证路径中所有点的高度约束（特别检查门网格）
|
||
/// </summary>
|
||
/// <param name="pathWithGridCoords">路径点和对应的网格坐标</param>
|
||
/// <param name="gridMap">网格地图</param>
|
||
/// <param name="objectHeight">物体高度（模型单位）</param>
|
||
private void ValidatePathHeightConstraints(List<(Point3D point, GridPoint2D gridPos)> pathWithGridCoords, GridMap gridMap, double objectHeight)
|
||
{
|
||
LogManager.Info($"[路径高度验证] 开始验证 {pathWithGridCoords.Count} 个路径点的高度约束");
|
||
|
||
int checkedPoints = 0;
|
||
int passedPoints = 0;
|
||
int failedPoints = 0;
|
||
int doorPoints = 0;
|
||
|
||
for (int i = 0; i < pathWithGridCoords.Count; i++)
|
||
{
|
||
var (point, gridPos) = pathWithGridCoords[i];
|
||
|
||
if (gridMap.IsValidGridPosition(gridPos))
|
||
{
|
||
var cell = gridMap.Cells[gridPos.X, gridPos.Y];
|
||
checkedPoints++;
|
||
|
||
// 特别标记门网格点
|
||
if (cell.CellType == "门")
|
||
{
|
||
doorPoints++;
|
||
LogManager.Info($"[路径高度验证] 检查路径点{i}（门网格）: ({point.X:F2}, {point.Y:F2}, {point.Z:F2}) -> 网格({gridPos.X},{gridPos.Y})");
|
||
}
|
||
|
||
// 进行高度约束检查
|
||
bool isPassable = IsPointPassableAtHeight(gridPos, point, gridMap, objectHeight);
|
||
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;
|
||
|
||
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}");
|
||
}
|
||
|
||
// 打印最终返回给用户的路径点详情
|
||
LogManager.Info($"[最终路径详情] 共 {pathResult.PathPoints.Count} 个点：");
|
||
for (int i = 0; i < pathResult.PathPoints.Count; i++)
|
||
{
|
||
var pt = pathResult.PathPoints[i];
|
||
LogManager.Info($" [最终点{i}] 位置=({pt.X:F2}, {pt.Y:F2}, Z={pt.Z:F2})");
|
||
}
|
||
|
||
return pathResult;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"[路径优化] 优化失败，使用原始路径: {ex.Message}", ex);
|
||
return pathResult; // 出错时返回原始路径
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 安全优先的A*网格转换方法
|
||
/// 🔥 重写：参照局部直线优先算法的成功模式，重新构建网格连接
|
||
/// </summary>
|
||
private (Grid grid, Dictionary<GridPoint2D, double> activeLayerZ) ConvertToAStarGridSafestCenter(GridMap gridMap, ChannelCoverage channelCoverage, double objectHeight, Point3D startPos, Point3D endPos)
|
||
{
|
||
try
|
||
{
|
||
LogManager.Info($"[安全优先] 开始实施安全优先算法，参照成功的直线优先模式");
|
||
|
||
// 初始化激活高度层字典和目标高度
|
||
var activeLayerZ = new Dictionary<GridPoint2D, double>();
|
||
double targetZ = gridMap.GetWorldElevation(endPos);
|
||
|
||
// 清空日志记录集合，确保每次执行都能看到映射关系
|
||
_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);
|
||
grid.DisconnectNode(pos);
|
||
}
|
||
}
|
||
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];
|
||
|
||
// 检查是否有兼容的高度层
|
||
if (!HasCompatibleLayer(cell, objectHeight))
|
||
{
|
||
if (cell.HasAnyWalkableLayer())
|
||
{
|
||
heightConstrainedCells++;
|
||
}
|
||
continue;
|
||
}
|
||
|
||
// 选择当前网格的最佳高度层
|
||
var currentZ = SelectBestLayerForTarget(cell.HeightLayers, targetZ, objectHeight);
|
||
if (!currentZ.HasValue)
|
||
{
|
||
continue;
|
||
}
|
||
|
||
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, objectHeight))
|
||
{
|
||
// 选择右侧网格的最佳高度层
|
||
var rightZ = SelectBestLayerForTarget(rightCell.HeightLayers, targetZ, objectHeight);
|
||
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 baseSafetySpeed = CalculateSpeedBySafetyDistance(maxSafetyDistance);
|
||
|
||
// 添加高度惩罚：离目标高度越远，速度越慢
|
||
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[finalSpeed] = 1;
|
||
|
||
// 添加单向边
|
||
grid.AddEdge(pos, rightPos, rightVelocity);
|
||
safetyConnections++;
|
||
}
|
||
}
|
||
}
|
||
|
||
// 🔥 核心改动：下方向连接（基于安全距离）
|
||
if (y + 1 < gridMap.Height)
|
||
{
|
||
var bottomCell = gridMap.Cells[x, y + 1];
|
||
|
||
// 检查下方网格是否有兼容的高度层
|
||
if (HasCompatibleLayer(bottomCell, objectHeight))
|
||
{
|
||
// 选择下方网格的最佳高度层
|
||
var bottomZ = SelectBestLayerForTarget(bottomCell.HeightLayers, targetZ, objectHeight);
|
||
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 baseSafetySpeed = CalculateSpeedBySafetyDistance(maxSafetyDistance);
|
||
|
||
// 添加高度惩罚：离目标高度越远，速度越慢
|
||
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[finalSpeed] = 1;
|
||
|
||
// 添加单向边
|
||
grid.AddEdge(pos, bottomPos, bottomVelocity);
|
||
safetyConnections++;
|
||
}
|
||
}
|
||
}
|
||
|
||
// 🔥 连通性修复：左方向连接（基于安全距离）
|
||
if (x - 1 >= 0)
|
||
{
|
||
var leftCell = gridMap.Cells[x - 1, y];
|
||
|
||
// 检查左侧网格是否有兼容的高度层
|
||
if (HasCompatibleLayer(leftCell, objectHeight))
|
||
{
|
||
// 选择左侧网格的最佳高度层
|
||
var leftZ = SelectBestLayerForTarget(leftCell.HeightLayers, targetZ, objectHeight);
|
||
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 baseSafetySpeed = CalculateSpeedBySafetyDistance(maxSafetyDistance);
|
||
|
||
// 添加高度惩罚：离目标高度越远，速度越慢
|
||
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[finalSpeed] = 1;
|
||
|
||
// 添加单向边
|
||
grid.AddEdge(pos, leftPos, leftVelocity);
|
||
safetyConnections++;
|
||
}
|
||
}
|
||
}
|
||
|
||
// 🔥 连通性修复：上方向连接（基于安全距离）
|
||
if (y - 1 >= 0)
|
||
{
|
||
var topCell = gridMap.Cells[x, y - 1];
|
||
|
||
// 检查上方网格是否有兼容的高度层
|
||
if (HasCompatibleLayer(topCell, objectHeight))
|
||
{
|
||
// 选择上方网格的最佳高度层
|
||
var topZ = SelectBestLayerForTarget(topCell.HeightLayers, targetZ, objectHeight);
|
||
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 baseSafetySpeed = CalculateSpeedBySafetyDistance(maxSafetyDistance);
|
||
|
||
// 添加高度惩罚：离目标高度越远，速度越慢
|
||
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[finalSpeed] = 1;
|
||
|
||
// 添加单向边
|
||
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}%");
|
||
LogManager.Info($"[安全优先] - 激活层记录数: {activeLayerZ.Count}");
|
||
|
||
// 输出速度统计信息
|
||
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, activeLayerZ);
|
||
}
|
||
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.HasAnyWalkableLayer();
|
||
|
||
distanceMap[x, y] = isUnsafeArea ? 0 : int.MaxValue;
|
||
|
||
// 统计网格类型
|
||
if (cell.CellType == "障碍物")
|
||
obstacleCount++;
|
||
else if (cell.CellType == "空洞")
|
||
unknownCount++;
|
||
else if (cell.HasAnyWalkableLayer())
|
||
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()}");
|
||
}
|
||
}
|
||
}
|
||
}
|