From 64211439a85f9e89e196e1e230076b1436301e4a Mon Sep 17 00:00:00 2001
From: tian <11429339@qq.com>
Date: Tue, 30 Sep 2025 00:03:03 +0800
Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E4=BA=86=E4=B8=96=E7=95=8C?=
 =?UTF-8?q?=E5=9D=90=E6=A0=87=E8=BD=AC=E6=8D=A2=E5=88=B0=E7=BD=91=E6=A0=BC?=
 =?UTF-8?q?=E5=9D=90=E6=A0=87=EF=BC=88=E5=9B=9B=E8=88=8D=E4=BA=94=E5=85=A5?=
 =?UTF-8?q?=EF=BC=89?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/PathPlanning/AutoPathFinder.cs | 23 +++++++++--------------
 src/PathPlanning/GridMap.cs        |  2 +-
 2 files changed, 10 insertions(+), 15 deletions(-)

diff --git a/src/PathPlanning/AutoPathFinder.cs b/src/PathPlanning/AutoPathFinder.cs
index cc2cb92..33d4a96 100644
--- a/src/PathPlanning/AutoPathFinder.cs
+++ b/src/PathPlanning/AutoPathFinder.cs
@@ -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++;
diff --git a/src/PathPlanning/GridMap.cs b/src/PathPlanning/GridMap.cs
index 52ea4a5..5ec512b 100644
--- a/src/PathPlanning/GridMap.cs
+++ b/src/PathPlanning/GridMap.cs
@@ -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>