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:03:03 +08:00
parent 3f2d66c255
commit 64211439a8
2 changed files with 10 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
src/PathPlanning/AutoPathFinder.cs
View File

@ -68,12 +68,14 @@ namespace NavisworksTransport.PathPlanning
var originalEnd = end;
// 1. 检查起点和终点的高度约束
if (!IsPointPassableAtHeight(start, gridMap, vehicleHeight))
var startGridPos = gridMap.WorldToGrid(start);
if (!IsPointPassableAtHeight(startGridPos, start, gridMap, vehicleHeight))
{
LogManager.Warning("[2.5D路径规划] 起点位置不满足高度约束");
}
if (!IsPointPassableAtHeight(end, gridMap, vehicleHeight))
var endGridPos = gridMap.WorldToGrid(end);
if (!IsPointPassableAtHeight(endGridPos, end, gridMap, vehicleHeight))
{
LogManager.Warning("[2.5D路径规划] 终点位置不满足高度约束");
}
@ -1175,23 +1177,16 @@ namespace NavisworksTransport.PathPlanning
/// <summary>
/// 检查指定点在给定高度约束下是否可通行
/// </summary>
/// <param name="point">检查点</param>
/// <param name="gridPos">网格坐标</param>
/// <param name="point">检查点（世界坐标）</param>
/// <param name="gridMap">网格地图</param>
/// <param name="vehicleHeight">车辆高度（模型单位）</param>
/// <returns>是否可通行</returns>
/// <summary>
/// 检查指定点在给定高度约束下是否可通行
/// </summary>
/// <param name="point">检查点</param>
/// <param name="gridMap">网格地图</param>
/// <param name="vehicleHeight">车辆高度（模型单位）</param>
/// <returns>是否可通行</returns>
private bool IsPointPassableAtHeight(Point3D point, GridMap gridMap, double vehicleHeight)
private bool IsPointPassableAtHeight(GridPoint2D gridPos, Point3D point, GridMap gridMap, double vehicleHeight)
{
try
{
// 将世界坐标转换为网格坐标
var gridPos = gridMap.WorldToGrid(point);
// 使用传入的网格坐标，避免重复转换
int gridX = gridPos.X;
int gridY = gridPos.Y;
@ -1432,7 +1427,7 @@ namespace NavisworksTransport.PathPlanning
}
// 进行高度约束检查
bool isPassable = IsPointPassableAtHeight(point, gridMap, vehicleHeight);
bool isPassable = IsPointPassableAtHeight(gridPos, point, gridMap, vehicleHeight);
if (isPassable)
{
passedPoints++;

2
src/PathPlanning/GridMap.cs
View File

@ -183,7 +183,7 @@ namespace NavisworksTransport.PathPlanning
double gridX = (worldPosition.X - Origin.X) / CellSize;
double gridY = (worldPosition.Y - Origin.Y) / CellSize;
return new GridPoint2D((int)Math.Floor(gridX), (int)Math.Floor(gridY));
return new GridPoint2D((int)Math.Round(gridX), (int)Math.Round(gridY));
}
/// <summary>