From 4e43fb89b37c994da0ebb8d7d8b13217d93c929b Mon Sep 17 00:00:00 2001 From: tian <11429339@qq.com> Date: Thu, 11 Sep 2025 03:14:02 +0800 Subject: [PATCH] =?UTF-8?q?=E7=BB=9F=E4=B8=80=E4=BD=BF=E7=94=A8=E7=BD=91?= =?UTF-8?q?=E6=A0=BC=E5=B7=A6=E4=B8=8B=E8=A7=92=E5=9D=90=E6=A0=87=E7=B3=BB?= =?UTF-8?q?=EF=BC=8C=E4=B8=8EA*=E7=AE=97=E6=B3=95=E5=BA=93=E4=BF=9D?= =?UTF-8?q?=E6=8C=81=E4=B8=80=E8=87=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/PathPlanning/AutoPathFinder.cs | 41 +++++++++---------- src/PathPlanning/GridMap.cs | 8 ++-- src/PathPlanning/GridMapGenerator.cs | 13 +++--- src/PathPlanning/OptimizedHeightCalculator.cs | 4 +- 4 files changed, 33 insertions(+), 33 deletions(-) diff --git a/src/PathPlanning/AutoPathFinder.cs b/src/PathPlanning/AutoPathFinder.cs index fda1225..1c834ae 100644 --- a/src/PathPlanning/AutoPathFinder.cs +++ b/src/PathPlanning/AutoPathFinder.cs @@ -632,15 +632,15 @@ namespace NavisworksTransport.PathPlanning { // 添加第一条边的起点 var startNode = astarPath.Edges[0].Start; - var startGridX = (int)Math.Round(startNode.Position.X / cellSizeInMeters); - var startGridY = (int)Math.Round(startNode.Position.Y / cellSizeInMeters); + var startGridX = (int)Math.Floor(startNode.Position.X / cellSizeInMeters); + var startGridY = (int)Math.Floor(startNode.Position.Y / cellSizeInMeters); gridPath.Add(new GridPoint2D(startGridX, startGridY)); // 添加每条边的终点（避免重复） foreach (var edge in astarPath.Edges) { - var gridX = (int)Math.Round(edge.End.Position.X / cellSizeInMeters); - var gridY = (int)Math.Round(edge.End.Position.Y / cellSizeInMeters); + var gridX = (int)Math.Floor(edge.End.Position.X / cellSizeInMeters); + var gridY = (int)Math.Floor(edge.End.Position.Y / cellSizeInMeters); var gridPoint = new GridPoint2D(gridX, gridY); // 检查是否与上一个点重复（理论上A*不应该产生重复，但保险起见） @@ -668,18 +668,15 @@ namespace NavisworksTransport.PathPlanning // 获取单位转换因子（米转世界单位） double metersToWorldUnit = 1.0 / UnitsConverter.ConvertToMeters(1.0); - - // 计算半个网格的偏移量（用于从左下角调整到中心） - double halfCellOffset = gridMap.CellSize * 0.5; - // 将A*返回的网格左下角坐标调整到网格中心 + // A*返回的是网格左下角坐标，直接使用，不需要偏移 if (astarPath.Edges.Count > 0) { - // 添加第一条边的起点（调整到网格中心） + // 添加第一条边的起点（网格左下角） var startNode = astarPath.Edges[0].Start; var startWorldPos = new Point3D( - gridMap.Origin.X + startNode.Position.X * metersToWorldUnit + halfCellOffset, // 添加半格偏移到中心 - gridMap.Origin.Y + startNode.Position.Y * metersToWorldUnit + halfCellOffset, // 添加半格偏移到中心 + gridMap.Origin.X + startNode.Position.X * metersToWorldUnit, // 直接转换，不加偏移 + gridMap.Origin.Y + startNode.Position.Y * metersToWorldUnit, // 直接转换，不加偏移 0 // Z坐标后续通过高度插值处理 ); @@ -697,10 +694,10 @@ namespace NavisworksTransport.PathPlanning // 添加每条边的终点（避免重复） foreach (var edge in astarPath.Edges) { - // 将A*的米坐标转换为世界坐标，并调整到网格中心 + // 将A*的米坐标转换为世界坐标（网格左下角） var endWorldPos = new Point3D( - gridMap.Origin.X + edge.End.Position.X * metersToWorldUnit + halfCellOffset, // 添加半格偏移到中心 - gridMap.Origin.Y + edge.End.Position.Y * metersToWorldUnit + halfCellOffset, // 添加半格偏移到中心 + gridMap.Origin.X + edge.End.Position.X * metersToWorldUnit, // 直接转换，不加偏移 + gridMap.Origin.Y + edge.End.Position.Y * metersToWorldUnit, // 直接转换，不加偏移 0 // Z坐标后续处理 ); @@ -724,7 +721,7 @@ namespace NavisworksTransport.PathPlanning } } - LogManager.Info($"[路径转换] 转换完成，生成{worldPath.Count}个世界坐标点（调整到网格中心）"); + LogManager.Info($"[路径转换] 转换完成，生成{worldPath.Count}个世界坐标点（网格左下角）"); return worldPath; } @@ -1163,8 +1160,9 @@ namespace NavisworksTransport.PathPlanning try { // 将世界坐标转换为网格坐标 - int gridX = (int)Math.Round((point.X - gridMap.Bounds.Min.X) / gridMap.CellSize); - int gridY = (int)Math.Round((point.Y - gridMap.Bounds.Min.Y) / gridMap.CellSize); + var gridPos = gridMap.WorldToGrid(point); + int gridX = gridPos.X; + int gridY = gridPos.Y; LogManager.Info($"[高度检查] 检查点 ({point.X:F2}, {point.Y:F2}, {point.Z:F2}) -> 网格坐标 ({gridX}, {gridY})"); @@ -1276,8 +1274,9 @@ namespace NavisworksTransport.PathPlanning try { // 将世界坐标转换为网格坐标 - int gridX = (int)Math.Round((point.X - gridMap.Bounds.Min.X) / gridMap.CellSize); - int gridY = (int)Math.Round((point.Y - gridMap.Bounds.Min.Y) / gridMap.CellSize); + var gridPos = gridMap.WorldToGrid(point); + int gridX = gridPos.X; + int gridY = gridPos.Y; // 确保坐标在有效范围内 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 actualEndGridX = (int)Math.Round(lastNode.Position.X / cellSizeInMeters); - var actualEndGridY = (int)Math.Round(lastNode.Position.Y / cellSizeInMeters); + var actualEndGridX = (int)Math.Floor(lastNode.Position.X / cellSizeInMeters); + var actualEndGridY = (int)Math.Floor(lastNode.Position.Y / cellSizeInMeters); var actualEndGrid = new GridPoint2D(actualEndGridX, actualEndGridY); var actualEndCell = gridMap.GetCell(actualEndGrid); var actualEndWorld = gridMap.GridToWorld3D(actualEndGrid, actualEndCell?.WorldPosition.Z ?? 0); diff --git a/src/PathPlanning/GridMap.cs b/src/PathPlanning/GridMap.cs index e19dab2..305b73b 100644 --- a/src/PathPlanning/GridMap.cs +++ b/src/PathPlanning/GridMap.cs @@ -181,18 +181,18 @@ namespace NavisworksTransport.PathPlanning double gridX = (worldPosition.X - Origin.X) / 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)); } /// /// 将网格坐标转换为世界2D坐标（纯坐标转换，不涉及Z坐标计算） /// /// 网格坐标 - /// 世界2D坐标点 + /// 世界2D坐标点（网格左下角） public Point3D GridToWorld2D(GridPoint2D gridPosition) { - double worldX = Origin.X + (gridPosition.X + 0.5) * CellSize; - double worldY = Origin.Y + (gridPosition.Y + 0.5) * CellSize; + double worldX = Origin.X + gridPosition.X * CellSize; + double worldY = Origin.Y + gridPosition.Y * CellSize; return new Point3D(worldX, worldY, 0); // Z设为0，调用者可以替换 } diff --git a/src/PathPlanning/GridMapGenerator.cs b/src/PathPlanning/GridMapGenerator.cs index d3b1f70..4afb17f 100644 --- a/src/PathPlanning/GridMapGenerator.cs +++ b/src/PathPlanning/GridMapGenerator.cs @@ -506,8 +506,9 @@ namespace NavisworksTransport.PathPlanning var intervals = kvp.Value; // 将世界坐标转换为网格坐标 - int gridX = (int)Math.Round((point.X - gridMap.Bounds.Min.X) / gridMap.CellSize); - int gridY = (int)Math.Round((point.Y - gridMap.Bounds.Min.Y) / gridMap.CellSize); + var gridPos = gridMap.WorldToGrid(new Point3D(point.X, point.Y, 0)); + int gridX = gridPos.X; + int gridY = gridPos.Y; // 确保坐标在有效范围内 if (gridX >= 0 && gridX < gridMap.Width && gridY >= 0 && gridY < gridMap.Height) @@ -533,8 +534,8 @@ namespace NavisworksTransport.PathPlanning { // 通道区域有足够净空高度，设置为可通行 // 🔥 关键修复：使用SetCell方法正确更新网格统计 - var gridPos = new GridPoint2D(gridX, gridY); - gridMap.SetCell(gridPos, true, 1.0, CategoryAttributeManager.LogisticsElementType.通道); + var cellPos = new GridPoint2D(gridX, gridY); + gridMap.SetCell(cellPos, true, 1.0, CategoryAttributeManager.LogisticsElementType.通道); // 设置世界坐标和高度信息 var updatedCell = gridMap.Cells[gridX, gridY]; @@ -546,8 +547,8 @@ namespace NavisworksTransport.PathPlanning else { // 通道区域但净空高度不足，标记为不可通行但保持通道类型 - var gridPos = new GridPoint2D(gridX, gridY); - gridMap.SetCell(gridPos, false, double.MaxValue, CategoryAttributeManager.LogisticsElementType.通道); + var cellPos = new GridPoint2D(gridX, gridY); + gridMap.SetCell(cellPos, false, double.MaxValue, CategoryAttributeManager.LogisticsElementType.通道); // 设置世界坐标和高度信息 var updatedCell = gridMap.Cells[gridX, gridY]; diff --git a/src/PathPlanning/OptimizedHeightCalculator.cs b/src/PathPlanning/OptimizedHeightCalculator.cs index 4a5d517..693338b 100644 --- a/src/PathPlanning/OptimizedHeightCalculator.cs +++ b/src/PathPlanning/OptimizedHeightCalculator.cs @@ -326,8 +326,8 @@ namespace NavisworksTransport.PathPlanning { // 对坐标进行网格对齐 double sampleIntervalModel = _sampleInterval * 39.37; - int gridX = (int)Math.Round(worldX / sampleIntervalModel); - int gridY = (int)Math.Round(worldY / sampleIntervalModel); + int gridX = (int)Math.Floor(worldX / sampleIntervalModel); + int gridY = (int)Math.Floor(worldY / sampleIntervalModel); return $"{gridX}_{gridY}"; }