完善A*算法，用graph代替grid，用3D来进行路径规划。
This commit is contained in:
parent
ed7bc13866
commit
88712cc156
@ -158,7 +158,8 @@
|
||||
"Bash(git restore:*)",
|
||||
"Read(//c/Program Files/Autodesk/Navisworks Manage 2026/**)",
|
||||
"Bash(where:*)",
|
||||
"Read(//c/Users/Tellme/Pictures/Screenshots/**)"
|
||||
"Read(//c/Users/Tellme/Pictures/Screenshots/**)",
|
||||
"Bash(./deploy-plugin.bat)"
|
||||
],
|
||||
"deny": [],
|
||||
"additionalDirectories": [
|
||||
|
||||
@ -43,21 +43,464 @@ namespace NavisworksTransport.PathPlanning
|
||||
/// <summary>
|
||||
/// 自动路径查找器
|
||||
/// 基于A*算法的路径规划核心类
|
||||
/// 支持传统模式和2.5D高度约束的路径规划
|
||||
/// 支持传统模式和真3D图路径规划
|
||||
/// </summary>
|
||||
public class AutoPathFinder
|
||||
{
|
||||
/// <summary>
|
||||
/// 2.5D路径规划（使用通道覆盖数据、高度约束和路径策略）
|
||||
/// 水平边连接允许的最大高度差（米）
|
||||
/// 足以支持台阶连接（约0.15米）和地面到楼梯底部的连接（约0.3米）
|
||||
/// 使用时需转换为模型单位
|
||||
/// </summary>
|
||||
private const double MAX_HEIGHT_DIFF_METERS = 0.35;
|
||||
|
||||
/// <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 CategoryAttributeManager.LogisticsElementType 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>
|
||||
/// <param name="start">起点（世界坐标）</param>
|
||||
/// <param name="end">终点（世界坐标）</param>
|
||||
/// <param name="gridMap">网格地图</param>
|
||||
/// <param name="channelCoverage">通道覆盖数据</param>
|
||||
/// <param name="vehicleHeight">车辆高度（模型单位）</param>
|
||||
/// <param name="strategy">路径规划策略</param>
|
||||
/// <returns>路径查找结果</returns>
|
||||
public PathFindingResult FindPath(Point3D start, Point3D end, GridMap gridMap, ChannelCoverage channelCoverage, double vehicleHeight, PathStrategy strategy = PathStrategy.Shortest)
|
||||
{
|
||||
try
|
||||
{
|
||||
LogManager.Info("[3D路径规划] 开始执行");
|
||||
|
||||
// 构建3D图
|
||||
var graph3D = BuildGraphWithStrategy(gridMap, channelCoverage, vehicleHeight, 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}");
|
||||
return new PathFindingResult
|
||||
{
|
||||
PathPoints = new List<Point3D> { start, end },
|
||||
IsComplete = false,
|
||||
OriginalEndPoint = end,
|
||||
ActualEndPoint = start,
|
||||
CompletionPercentage = 0
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 构建3D图（每个(x,y,layerIndex)作为独立节点）
|
||||
/// </summary>
|
||||
private Graph3DResult BuildGraph3D(GridMap gridMap, ChannelCoverage channelCoverage, double vehicleHeight, Point3D startPos, Point3D endPos)
|
||||
{
|
||||
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 maxHeightDiffInModelUnits = MAX_HEIGHT_DIFF_METERS * UnitsConverter.GetMetersToUnitsConversionFactor();
|
||||
LogManager.Info($"[3D图构建] 高度差阈值: {MAX_HEIGHT_DIFF_METERS}米 = {maxHeightDiffInModelUnits: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.IsWalkable || cell.HeightLayers.Count == 0)
|
||||
continue;
|
||||
|
||||
// 为每个高度层创建独立节点
|
||||
for (int li = 0; li < cell.HeightLayers.Count; li++)
|
||||
{
|
||||
var layer = cell.HeightLayers[li];
|
||||
|
||||
// 楼梯/电梯区域跳过层0（下方楼板，不是可行走表面）
|
||||
if (li == 0 && (layer.Type == CategoryAttributeManager.LogisticsElementType.楼梯 ||
|
||||
layer.Type == CategoryAttributeManager.LogisticsElementType.电梯))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
if (layer.PassableHeight.GetSpan() < vehicleHeight)
|
||||
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.IsWalkable || cell.HeightLayers.Count == 0)
|
||||
continue;
|
||||
|
||||
for (int li = 0; li < cell.HeightLayers.Count; li++)
|
||||
{
|
||||
var currentLayer = cell.HeightLayers[li];
|
||||
if (currentLayer.PassableHeight.GetSpan() < vehicleHeight)
|
||||
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.IsWalkable)
|
||||
continue;
|
||||
|
||||
// 查找邻居中所有满足高度约束的层
|
||||
for (int nli = 0; nli < neighborCell.HeightLayers.Count; nli++)
|
||||
{
|
||||
var neighborLayer = neighborCell.HeightLayers[nli];
|
||||
if (neighborLayer.PassableHeight.GetSpan() < vehicleHeight)
|
||||
continue;
|
||||
|
||||
double heightDiff = Math.Abs(neighborLayer.Z - currentZ);
|
||||
if (heightDiff > maxHeightDiffInModelUnits)
|
||||
continue;
|
||||
|
||||
if (!nodes.TryGetValue((nx, ny, nli), out var neighborNode))
|
||||
continue;
|
||||
|
||||
float speed = CalculateSpeedByHeightDiff(heightDiff, baseSpeed, maxHeightDiffInModelUnits);
|
||||
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 == CategoryAttributeManager.LogisticsElementType.楼梯 ||
|
||||
layer.Type == CategoryAttributeManager.LogisticsElementType.电梯);
|
||||
|
||||
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 != CategoryAttributeManager.LogisticsElementType.楼梯 &&
|
||||
layer1.Type != CategoryAttributeManager.LogisticsElementType.电梯)
|
||||
continue;
|
||||
|
||||
if (layer1.PassableHeight.GetSpan() < vehicleHeight ||
|
||||
layer2.PassableHeight.GetSpan() < vehicleHeight)
|
||||
continue;
|
||||
|
||||
double heightDiff = Math.Abs(layer2.Z - layer1.Z);
|
||||
if (heightDiff > maxHeightDiffInModelUnits)
|
||||
continue;
|
||||
|
||||
if (!nodes.TryGetValue((x2, y2, li), out var node1) ||
|
||||
!nodes.TryGetValue((x2, y2, li + 1), out var node2))
|
||||
continue;
|
||||
|
||||
// 垂直移动速度：电梯较快，楼梯较慢
|
||||
float verticalSpeed = layer1.Type == CategoryAttributeManager.LogisticsElementType.电梯
|
||||
? (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 * 0.8); // 缓坡 (≤0.25m)
|
||||
if (heightRatio <= 1.0) return (float)(baseSpeed * 0.5); // 楼梯 (≤0.5m)
|
||||
return (float)(baseSpeed * 0.1); // 过陡 (>0.5m)
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据策略构建图
|
||||
/// </summary>
|
||||
private Graph3DResult BuildGraphWithStrategy(GridMap gridMap, ChannelCoverage channelCoverage, double vehicleHeight, Point3D startPos, Point3D endPos, PathStrategy strategy)
|
||||
{
|
||||
return BuildGraph3D(gridMap, channelCoverage, vehicleHeight, startPos, endPos);
|
||||
}
|
||||
|
||||
/// <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节点
|
||||
var startNode = FindClosest3DNode(graph3D, startGridPos, start.Z);
|
||||
var endNode = FindClosest3DNode(graph3D, endGridPos, end.Z);
|
||||
|
||||
if (startNode == null || endNode == null)
|
||||
{
|
||||
LogManager.Error($"[A*执行-3D] 无法找到起点或终点节点");
|
||||
return new PathFindingResult
|
||||
{
|
||||
PathPoints = new List<Point3D> { start, end },
|
||||
IsComplete = false,
|
||||
OriginalEndPoint = end,
|
||||
ActualEndPoint = start,
|
||||
CompletionPercentage = 0
|
||||
};
|
||||
}
|
||||
|
||||
var startInfo = graph3D.NodeInfo[startNode];
|
||||
var endInfo = graph3D.NodeInfo[endNode];
|
||||
LogManager.Info($"[A*执行-3D] 起点映射: ({startGridPos.X},{startGridPos.Y},Z={start.Z:F2}) -> 节点({startInfo.X},{startInfo.Y},层{startInfo.LayerIndex},Z={startInfo.Z:F2})");
|
||||
LogManager.Info($"[A*执行-3D] 终点映射: ({endGridPos.X},{endGridPos.Y},Z={end.Z:F2}) -> 节点({endInfo.X},{endInfo.Y},层{endInfo.LayerIndex},Z={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] 未找到路径");
|
||||
return new PathFindingResult
|
||||
{
|
||||
PathPoints = new List<Point3D> { start, end },
|
||||
IsComplete = false,
|
||||
OriginalEndPoint = end,
|
||||
ActualEndPoint = start,
|
||||
CompletionPercentage = 0
|
||||
};
|
||||
}
|
||||
|
||||
// 转换为世界坐标路径
|
||||
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;
|
||||
|
||||
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.Info($"[路径点{pointIndex}] 原始起点: ({originalStart.X:F2}, {originalStart.Y:F2}, Z={originalStart.Z: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 firstPos = gridMap.GridToWorld2D(new GridPoint2D(firstInfo.X, firstInfo.Y));
|
||||
var firstPoint = new Point3D(firstPos.X, firstPos.Y, firstInfo.Z);
|
||||
|
||||
// 如果与起点高度不同，添加这个节点（避免直接跳跃）
|
||||
if (Math.Abs(firstPoint.Z - originalStart.Z) > 0.01)
|
||||
{
|
||||
path3D.Add(firstPoint);
|
||||
LogManager.Info($"[路径点{pointIndex}] A*起点: 网格({firstInfo.X},{firstInfo.Y}), 层{firstInfo.LayerIndex}, Z={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];
|
||||
var worldPos = gridMap.GridToWorld2D(new GridPoint2D(nodeInfo.X, nodeInfo.Y));
|
||||
path3D.Add(new Point3D(worldPos.X, worldPos.Y, nodeInfo.Z));
|
||||
|
||||
LogManager.Info($"[路径点{pointIndex}] 网格({nodeInfo.X},{nodeInfo.Y}), 层{nodeInfo.LayerIndex}, Z={nodeInfo.Z:F2}, 类型={nodeInfo.CellType}");
|
||||
pointIndex++;
|
||||
}
|
||||
|
||||
path3D.Add(originalEnd);
|
||||
LogManager.Info($"[路径点{pointIndex}] 原始终点: ({originalEnd.X:F2}, {originalEnd.Y:F2}, Z={originalEnd.Z:F2})");
|
||||
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 vehicleHeight, PathStrategy strategy = PathStrategy.Shortest)
|
||||
{
|
||||
try
|
||||
{
|
||||
@ -944,21 +1387,13 @@ namespace NavisworksTransport.PathPlanning
|
||||
var distance = Math.Sqrt(Math.Pow(endPoint.X - startPoint.X, 2) +
|
||||
Math.Pow(endPoint.Y - startPoint.Y, 2));
|
||||
|
||||
// 🔥 新增：在尝试连接远点之前，先检查是否会跳过高度变化点
|
||||
bool canSkip = true;
|
||||
const double heightTolerance = 1e-6; // 与PathOptimizer保持一致
|
||||
// 🔥 关键修复：检查起点和终点的高度差，如果超过阈值就不能跳过
|
||||
// 这样可以防止从地面直接跳到楼梯上层
|
||||
double totalHeightDiff = Math.Abs(endPoint.Z - startPoint.Z);
|
||||
const double maxAllowedHeightDiffMeters = 0.3; // 0.3米，允许小台阶但不允许跨越整个楼梯
|
||||
double maxAllowedHeightDiffInModelUnits = maxAllowedHeightDiffMeters * UnitsConverter.GetMetersToUnitsConversionFactor();
|
||||
|
||||
for (int checkIdx = currentIndex + 1; checkIdx < testIndex; checkIdx++)
|
||||
{
|
||||
// 检查被跳过的点是否有高度变化
|
||||
if (Math.Abs(optimizedPath[checkIdx].Z - optimizedPath[checkIdx-1].Z) > heightTolerance ||
|
||||
(checkIdx + 1 < optimizedPath.Count &&
|
||||
Math.Abs(optimizedPath[checkIdx+1].Z - optimizedPath[checkIdx].Z) > heightTolerance))
|
||||
{
|
||||
canSkip = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
bool canSkip = totalHeightDiff <= maxAllowedHeightDiffInModelUnits;
|
||||
|
||||
if (canSkip && IsDirectPathClear(startPoint, endPoint, gridMap))
|
||||
{
|
||||
@ -997,6 +1432,14 @@ namespace NavisworksTransport.PathPlanning
|
||||
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)
|
||||
@ -1851,6 +2294,7 @@ namespace NavisworksTransport.PathPlanning
|
||||
tempRoute.Points.Add(pathPoint);
|
||||
}
|
||||
|
||||
// 使用基于网格的路径优化算法
|
||||
// 使用基于网格的路径优化算法
|
||||
if (gridMap != null)
|
||||
{
|
||||
@ -1874,6 +2318,8 @@ namespace NavisworksTransport.PathPlanning
|
||||
pathResult.PathPoints = optimizedPoints;
|
||||
|
||||
// 🔥 步骤2：在现有优化基础上应用专门的斜线优化
|
||||
// 2D场景：高度差为0，不影响原有逻辑
|
||||
// 3D场景：通过高度差检查保护楼梯路径
|
||||
var diagonalOptimizedPoints = ApplyDiagonalOptimization(optimizedPoints, gridMap);
|
||||
|
||||
if (diagonalOptimizedPoints.Count != optimizedPoints.Count)
|
||||
@ -1892,6 +2338,14 @@ namespace NavisworksTransport.PathPlanning
|
||||
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)
|
||||
|
||||
@ -60,17 +60,31 @@ namespace NavisworksTransport.PathPlanning
|
||||
{
|
||||
LogManager.Info("[通道网格构建器] 开始构建通道覆盖网格");
|
||||
|
||||
// 1. 获取所有通道物品
|
||||
var channelItems = CategoryAttributeManager.GetLogisticsItemsByType(
|
||||
// 1. 获取所有通行相关的物品（通道 + 楼梯 + 电梯）
|
||||
var channelItems = new List<ModelItem>();
|
||||
|
||||
// 获取通道
|
||||
var channels = CategoryAttributeManager.GetLogisticsItemsByType(
|
||||
CategoryAttributeManager.LogisticsElementType.通道);
|
||||
|
||||
channelItems.AddRange(channels);
|
||||
|
||||
// 获取楼梯
|
||||
var stairs = CategoryAttributeManager.GetLogisticsItemsByType(
|
||||
CategoryAttributeManager.LogisticsElementType.楼梯);
|
||||
channelItems.AddRange(stairs);
|
||||
|
||||
// 获取电梯
|
||||
var elevators = CategoryAttributeManager.GetLogisticsItemsByType(
|
||||
CategoryAttributeManager.LogisticsElementType.电梯);
|
||||
channelItems.AddRange(elevators);
|
||||
|
||||
if (!channelItems.Any())
|
||||
{
|
||||
LogManager.Warning("[通道网格构建器] 未找到任何通道物品");
|
||||
LogManager.Warning("[通道网格构建器] 未找到任何通行物品（通道/楼梯/电梯）");
|
||||
return CreateEmptyChannelCoverage();
|
||||
}
|
||||
|
||||
LogManager.Info($"[通道网格构建器] 找到 {channelItems.Count} 个通道物品");
|
||||
LogManager.Info($"[通道网格构建器] 找到通行物品: 通道{channels.Count}个, 楼梯{stairs.Count}个, 电梯{elevators.Count}个, 总计{channelItems.Count}个");
|
||||
|
||||
// 2. 计算通道总边界
|
||||
var totalBounds = BoundingBoxGeometryUtils.CalculateTotalBounds(channelItems);
|
||||
|
||||
@ -151,13 +151,19 @@ namespace NavisworksTransport.PathPlanning
|
||||
var worldPath = points.Select(p => p.Position).ToList();
|
||||
var gridPath = worldPath.Select(p => gridMap.WorldToGrid(p)).ToList();
|
||||
|
||||
// 步骤1：先去除重复点，但保护限速边界
|
||||
// 步骤1：先去除重复点，但保护限速边界和多层高度
|
||||
var dedupedGridPath = new List<GridPoint2D>();
|
||||
var dedupedPoints = new List<PathPoint>();
|
||||
for (int i = 0; i < gridPath.Count; i++)
|
||||
{
|
||||
// 🔥 关键修复：即使位置相同，如果限速不同也不能去重
|
||||
bool shouldKeep = i == 0 || !gridPath[i].Equals(gridPath[i-1]) || HasSpeedLimitChange(points[i-1], points[i]);
|
||||
// 🔥 关键修复：即使网格坐标相同，如果限速不同或高度不同也不能去重
|
||||
// 2D场景：hasHeightChange为false，不影响原有逻辑
|
||||
// 3D场景：保留同网格不同高度的点
|
||||
bool hasGridChange = i == 0 || !gridPath[i].Equals(gridPath[i-1]);
|
||||
bool hasSpeedChange = i > 0 && HasSpeedLimitChange(points[i-1], points[i]);
|
||||
bool hasHeightChange = i > 0 && Math.Abs(points[i].Position.Z - points[i-1].Position.Z) > 1e-6;
|
||||
|
||||
bool shouldKeep = hasGridChange || hasSpeedChange || hasHeightChange;
|
||||
|
||||
if (shouldKeep)
|
||||
{
|
||||
@ -165,9 +171,12 @@ namespace NavisworksTransport.PathPlanning
|
||||
dedupedPoints.Add(points[i]);
|
||||
}
|
||||
}
|
||||
LogManager.Info($"[路径优化] 去重完成：{gridPath.Count} -> {dedupedPoints.Count} 个点");
|
||||
|
||||
// 步骤2：基于去重后的网格路径进行方向优化
|
||||
var simplified = new List<GridPoint2D> { dedupedGridPath[0] };
|
||||
// 步骤2：基于去重后的路径进行方向优化
|
||||
// 🔥 关键修复：不使用网格坐标，直接使用索引来跟踪保留的点
|
||||
// 这样可以避免同一网格坐标的不同高度点被混淆
|
||||
var simplifiedIndices = new List<int> { 0 }; // 保留起点索引
|
||||
|
||||
// 获取初始方向（归一化）
|
||||
int initialDx = dedupedGridPath[1].X - dedupedGridPath[0].X;
|
||||
@ -177,7 +186,8 @@ namespace NavisworksTransport.PathPlanning
|
||||
initialDy == 0 ? 0 : Math.Sign(initialDy)
|
||||
);
|
||||
|
||||
// 简化路径：只保留转弯点、高度变化点和限速变化点（使用归一化方向）
|
||||
// 简化路径：只保留转弯点、高度变化点和限速变化点
|
||||
bool forceKeepNext = false;
|
||||
for (int i = 2; i < dedupedGridPath.Count; i++)
|
||||
{
|
||||
// 计算当前线段的位移
|
||||
@ -190,14 +200,14 @@ namespace NavisworksTransport.PathPlanning
|
||||
dy == 0 ? 0 : Math.Sign(dy)
|
||||
);
|
||||
|
||||
// 检查高度变化
|
||||
// 检查高度变化（2D场景下始终为false，不影响原有逻辑）
|
||||
bool hasHeightChange = HasHeightChange(
|
||||
dedupedPoints[i-2],
|
||||
dedupedPoints[i-1],
|
||||
dedupedPoints[i]
|
||||
);
|
||||
|
||||
// 🔥 关键修复：检查限速变化
|
||||
// 检查限速变化
|
||||
bool hasSpeedLimitChange = HasSpeedLimitChange(
|
||||
dedupedPoints[i-2],
|
||||
dedupedPoints[i-1],
|
||||
@ -205,37 +215,36 @@ namespace NavisworksTransport.PathPlanning
|
||||
);
|
||||
|
||||
// 保留转折点、高度变化点或限速变化点
|
||||
if (!currentDirection.Equals(prevDirection) || hasHeightChange || hasSpeedLimitChange)
|
||||
{
|
||||
simplified.Add(dedupedGridPath[i - 1]);
|
||||
prevDirection = currentDirection;
|
||||
bool shouldKeep = !currentDirection.Equals(prevDirection) || hasHeightChange || hasSpeedLimitChange || forceKeepNext;
|
||||
|
||||
if (hasHeightChange)
|
||||
{
|
||||
LogManager.Debug($"[网格路径优化] 保留高度变化点 {i-1}");
|
||||
}
|
||||
if (hasSpeedLimitChange)
|
||||
{
|
||||
LogManager.Debug($"[网格路径优化] 保留限速变化点 {i-1}");
|
||||
}
|
||||
if (shouldKeep)
|
||||
{
|
||||
simplifiedIndices.Add(i - 1); // 保留索引而不是网格坐标
|
||||
prevDirection = currentDirection;
|
||||
}
|
||||
|
||||
// 🔥 如果当前检测到高度变化，下一个点也必须保留
|
||||
forceKeepNext = hasHeightChange;
|
||||
}
|
||||
|
||||
// 添加终点
|
||||
simplified.Add(dedupedGridPath[dedupedGridPath.Count - 1]);
|
||||
// 添加终点索引
|
||||
simplifiedIndices.Add(dedupedGridPath.Count - 1);
|
||||
|
||||
LogManager.Info($"[网格路径优化] 最终优化完成：{gridPath.Count} -> {dedupedGridPath.Count}(去重) -> {simplified.Count}(简化) 个点");
|
||||
LogManager.Info($"[网格路径优化] 简化完成：{dedupedGridPath.Count} -> {simplifiedIndices.Count} 个点");
|
||||
|
||||
// 步骤3：转换回PathPoint格式
|
||||
// 步骤3：根据索引提取PathPoint
|
||||
var simplifiedPoints = new List<PathPoint>();
|
||||
foreach (var gridPoint in simplified)
|
||||
foreach (int index in simplifiedIndices)
|
||||
{
|
||||
// 在去重后的路径中找到对应的点
|
||||
int originalIndex = dedupedGridPath.FindIndex(g => g.Equals(gridPoint));
|
||||
if (originalIndex >= 0)
|
||||
{
|
||||
simplifiedPoints.Add(dedupedPoints[originalIndex]);
|
||||
}
|
||||
simplifiedPoints.Add(dedupedPoints[index]);
|
||||
}
|
||||
|
||||
// 打印优化后的路径点详情
|
||||
LogManager.Info($"[优化后路径详情] 共 {simplifiedPoints.Count} 个点：");
|
||||
for (int i = 0; i < simplifiedPoints.Count; i++)
|
||||
{
|
||||
var pt = simplifiedPoints[i];
|
||||
LogManager.Info($" [优化后点{i}] 位置=({pt.Position.X:F2}, {pt.Position.Y:F2}, Z={pt.Position.Z:F2}), 限速={pt.SpeedLimit:F1}");
|
||||
}
|
||||
|
||||
return simplifiedPoints;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user