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

简化了自动寻路代码

Browse Source
This commit is contained in:
tian 2025-09-30 00:22:28 +08:00
parent 64211439a8
commit 47ade72438
1 changed files with 19 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
src/PathPlanning/AutoPathFinder.cs
View File

@ -100,11 +100,16 @@ namespace NavisworksTransport.PathPlanning
LogManager.Info($"[2.5D路径规划] A*算法找到原始路径，包含 {pathResult.PathPoints.Count} 个点");
// 5. 根据网格点通行高度区间对路径点Z坐标调整
var enhancedPath = ApplyGridHeightConstraints(pathResult.PathPoints, gridMap, vehicleHeight);
// 5. 预先生成路径点和网格坐标的对应关系，避免重复转换
var pathWithGridCoords = pathResult.PathPoints
.Select(point => (point, gridMap.WorldToGrid(point)))
.ToList();
// 6. 验证路径中所有点的高度约束（特别是门网格）
ValidatePathHeightConstraints(enhancedPath, gridMap, vehicleHeight);
// 6. 根据网格点通行高度区间对路径点Z坐标调整
var enhancedPath = ApplyGridHeightConstraints(pathWithGridCoords, gridMap, vehicleHeight);
// 7. 验证路径中所有点的高度约束（直接使用原始网格坐标，因为网格位置未变）
ValidatePathHeightConstraints(pathWithGridCoords, gridMap, vehicleHeight);
// 更新PathFindingResult
pathResult.PathPoints = enhancedPath;
@ -1234,21 +1239,19 @@ namespace NavisworksTransport.PathPlanning
/// <summary>
/// 根据网格高度区间调整路径点Z坐标
/// </summary>
/// <param name="path">原始路径</param>
/// <param name="pathWithGridCoords">路径点和对应的网格坐标</param>
/// <param name="gridMap">网格地图</param>
/// <param name="vehicleHeight">车辆高度（模型单位）</param>
/// <returns>调整后的路径</returns>
private List<Point3D> ApplyGridHeightConstraints(List<Point3D> path, GridMap gridMap, double vehicleHeight)
private List<Point3D> ApplyGridHeightConstraints(List<(Point3D point, GridPoint2D gridPos)> pathWithGridCoords, GridMap gridMap, double vehicleHeight)
{
LogManager.Info($"[应用高度区间] 开始调整，路径点数: {path.Count}");
LogManager.Info($"[应用高度区间] 开始调整，路径点数: {pathWithGridCoords.Count}");
var adjustedPath = new List<Point3D>();
int adjustedCount = 0;
foreach (var point in path)
foreach (var (point, gridPos) in pathWithGridCoords)
{
var gridPos = gridMap.WorldToGrid(point);
// 检查网格位置有效性
if (!gridMap.IsValidGridPosition(gridPos))
{
@ -1273,7 +1276,7 @@ namespace NavisworksTransport.PathPlanning
adjustedCount++;
}
LogManager.Info($"[应用高度区间] 完成，调整了 {adjustedCount}/{path.Count} 个点");
LogManager.Info($"[应用高度区间] 完成，调整了 {adjustedCount}/{pathWithGridCoords.Count} 个点");
return adjustedPath;
}
@ -1285,14 +1288,10 @@ namespace NavisworksTransport.PathPlanning
/// <returns>网格坐标</returns>
private GridPoint2D ConvertAStarPositionToGrid(Position astarPosition, GridMap gridMap)
{
LogManager.Info($"[A*原始坐标] A*返回: ({astarPosition.X:F3}, {astarPosition.Y:F3})");
double cellSizeInMeters = UnitsConverter.ConvertToMeters(gridMap.CellSize);
int gridX = (int)Math.Round(astarPosition.X / cellSizeInMeters);
int gridY = (int)Math.Round(astarPosition.Y / cellSizeInMeters);
LogManager.Info($"[A*转换坐标] 转换结果: ({gridX}, {gridY})");
return new GridPoint2D(gridX, gridY);
}
@ -1397,22 +1396,21 @@ namespace NavisworksTransport.PathPlanning
/// <summary>
/// 验证路径中所有点的高度约束（特别检查门网格）
/// </summary>
/// <param name="path">路径点列表</param>
/// <param name="pathWithGridCoords">路径点和对应的网格坐标</param>
/// <param name="gridMap">网格地图</param>
/// <param name="vehicleHeight">车辆高度（模型单位）</param>
private void ValidatePathHeightConstraints(List<Point3D> path, GridMap gridMap, double vehicleHeight)
private void ValidatePathHeightConstraints(List<(Point3D point, GridPoint2D gridPos)> pathWithGridCoords, GridMap gridMap, double vehicleHeight)
{
LogManager.Info($"[路径高度验证] 开始验证 {path.Count} 个路径点的高度约束");
LogManager.Info($"[路径高度验证] 开始验证 {pathWithGridCoords.Count} 个路径点的高度约束");
int checkedPoints = 0;
int passedPoints = 0;
int failedPoints = 0;
int doorPoints = 0;
for (int i = 0; i < path.Count; i++)
for (int i = 0; i < pathWithGridCoords.Count; i++)
{
var point = path[i];
var gridPos = gridMap.WorldToGrid(point);
var (point, gridPos) = pathWithGridCoords[i];
if (gridMap.IsValidGridPosition(gridPos))
{