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

统一使用网格左下角坐标系，与A*算法库保持一致

Browse Source
This commit is contained in:
tian 2025-09-11 03:14:02 +08:00
parent 3b5d5963e5
commit 4e43fb89b3
4 changed files with 33 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
src/PathPlanning/AutoPathFinder.cs
View File

@ -632,15 +632,15 @@ namespace NavisworksTransport.PathPlanning
{ {
// 添加第一条边的起点 // 添加第一条边的起点
var startNode = astarPath.Edges[0].Start; var startNode = astarPath.Edges[0].Start;
var startGridX = (int)Math.Round(startNode.Position.X / cellSizeInMeters); var startGridX = (int)Math.Floor(startNode.Position.X / cellSizeInMeters);
var startGridY = (int)Math.Round(startNode.Position.Y / cellSizeInMeters); var startGridY = (int)Math.Floor(startNode.Position.Y / cellSizeInMeters);
gridPath.Add(new GridPoint2D(startGridX, startGridY)); gridPath.Add(new GridPoint2D(startGridX, startGridY));
// 添加每条边的终点（避免重复） // 添加每条边的终点（避免重复）
foreach (var edge in astarPath.Edges) foreach (var edge in astarPath.Edges)
{ {
var gridX = (int)Math.Round(edge.End.Position.X / cellSizeInMeters); var gridX = (int)Math.Floor(edge.End.Position.X / cellSizeInMeters);
var gridY = (int)Math.Round(edge.End.Position.Y / cellSizeInMeters); var gridY = (int)Math.Floor(edge.End.Position.Y / cellSizeInMeters);
var gridPoint = new GridPoint2D(gridX, gridY); var gridPoint = new GridPoint2D(gridX, gridY);
// 检查是否与上一个点重复（理论上A*不应该产生重复，但保险起见） // 检查是否与上一个点重复（理论上A*不应该产生重复，但保险起见）
@ -668,18 +668,15 @@ namespace NavisworksTransport.PathPlanning
// 获取单位转换因子（米转世界单位） // 获取单位转换因子（米转世界单位）
double metersToWorldUnit = 1.0 / UnitsConverter.ConvertToMeters(1.0); double metersToWorldUnit = 1.0 / UnitsConverter.ConvertToMeters(1.0);
// 计算半个网格的偏移量（用于从左下角调整到中心）
double halfCellOffset = gridMap.CellSize * 0.5;
// 将A*返回的网格左下角坐标调整到网格中心 // A*返回的是网格左下角坐标，直接使用，不需要偏移
if (astarPath.Edges.Count > 0) if (astarPath.Edges.Count > 0)
{ {
// 添加第一条边的起点（调整到网格中心） // 添加第一条边的起点（网格左下角）
var startNode = astarPath.Edges[0].Start; var startNode = astarPath.Edges[0].Start;
var startWorldPos = new Point3D( var startWorldPos = new Point3D(
gridMap.Origin.X + startNode.Position.X * metersToWorldUnit + halfCellOffset, // 添加半格偏移到中心 gridMap.Origin.X + startNode.Position.X * metersToWorldUnit, // 直接转换，不加偏移
gridMap.Origin.Y + startNode.Position.Y * metersToWorldUnit + halfCellOffset, // 添加半格偏移到中心 gridMap.Origin.Y + startNode.Position.Y * metersToWorldUnit, // 直接转换，不加偏移
0 // Z坐标后续通过高度插值处理 0 // Z坐标后续通过高度插值处理
); );
@ -697,10 +694,10 @@ namespace NavisworksTransport.PathPlanning
// 添加每条边的终点（避免重复） // 添加每条边的终点（避免重复）
foreach (var edge in astarPath.Edges) foreach (var edge in astarPath.Edges)
{ {
// 将A*的米坐标转换为世界坐标，并调整到网格中心 // 将A*的米坐标转换为世界坐标（网格左下角）
var endWorldPos = new Point3D( var endWorldPos = new Point3D(
gridMap.Origin.X + edge.End.Position.X * metersToWorldUnit + halfCellOffset, // 添加半格偏移到中心 gridMap.Origin.X + edge.End.Position.X * metersToWorldUnit, // 直接转换，不加偏移
gridMap.Origin.Y + edge.End.Position.Y * metersToWorldUnit + halfCellOffset, // 添加半格偏移到中心 gridMap.Origin.Y + edge.End.Position.Y * metersToWorldUnit, // 直接转换，不加偏移
0 // Z坐标后续处理 0 // Z坐标后续处理
); );
@ -724,7 +721,7 @@ namespace NavisworksTransport.PathPlanning
} }
} }
LogManager.Info($"[路径转换] 转换完成，生成{worldPath.Count}个世界坐标点（调整到网格中心）"); LogManager.Info($"[路径转换] 转换完成，生成{worldPath.Count}个世界坐标点（网格左下角）");
return worldPath; return worldPath;
} }
@ -1163,8 +1160,9 @@ namespace NavisworksTransport.PathPlanning
try try
{ {
// 将世界坐标转换为网格坐标 // 将世界坐标转换为网格坐标
int gridX = (int)Math.Round((point.X - gridMap.Bounds.Min.X) / gridMap.CellSize); var gridPos = gridMap.WorldToGrid(point);
int gridY = (int)Math.Round((point.Y - gridMap.Bounds.Min.Y) / gridMap.CellSize); int gridX = gridPos.X;
int gridY = gridPos.Y;
LogManager.Info($"[高度检查] 检查点 ({point.X:F2}, {point.Y:F2}, {point.Z:F2}) -> 网格坐标 ({gridX}, {gridY})"); LogManager.Info($"[高度检查] 检查点 ({point.X:F2}, {point.Y:F2}, {point.Z:F2}) -> 网格坐标 ({gridX}, {gridY})");
@ -1276,8 +1274,9 @@ namespace NavisworksTransport.PathPlanning
try try
{ {
// 将世界坐标转换为网格坐标 // 将世界坐标转换为网格坐标
int gridX = (int)Math.Round((point.X - gridMap.Bounds.Min.X) / gridMap.CellSize); var gridPos = gridMap.WorldToGrid(point);
int gridY = (int)Math.Round((point.Y - gridMap.Bounds.Min.Y) / gridMap.CellSize); int gridX = gridPos.X;
int gridY = gridPos.Y;
// 确保坐标在有效范围内 // 确保坐标在有效范围内
if (gridX < 0 || gridX >= gridMap.Width || gridY < 0 || gridY >= gridMap.Height) if (gridX < 0 || gridX >= gridMap.Width || gridY < 0 || gridY >= gridMap.Height)
@ -1363,8 +1362,8 @@ namespace NavisworksTransport.PathPlanning
// 计算完成百分比 // 计算完成百分比
var lastNode = astarPath.Edges.Last().End; var lastNode = astarPath.Edges.Last().End;
var actualEndGridX = (int)Math.Round(lastNode.Position.X / cellSizeInMeters); var actualEndGridX = (int)Math.Floor(lastNode.Position.X / cellSizeInMeters);
var actualEndGridY = (int)Math.Round(lastNode.Position.Y / cellSizeInMeters); var actualEndGridY = (int)Math.Floor(lastNode.Position.Y / cellSizeInMeters);
var actualEndGrid = new GridPoint2D(actualEndGridX, actualEndGridY); var actualEndGrid = new GridPoint2D(actualEndGridX, actualEndGridY);
var actualEndCell = gridMap.GetCell(actualEndGrid); var actualEndCell = gridMap.GetCell(actualEndGrid);
var actualEndWorld = gridMap.GridToWorld3D(actualEndGrid, actualEndCell?.WorldPosition.Z ?? 0); var actualEndWorld = gridMap.GridToWorld3D(actualEndGrid, actualEndCell?.WorldPosition.Z ?? 0);

8
src/PathPlanning/GridMap.cs
View File

@ -181,18 +181,18 @@ namespace NavisworksTransport.PathPlanning
double gridX = (worldPosition.X - Origin.X) / CellSize; double gridX = (worldPosition.X - Origin.X) / CellSize;
double gridY = (worldPosition.Y - Origin.Y) / CellSize; double gridY = (worldPosition.Y - Origin.Y) / CellSize;
return new GridPoint2D((int)Math.Round(gridX), (int)Math.Round(gridY)); return new GridPoint2D((int)Math.Floor(gridX), (int)Math.Floor(gridY));
} }
/// <summary> /// <summary>
/// 将网格坐标转换为世界2D坐标（纯坐标转换，不涉及Z坐标计算） /// 将网格坐标转换为世界2D坐标（纯坐标转换，不涉及Z坐标计算）
/// </summary> /// </summary>
/// <param name="gridPosition">网格坐标</param> /// <param name="gridPosition">网格坐标</param>
/// <returns>世界2D坐标点</returns> /// <returns>世界2D坐标点（网格左下角）</returns>
public Point3D GridToWorld2D(GridPoint2D gridPosition) public Point3D GridToWorld2D(GridPoint2D gridPosition)
{ {
double worldX = Origin.X + (gridPosition.X + 0.5) * CellSize; double worldX = Origin.X + gridPosition.X * CellSize;
double worldY = Origin.Y + (gridPosition.Y + 0.5) * CellSize; double worldY = Origin.Y + gridPosition.Y * CellSize;
return new Point3D(worldX, worldY, 0); // Z设为0，调用者可以替换 return new Point3D(worldX, worldY, 0); // Z设为0，调用者可以替换
} }

13
src/PathPlanning/GridMapGenerator.cs
View File

@ -506,8 +506,9 @@ namespace NavisworksTransport.PathPlanning
var intervals = kvp.Value; var intervals = kvp.Value;
// 将世界坐标转换为网格坐标 // 将世界坐标转换为网格坐标
int gridX = (int)Math.Round((point.X - gridMap.Bounds.Min.X) / gridMap.CellSize); var gridPos = gridMap.WorldToGrid(new Point3D(point.X, point.Y, 0));
int gridY = (int)Math.Round((point.Y - gridMap.Bounds.Min.Y) / gridMap.CellSize); int gridX = gridPos.X;
int gridY = gridPos.Y;
// 确保坐标在有效范围内 // 确保坐标在有效范围内
if (gridX >= 0 && gridX < gridMap.Width && gridY >= 0 && gridY < gridMap.Height) if (gridX >= 0 && gridX < gridMap.Width && gridY >= 0 && gridY < gridMap.Height)
@ -533,8 +534,8 @@ namespace NavisworksTransport.PathPlanning
{ {
// 通道区域有足够净空高度，设置为可通行 // 通道区域有足够净空高度，设置为可通行
// 🔥 关键修复：使用SetCell方法正确更新网格统计 // 🔥 关键修复：使用SetCell方法正确更新网格统计
var gridPos = new GridPoint2D(gridX, gridY); var cellPos = new GridPoint2D(gridX, gridY);
gridMap.SetCell(gridPos, true, 1.0, CategoryAttributeManager.LogisticsElementType.通道); gridMap.SetCell(cellPos, true, 1.0, CategoryAttributeManager.LogisticsElementType.通道);
// 设置世界坐标和高度信息 // 设置世界坐标和高度信息
var updatedCell = gridMap.Cells[gridX, gridY]; var updatedCell = gridMap.Cells[gridX, gridY];
@ -546,8 +547,8 @@ namespace NavisworksTransport.PathPlanning
else else
{ {
// 通道区域但净空高度不足，标记为不可通行但保持通道类型 // 通道区域但净空高度不足，标记为不可通行但保持通道类型
var gridPos = new GridPoint2D(gridX, gridY); var cellPos = new GridPoint2D(gridX, gridY);
gridMap.SetCell(gridPos, false, double.MaxValue, CategoryAttributeManager.LogisticsElementType.通道); gridMap.SetCell(cellPos, false, double.MaxValue, CategoryAttributeManager.LogisticsElementType.通道);
// 设置世界坐标和高度信息 // 设置世界坐标和高度信息
var updatedCell = gridMap.Cells[gridX, gridY]; var updatedCell = gridMap.Cells[gridX, gridY];

4
src/PathPlanning/OptimizedHeightCalculator.cs
View File

@ -326,8 +326,8 @@ namespace NavisworksTransport.PathPlanning
{ {
// 对坐标进行网格对齐 // 对坐标进行网格对齐
double sampleIntervalModel = _sampleInterval * 39.37; double sampleIntervalModel = _sampleInterval * 39.37;
int gridX = (int)Math.Round(worldX / sampleIntervalModel); int gridX = (int)Math.Floor(worldX / sampleIntervalModel);
int gridY = (int)Math.Round(worldY / sampleIntervalModel); int gridY = (int)Math.Floor(worldY / sampleIntervalModel);
return $"{gridX}_{gridY}"; return $"{gridX}_{gridY}";
} }