From e46931311f4022a74283ca6dc3f78799b49ee65f Mon Sep 17 00:00:00 2001 From: tian <11429339@qq.com> Date: Sat, 4 Oct 2025 21:26:49 +0800 Subject: [PATCH] =?UTF-8?q?=E5=B0=86=E6=9C=80=E5=A4=A7=E5=AE=BD=E5=BA=A6?= =?UTF-8?q?=E3=80=81=E9=95=BF=E5=BA=A6=E3=80=81=E9=AB=98=E5=BA=A6=E3=80=81?= =?UTF-8?q?=E5=AE=89=E5=85=A8=E9=97=B4=E9=9A=99=E3=80=81=E7=BD=91=E6=A0=BC?= =?UTF-8?q?=E5=B0=BA=E5=AF=B8=EF=BC=8C=E5=8A=A0=E5=85=A5=E8=B7=AF=E5=BE=84?= =?UTF-8?q?=E4=BF=9D=E5=AD=98=E5=86=85=E5=AE=B9=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- AStarTestRunner.csproj | 57 -- .../AStarDebuggingTest.cs | 867 ------------------ src/Core/PathDataManager.cs | 36 +- src/Core/PathDatabase.cs | 19 +- src/Core/PathPlanningManager.cs | 6 +- src/Core/PathPlanningModels.cs | 26 +- 6 files changed, 71 insertions(+), 940 deletions(-) delete mode 100644 AStarTestRunner.csproj delete mode 100644 NavisworksTransport.UnitTests/AStarDebuggingTest.cs diff --git a/AStarTestRunner.csproj b/AStarTestRunner.csproj deleted file mode 100644 index d96ad8f..0000000 --- a/AStarTestRunner.csproj +++ /dev/null @@ -1,57 +0,0 @@ - - - - - - Debug - AnyCPU - {F2B4A8C1-2DEF-4765-8392-E67AD8372FG2} - Exe - Properties - TestRunner - AStarTestRunner - v4.8 - 512 - true - x64 - - - - true - full - false - bin\Debug\ - DEBUG;TRACE - prompt - 4 - - - - - - - - - packages\RoyT.AStar.2.1.0\lib\netstandard2.0\Roy-T.AStar.dll - True - - - - - bin\Debug\NavisworksTransport.UnitTests.dll - True - - - - - bin\Debug\NavisworksTransportPlugin.dll - True - - - - - - - - - \ No newline at end of file diff --git a/NavisworksTransport.UnitTests/AStarDebuggingTest.cs b/NavisworksTransport.UnitTests/AStarDebuggingTest.cs deleted file mode 100644 index c34495a..0000000 --- a/NavisworksTransport.UnitTests/AStarDebuggingTest.cs +++ /dev/null @@ -1,867 +0,0 @@ -using System; -using System.Collections.Generic; -using System.Linq; -using Microsoft.VisualStudio.TestTools.UnitTesting; -using Roy_T.AStar.Grids; -using Roy_T.AStar.Primitives; -using Roy_T.AStar.Paths; -using NavisworksTransport.PathPlanning; -using NavisworksTransport.Utils; -using NavisworksTransport; - -namespace NavisworksTransport.UnitTests -{ - /// - /// 简化的3D点结构体（用于测试） - /// - public struct Point3D - { - public double X { get; set; } - public double Y { get; set; } - public double Z { get; set; } - - public Point3D(double x, double y, double z) - { - X = x; - Y = y; - Z = z; - } - - public override string ToString() => $"({X:F2}, {Y:F2}, {Z:F2})"; - } - - /// - /// 简化的2D点结构体（用于测试） - /// - public struct Point2D - { - public double X { get; set; } - public double Y { get; set; } - - public Point2D(double x, double y) - { - X = x; - Y = y; - } - - public override string ToString() => $"({X:F2}, {Y:F2})"; - } - - /// - /// 简化的网格点结构体（用于测试） - /// - public struct GridPoint2D - { - public int X { get; set; } - public int Y { get; set; } - - public GridPoint2D(int x, int y) - { - X = x; - Y = y; - } - - public override string ToString() => $"({X}, {Y})"; - } - - /// - /// 简化的边界框结构体（用于测试） - /// - public struct BoundingBox3D - { - public Point3D Min { get; set; } - public Point3D Max { get; set; } - - public BoundingBox3D(Point3D min, Point3D max) - { - Min = min; - Max = max; - } - } - - /// - /// A*算法问题检测测试用例 - /// 专门用于调试A*路径穿越障碍物网格的问题 - /// - [TestClass] - public class AStarDebuggingTest - { - /// - /// 基础障碍物避让测试 - /// 创建一个3x3网格，中间设置障碍物，测试A*是否能正确绕过 - /// - [TestMethod] - public void TestBasicObstacleAvoidance() - { - // 创建3x3网格 - var gridSize = new GridSize(3, 3); - var cellSize = new Size(Distance.FromMeters(1.0f), Distance.FromMeters(1.0f)); - var velocity = Velocity.FromKilometersPerHour(5.0f); - - var grid = Grid.CreateGridWithLateralConnections(gridSize, cellSize, velocity); - - // 断开中间位置(1,1)，模拟障碍物 - var obstaclePos = new GridPosition(1, 1); - grid.DisconnectNode(obstaclePos); - - // 验证障碍物位置确实被断开 - var obstacleNode = grid.GetNode(obstaclePos); - var obstacleEdges = obstacleNode.Outgoing.Count(); - Console.WriteLine($"障碍物位置({obstaclePos.X},{obstaclePos.Y})的出边数: {obstacleEdges}"); - Assert.AreEqual(0, obstacleEdges, "障碍物节点应该没有出边"); - - // 尝试从(0,0)到(2,2)寻路，必须绕过中间的障碍物 - var startPos = new GridPosition(0, 0); - var endPos = new GridPosition(2, 2); - - var pathfinder = new PathFinder(); - var path = pathfinder.FindPath(startPos, endPos, grid); - - Assert.IsNotNull(path, "应该能找到绕过障碍物的路径"); - Assert.IsTrue(path.Edges.Count > 0, "路径应该包含边"); - - // 关键检查：验证路径不经过障碍物位置 - var pathNodes = new List { startPos }; - foreach (var edge in path.Edges) - { - var nodeGridX = (int)Math.Floor(edge.End.Position.X / 1.0); // cellSize = 1米 - var nodeGridY = (int)Math.Floor(edge.End.Position.Y / 1.0); - var nodeGridPos = new GridPosition(nodeGridX, nodeGridY); - pathNodes.Add(nodeGridPos); - } - - Console.WriteLine("路径经过的网格坐标:"); - foreach (var node in pathNodes) - { - Console.WriteLine($" 网格({node.X},{node.Y})"); - } - - // 验证路径不包含障碍物坐标 - bool pathContainsObstacle = pathNodes.Any(pos => pos.X == obstaclePos.X && pos.Y == obstaclePos.Y); - Assert.IsFalse(pathContainsObstacle, $"路径不应该经过障碍物位置({obstaclePos.X},{obstaclePos.Y})"); - } - - /// - /// 坐标映射验证测试 - /// 验证GridPosition和Node.Position之间的对应关系 - /// - [TestMethod] - public void TestCoordinateMapping() - { - var gridSize = new GridSize(5, 5); - var cellSize = new Size(Distance.FromMeters(2.0f), Distance.FromMeters(2.0f)); - var velocity = Velocity.FromKilometersPerHour(5.0f); - - var grid = Grid.CreateGridWithLateralConnections(gridSize, cellSize, velocity); - - // 测试几个关键坐标的映射关系 - var testPositions = new[] - { - new GridPosition(0, 0), - new GridPosition(1, 1), - new GridPosition(2, 3), - new GridPosition(4, 4) - }; - - Console.WriteLine("GridPosition到Node.Position映射验证:"); - foreach (var gridPos in testPositions) - { - var node = grid.GetNode(gridPos); - var nodePos = node.Position; - - Console.WriteLine($"网格({gridPos.X},{gridPos.Y}) -> 节点位置({nodePos.X:F2},{nodePos.Y:F2})"); - - // 验证映射关系：GridPosition * CellSize = Node.Position - var expectedX = gridPos.X * 2.0; // cellSize = 2米 - var expectedY = gridPos.Y * 2.0; - - Assert.AreEqual(expectedX, nodePos.X, 0.01, $"X坐标映射错误：网格{gridPos.X}应对应节点{expectedX}"); - Assert.AreEqual(expectedY, nodePos.Y, 0.01, $"Y坐标映射错误：网格{gridPos.Y}应对应节点{expectedY}"); - } - } - - /// - /// 实际问题场景重现测试 - /// 重现日志中出现的49,25到62,25障碍物路径问题 - /// - [TestMethod] - public void TestActualProblemScenario() - { - // 模拟实际场景：创建包含问题区域的网格 - var gridSize = new GridSize(120, 50); // 足够大的网格 - var cellSize = new Size(Distance.FromMeters(1.0f), Distance.FromMeters(1.0f)); - var velocity = Velocity.FromKilometersPerHour(5.0f); - - var grid = Grid.CreateGridWithLateralConnections(gridSize, cellSize, velocity); - - // 根据日志，将50,25到61,25设置为障碍物 - var obstaclePositions = new List(); - for (int x = 50; x <= 61; x++) - { - obstaclePositions.Add(new GridPosition(x, 25)); - } - - Console.WriteLine("设置障碍物位置:"); - foreach (var obstaclePos in obstaclePositions) - { - grid.DisconnectNode(obstaclePos); - Console.WriteLine($" 断开网格({obstaclePos.X},{obstaclePos.Y})"); - - // 验证断开成功 - var node = grid.GetNode(obstaclePos); - var edgeCount = node.Outgoing.Count(); - Assert.AreEqual(0, edgeCount, $"障碍物网格({obstaclePos.X},{obstaclePos.Y})应该没有出边，实际有{edgeCount}条"); - } - - // 尝试从49,25到62,25寻路（跨越障碍物区域） - var startPos = new GridPosition(49, 25); - var endPos = new GridPosition(62, 25); - - Console.WriteLine($"开始寻路：从({startPos.X},{startPos.Y})到({endPos.X},{endPos.Y})"); - - var pathfinder = new PathFinder(); - var path = pathfinder.FindPath(startPos, endPos, grid); - - if (path != null && path.Edges.Count > 0) - { - Console.WriteLine($"找到路径，包含{path.Edges.Count}条边"); - Console.WriteLine($"路径类型: {path.Type}"); - - // 提取路径经过的所有网格坐标 - var pathGridPositions = new List { startPos }; - - foreach (var edge in path.Edges) - { - var nodeGridX = (int)Math.Floor(edge.End.Position.X / 1.0); - var nodeGridY = (int)Math.Floor(edge.End.Position.Y / 1.0); - var nodeGridPos = new GridPosition(nodeGridX, nodeGridY); - pathGridPositions.Add(nodeGridPos); - } - - Console.WriteLine("路径经过的网格坐标:"); - foreach (var pos in pathGridPositions) - { - Console.WriteLine($" 网格({pos.X},{pos.Y})"); - } - - // 关键验证：检查路径是否经过任何障碍物位置 - var pathThroughObstacles = new List(); - foreach (var pathPos in pathGridPositions) - { - if (obstaclePositions.Any(obs => obs.X == pathPos.X && obs.Y == pathPos.Y)) - { - pathThroughObstacles.Add(pathPos); - } - } - - if (pathThroughObstacles.Any()) - { - Console.WriteLine("❌ 发现问题：路径穿越了以下障碍物位置:"); - foreach (var obs in pathThroughObstacles) - { - Console.WriteLine($" 障碍物网格({obs.X},{obs.Y})"); - } - - Assert.Fail($"路径不应该穿越障碍物！发现{pathThroughObstacles.Count}个穿越点"); - } - else - { - Console.WriteLine("✅ 路径正确避开了所有障碍物"); - } - } - else - { - Console.WriteLine("未找到路径"); - // 在这种情况下，应该尝试找到最近接近路径 - Assert.IsNotNull(path, "至少应该返回ClosestApproach类型的路径"); - } - } - - /// - /// 网格连接状态详细检查测试 - /// 验证断开节点后，相邻节点的连接状态 - /// - [TestMethod] - public void TestGridConnectionState() - { - var gridSize = new GridSize(5, 5); - var cellSize = new Size(Distance.FromMeters(1.0f), Distance.FromMeters(1.0f)); - var velocity = Velocity.FromKilometersPerHour(5.0f); - - var grid = Grid.CreateGridWithLateralConnections(gridSize, cellSize, velocity); - - // 断开中心位置(2,2) - var centerPos = new GridPosition(2, 2); - grid.DisconnectNode(centerPos); - - // 检查中心位置的连接状态 - var centerNode = grid.GetNode(centerPos); - Console.WriteLine($"中心节点({centerPos.X},{centerPos.Y})出边数: {centerNode.Outgoing.Count()}"); - Console.WriteLine($"中心节点({centerPos.X},{centerPos.Y})入边数: {centerNode.Incoming.Count()}"); - - // 检查相邻节点是否还指向已断开的中心节点 - var adjacentPositions = new[] - { - new GridPosition(1, 2), // 左 - new GridPosition(3, 2), // 右 - new GridPosition(2, 1), // 下 - new GridPosition(2, 3) // 上 - }; - - Console.WriteLine("相邻节点连接检查:"); - foreach (var adjPos in adjacentPositions) - { - var adjNode = grid.GetNode(adjPos); - var connectsToCenter = adjNode.Outgoing.Any(edge => - { - var targetGridX = (int)Math.Floor(edge.End.Position.X / 1.0); - var targetGridY = (int)Math.Floor(edge.End.Position.Y / 1.0); - return targetGridX == centerPos.X && targetGridY == centerPos.Y; - }); - - Console.WriteLine($" 节点({adjPos.X},{adjPos.Y}) -> 中心节点: {(connectsToCenter ? "是" : "否")}"); - Assert.IsFalse(connectsToCenter, $"相邻节点({adjPos.X},{adjPos.Y})不应该连接到已断开的中心节点"); - } - } - - /// - /// 使用真实坐标系统的A*算法测试 - /// 集成实际系统的坐标转换方法，以复现真实环境中的问题 - /// - [TestMethod] - public void TestWithRealCoordinateSystem() - { - Console.WriteLine("=== 开始真实坐标系统A*测试 ==="); - - // 创建模拟的GridMap - 使用实际系统的参数 - var mockGridMap = CreateRealGridMap(); - - // 创建对应的Roy_T.AStar网格 - var astarGrid = CreateAStarGridFromGridMap(mockGridMap); - - // 设置障碍物 - 复现实际问题场景中的障碍物位置 - SetupObstaclesInMockGridMap(mockGridMap, astarGrid); - - // 定义测试起点和终点（使用实际世界坐标） - var startWorld = new Point3D(25.0, 12.5, 0.0); // 对应网格(49,25)的世界坐标 - var endWorld = new Point3D(31.0, 12.5, 0.0); // 对应网格(62,25)的世界坐标 - - Console.WriteLine($"测试场景: 从世界坐标({startWorld.X}, {startWorld.Y})到({endWorld.X}, {endWorld.Y})"); - - // 执行真实的坐标转换和A*寻路 - var pathResult = ExecuteRealCoordinatePathfinding(startWorld, endWorld, mockGridMap, astarGrid); - - // 验证结果 - ValidateRealCoordinatePathResult(pathResult, startWorld, endWorld, mockGridMap); - } - - /// - /// 通道路径查找测试 - /// 创建一个有明确通道的场景，验证A*返回的路径坐标是否正确 - /// - [TestMethod] - public void TestChannelPathfinding() - { - Console.WriteLine("=== 开始通道路径查找测试 ==="); - - // 创建测试场景：60x50网格，0.5米单元 - var gridSize = new GridSize(60, 50); - var cellSize = new Size(Distance.FromMeters(0.5f), Distance.FromMeters(0.5f)); - var velocity = Velocity.FromKilometersPerHour(5.0f); - - var grid = Grid.CreateGridWithLateralConnections(gridSize, cellSize, velocity); - - // 设置障碍物：创建一个通道场景 - // 障碍物区域：X=48-54, Y=24和Y=27（上下墙） - // X=48和X=54, Y=24-27（左右墙） - // 通道区域：X=49-53, Y=25-26（可通行） - - var obstaclePositions = new List(); - - // 上下墙 - for (int x = 48; x <= 54; x++) - { - obstaclePositions.Add(new GridPosition(x, 24)); // 下墙 - obstaclePositions.Add(new GridPosition(x, 27)); // 上墙 - } - - // 左右墙 - for (int y = 25; y <= 26; y++) - { - obstaclePositions.Add(new GridPosition(48, y)); // 左墙 - obstaclePositions.Add(new GridPosition(54, y)); // 右墙 - } - - Console.WriteLine("设置障碍物布局："); - Console.WriteLine(" 上墙: X=48-54, Y=27"); - Console.WriteLine(" 下墙: X=48-54, Y=24"); - Console.WriteLine(" 左墙: X=48, Y=25-26"); - Console.WriteLine(" 右墙: X=54, Y=25-26"); - Console.WriteLine(" 通道: X=49-53, Y=25-26"); - - // 设置障碍物 - foreach (var obstaclePos in obstaclePositions) - { - grid.DisconnectNode(obstaclePos); - } - - // 验证障碍物设置 - Console.WriteLine("\n障碍物设置验证："); - int obstacleCount = 0; - foreach (var obstaclePos in obstaclePositions) - { - var node = grid.GetNode(obstaclePos); - var edgeCount = node.Outgoing.Count(); - if (edgeCount == 0) obstacleCount++; - Console.WriteLine($" 障碍物({obstaclePos.X},{obstaclePos.Y})出边数: {edgeCount}"); - } - Console.WriteLine($"成功设置{obstacleCount}个障碍物网格"); - - // 设置测试起点和终点：在通道内但靠近障碍物 - var startPos = new GridPosition(49, 25); // 通道左下角 - var endPos = new GridPosition(53, 26); // 通道右上角 - - Console.WriteLine($"\n开始寻路：从网格({startPos.X},{startPos.Y})到网格({endPos.X},{endPos.Y})"); - - // 执行A*寻路 - var pathfinder = new PathFinder(); - var path = pathfinder.FindPath(startPos, endPos, grid); - - // 验证找到了路径 - Assert.IsNotNull(path, "应该能在通道中找到路径"); - Assert.IsTrue(path.Edges.Count > 0, $"路径应该包含边，实际边数: {path.Edges.Count}"); - - Console.WriteLine($"✅ 找到路径，包含{path.Edges.Count}条边"); - Console.WriteLine($"路径类型: {path.Type}"); - - // 关键验证：检查路径上的每个点 - var pathPoints = new List { startPos }; - - Console.WriteLine("\nA*路径坐标详细分析："); - Console.WriteLine("起点转换: A*位置 -> 米坐标 -> 网格坐标"); - - int pointIndex = 0; - foreach (var edge in path.Edges) - { - pointIndex++; - var endMeterPos = edge.End.Position; - - // 关键：将A*返回的米坐标转换回网格坐标 - var calculatedGridX = (int)Math.Floor(endMeterPos.X / 0.5); // cellSize = 0.5米 - var calculatedGridY = (int)Math.Floor(endMeterPos.Y / 0.5); - var calculatedGridPos = new GridPosition(calculatedGridX, calculatedGridY); - - pathPoints.Add(calculatedGridPos); - - Console.WriteLine($" 点{pointIndex}: A*米坐标({endMeterPos.X:F2}, {endMeterPos.Y:F2}) -> 计算网格({calculatedGridPos.X}, {calculatedGridPos.Y})"); - } - - // 验证所有路径点是否在允许的通道区域内 - Console.WriteLine("\n路径点合法性检查："); - var invalidPoints = new List(); - - foreach (var point in pathPoints) - { - bool isInChannel = (point.X >= 49 && point.X <= 53) && (point.Y >= 25 && point.Y <= 26); - bool isObstacle = obstaclePositions.Any(obs => obs.X == point.X && obs.Y == point.Y); - - string status; - if (isObstacle) - { - status = "❌障碍物"; - invalidPoints.Add(point); - } - else if (isInChannel) - { - status = "✅通道内"; - } - else - { - status = "⚠️通道外"; - invalidPoints.Add(point); - } - - Console.WriteLine($" 网格({point.X},{point.Y}) - {status}"); - } - - // 最终验证 - if (invalidPoints.Any()) - { - Console.WriteLine($"\n🔥 发现问题：{invalidPoints.Count}个路径点在非法位置！"); - foreach (var invalid in invalidPoints) - { - Console.WriteLine($" 非法点: 网格({invalid.X},{invalid.Y})"); - } - - Assert.Fail($"A*路径包含{invalidPoints.Count}个非法位置的点！这证明了A*算法或坐标转换存在问题。"); - } - else - { - Console.WriteLine("\n✅ 所有路径点都在合法的通道区域内"); - Console.WriteLine("✅ A*算法返回的坐标转换正确"); - } - } - - /// - /// 创建模拟的GridMap，使用实际系统的参数 - /// - private MockGridMap CreateRealGridMap() - { - // 模拟实际系统的GridMap参数 - var origin = new Point2D(0.0, 0.0); // 网格原点 - var cellSize = 0.5; // 网格单元大小：0.5米 - var width = 150; // 网格宽度：150个单元 - var height = 80; // 网格高度：80个单元 - var bounds = new BoundingBox3D( - new Point3D(0, 0, 0), - new Point3D(width * cellSize, height * cellSize, 10.0) - ); - - Console.WriteLine($"创建GridMap - 原点:({origin.X}, {origin.Y}), 单元大小:{cellSize}m, 尺寸:{width}x{height}"); - - var gridMap = new MockGridMap(origin, cellSize, width, height, bounds); - - // 初始化为可通行网格 - for (int x = 0; x < width; x++) - { - for (int y = 0; y < height; y++) - { - gridMap.SetCell(new GridPoint2D(x, y), true); - } - } - - return gridMap; - } - - /// - /// 从GridMap创建对应的Roy_T.AStar网格 - /// - private Grid CreateAStarGridFromGridMap(MockGridMap gridMap) - { - // 使用真实的单位转换 - double cellSizeInMeters = MockUnitsConverter.ConvertToMeters(gridMap.CellSize); - - var gridSize = new GridSize(gridMap.Width, gridMap.Height); - var cellSize = new Size( - Distance.FromMeters((float)cellSizeInMeters), - Distance.FromMeters((float)cellSizeInMeters) - ); - var velocity = Velocity.FromKilometersPerHour(5.0f); - - Console.WriteLine($"创建A*网格 - 网格大小:{gridSize.Columns}x{gridSize.Rows}, 单元大小:{cellSizeInMeters}m"); - - return Grid.CreateGridWithLateralConnections(gridSize, cellSize, velocity); - } - - /// - /// 在网格中设置障碍物 - /// - private void SetupObstaclesInMockGridMap(MockGridMap gridMap, Grid astarGrid) - { - // 模拟实际问题场景：在路径中间设置障碍物 - var obstaclePositions = new List(); - - // 设置从网格(50,25)到(61,25)为障碍物 - for (int x = 50; x <= 61; x++) - { - obstaclePositions.Add(new GridPoint2D(x, 25)); - } - - Console.WriteLine("设置障碍物位置:"); - foreach (var obstaclePos in obstaclePositions) - { - if (gridMap.IsValidGridPosition(obstaclePos)) - { - // 在MockGridMap中设置为不可通行 - gridMap.SetCell(obstaclePos, false); - - // 在A*网格中断开连接 - var astarPos = new GridPosition(obstaclePos.X, obstaclePos.Y); - astarGrid.DisconnectNode(astarPos); - - var worldPos = gridMap.GridToWorld2D(obstaclePos); - Console.WriteLine($" 障碍物网格({obstaclePos.X},{obstaclePos.Y}) -> 世界坐标({worldPos.X:F2},{worldPos.Y:F2})"); - - // 验证断开成功 - var node = astarGrid.GetNode(astarPos); - var edgeCount = node.Outgoing.Count(); - Console.WriteLine($" A*节点出边数: {edgeCount}"); - } - } - } - - /// - /// 执行真实的坐标转换和路径查找 - /// - private PathFindingResult ExecuteRealCoordinatePathfinding(Point3D startWorld, Point3D endWorld, MockGridMap gridMap, Grid astarGrid) - { - Console.WriteLine("=== 执行真实坐标转换和A*寻路 ==="); - - // 步骤1: 世界坐标转换为网格坐标 - var startGrid = gridMap.WorldToGrid(startWorld); - var endGrid = gridMap.WorldToGrid(endWorld); - - Console.WriteLine($"世界坐标转换:"); - Console.WriteLine($" 起点: 世界({startWorld.X}, {startWorld.Y}) -> 网格({startGrid.X}, {startGrid.Y})"); - Console.WriteLine($" 终点: 世界({endWorld.X}, {endWorld.Y}) -> 网格({endGrid.X}, {endGrid.Y})"); - - // 步骤2: 网格坐标转换为A*位置 - var startPos = new GridPosition(startGrid.X, startGrid.Y); - var endPos = new GridPosition(endGrid.X, endGrid.Y); - - Console.WriteLine($"A*位置设置:"); - Console.WriteLine($" 起点A*位置: ({startPos.X}, {startPos.Y})"); - Console.WriteLine($" 终点A*位置: ({endPos.X}, {endPos.Y})"); - - // 步骤3: 执行A*算法 - var pathfinder = new PathFinder(); - var astarPath = pathfinder.FindPath(startPos, endPos, astarGrid); - - Console.WriteLine($"A*寻路结果: {(astarPath != null ? $"找到路径，包含{astarPath.Edges.Count}条边" : "未找到路径")}"); - - if (astarPath != null && astarPath.Edges.Count > 0) - { - // 步骤4: 转换A*路径回世界坐标 - var worldPath = ConvertAStarPathToWorldCoordinates(astarPath, gridMap); - - return new PathFindingResult - { - PathPoints = worldPath, - OriginalEndPoint = endWorld, - IsSuccessful = true - }; - } - - return new PathFindingResult - { - PathPoints = new List(), - OriginalEndPoint = endWorld, - IsSuccessful = false - }; - } - - /// - /// 转换A*路径为世界坐标（模拟AutoPathFinder的逻辑） - /// - private List ConvertAStarPathToWorldCoordinates(Path astarPath, MockGridMap gridMap) - { - var worldPath = new List(); - - Console.WriteLine("=== A*路径转换为世界坐标 ==="); - - // 获取单位转换因子（模拟真实系统） - double metersToWorldUnit = 1.0 / MockUnitsConverter.ConvertToMeters(1.0); - Console.WriteLine($"单位转换因子: metersToWorldUnit = {metersToWorldUnit}"); - - if (astarPath.Edges.Count > 0) - { - // 添加起点 - var startNode = astarPath.Edges[0].Start; - var startWorldPos = new Point3D( - gridMap.Origin.X + startNode.Position.X * metersToWorldUnit, - gridMap.Origin.Y + startNode.Position.Y * metersToWorldUnit, - 0 - ); - - Console.WriteLine($"起点转换: A*米坐标({startNode.Position.X:F2}, {startNode.Position.Y:F2}) -> 世界坐标({startWorldPos.X:F2}, {startWorldPos.Y:F2})"); - worldPath.Add(startWorldPos); - - // 添加路径上的每个点 - foreach (var edge in astarPath.Edges) - { - var endWorldPos = new Point3D( - gridMap.Origin.X + edge.End.Position.X * metersToWorldUnit, - gridMap.Origin.Y + edge.End.Position.Y * metersToWorldUnit, - 0 - ); - - // 转换为网格坐标以便分析 - var endGridPos = gridMap.WorldToGrid(endWorldPos); - - Console.WriteLine($"路径点: A*米坐标({edge.End.Position.X:F2}, {edge.End.Position.Y:F2}) -> 世界坐标({endWorldPos.X:F2}, {endWorldPos.Y:F2}) -> 网格({endGridPos.X}, {endGridPos.Y})"); - - worldPath.Add(endWorldPos); - } - } - - return worldPath; - } - - /// - /// 验证真实坐标系统下的路径结果 - /// - private void ValidateRealCoordinatePathResult(PathFindingResult result, Point3D startWorld, Point3D endWorld, MockGridMap gridMap) - { - Console.WriteLine("=== 验证路径结果 ==="); - - if (!result.IsSuccessful || result.PathPoints.Count == 0) - { - Console.WriteLine("❌ 未找到路径 - 这可能是正常的，因为障碍物阻挡了直接路径"); - return; - } - - Console.WriteLine($"✅ 找到路径，包含{result.PathPoints.Count}个点"); - - // 转换所有路径点为网格坐标以便检查 - var pathGridPositions = new List(); - foreach (var worldPos in result.PathPoints) - { - var gridPos = gridMap.WorldToGrid(worldPos); - pathGridPositions.Add(gridPos); - } - - Console.WriteLine("路径经过的网格坐标:"); - foreach (var gridPos in pathGridPositions) - { - var cell = gridMap.GetCell(gridPos); - var isWalkable = cell.IsWalkable; - var status = isWalkable ? "✅可通行" : "❌障碍物"; - Console.WriteLine($" 网格({gridPos.X},{gridPos.Y}) - {status}"); - } - - // 检查是否有路径穿越障碍物 - var obstacleTraversals = new List(); - foreach (var gridPos in pathGridPositions) - { - var cell = gridMap.GetCell(gridPos); - if (!cell.IsWalkable) - { - obstacleTraversals.Add(gridPos); - } - } - - if (obstacleTraversals.Any()) - { - Console.WriteLine($"🔥 发现问题：路径穿越了{obstacleTraversals.Count}个障碍物网格!"); - foreach (var obs in obstacleTraversals) - { - Console.WriteLine($" 障碍物穿越: 网格({obs.X},{obs.Y})"); - } - - Assert.Fail($"真实坐标系统下发现A*路径穿越障碍物问题！穿越了{obstacleTraversals.Count}个障碍物网格。"); - } - else - { - Console.WriteLine("✅ 路径正确避开了所有障碍物"); - } - } - - /// - /// 模拟路径查找结果类 - /// - private class PathFindingResult - { - public List PathPoints { get; set; } = new List(); - public Point3D OriginalEndPoint { get; set; } - public bool IsSuccessful { get; set; } - } - - /// - /// 模拟UnitsConverter类（用于测试） - /// - private static class MockUnitsConverter - { - /// - /// 模拟：将距离从文档单位转换为米 - /// 假设文档单位就是米，转换因子为1.0 - /// - public static double ConvertToMeters(double distance) - { - return distance * 1.0; // 假设文档单位就是米 - } - } - - /// - /// 模拟GridMap类（用于测试） - /// - private class MockGridMap - { - public Point2D Origin { get; private set; } - public double CellSize { get; private set; } - public int Width { get; private set; } - public int Height { get; private set; } - public BoundingBox3D Bounds { get; private set; } - - private MockGridCell[,] cells; - - public MockGridMap(Point2D origin, double cellSize, int width, int height, BoundingBox3D bounds) - { - Origin = origin; - CellSize = cellSize; - Width = width; - Height = height; - Bounds = bounds; - cells = new MockGridCell[width, height]; - - // 初始化所有单元格 - for (int x = 0; x < width; x++) - { - for (int y = 0; y < height; y++) - { - cells[x, y] = new MockGridCell { IsWalkable = false }; - } - } - } - - /// - /// 世界坐标转换为网格坐标 - /// - public GridPoint2D WorldToGrid(Point3D worldPosition) - { - 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)); - } - - /// - /// 网格坐标转换为世界2D坐标（网格左下角） - /// - public Point3D GridToWorld2D(GridPoint2D gridPosition) - { - double worldX = Origin.X + gridPosition.X * CellSize; - double worldY = Origin.Y + gridPosition.Y * CellSize; - return new Point3D(worldX, worldY, 0); - } - - /// - /// 检查网格坐标是否有效 - /// - public bool IsValidGridPosition(GridPoint2D gridPosition) - { - return gridPosition.X >= 0 && gridPosition.X < Width && - gridPosition.Y >= 0 && gridPosition.Y < Height; - } - - /// - /// 设置网格单元 - /// - public void SetCell(GridPoint2D gridPosition, bool isWalkable) - { - if (IsValidGridPosition(gridPosition)) - { - cells[gridPosition.X, gridPosition.Y].IsWalkable = isWalkable; - } - } - - /// - /// 获取网格单元 - /// - public MockGridCell GetCell(GridPoint2D gridPosition) - { - if (IsValidGridPosition(gridPosition)) - { - return cells[gridPosition.X, gridPosition.Y]; - } - return new MockGridCell { IsWalkable = false }; - } - } - - /// - /// 模拟GridCell类（用于测试） - /// - private class MockGridCell - { - public bool IsWalkable { get; set; } - } - } -} \ No newline at end of file diff --git a/src/Core/PathDataManager.cs b/src/Core/PathDataManager.cs index 0c9b30c..e637879 100644 --- a/src/Core/PathDataManager.cs +++ b/src/Core/PathDataManager.cs @@ -192,6 +192,14 @@ namespace NavisworksTransport name = route.Name, description = route.Description ?? "", totalLength = Math.Round(route.TotalLength, exportSettings?.Precision ?? 6), + vehicleLimits = new + { + maxLength = Math.Round(route.MaxVehicleLength, exportSettings?.Precision ?? 6), + maxWidth = Math.Round(route.MaxVehicleWidth, exportSettings?.Precision ?? 6), + maxHeight = Math.Round(route.MaxVehicleHeight, exportSettings?.Precision ?? 6), + safetyMargin = Math.Round(route.SafetyMargin, exportSettings?.Precision ?? 6) + }, + gridSize = Math.Round(route.GridSize, exportSettings?.Precision ?? 6), created = route.CreatedTime.ToString("yyyy-MM-ddTHH:mm:ss"), points = route.GetSortedPoints().Select(point => new { @@ -253,7 +261,7 @@ namespace NavisworksTransport using (var writer = new StreamWriter(filePath, false, Encoding.UTF8)) { // 写入CSV表头 - writer.WriteLine("路径名称,路径描述,点编号,点名称,X坐标,Y坐标,Z坐标,点类型,点索引,创建时间,路径总长度,路径创建时间"); + writer.WriteLine("路径名称,路径描述,点编号,点名称,X坐标,Y坐标,Z坐标,点类型,点索引,创建时间,路径总长度,限长,限宽,限高,安全间隙,网格尺寸,路径创建时间"); // 写入数据行 foreach (var route in routes.Where(r => r.IsValid())) @@ -273,6 +281,11 @@ namespace NavisworksTransport point.Index.ToString(), point.CreatedTime.ToString("yyyy-MM-dd HH:mm:ss"), route.TotalLength.ToString("F3", CultureInfo.InvariantCulture), + route.MaxVehicleLength.ToString("F3", CultureInfo.InvariantCulture), + route.MaxVehicleWidth.ToString("F3", CultureInfo.InvariantCulture), + route.MaxVehicleHeight.ToString("F3", CultureInfo.InvariantCulture), + route.SafetyMargin.ToString("F3", CultureInfo.InvariantCulture), + route.GridSize.ToString("F3", CultureInfo.InvariantCulture), route.CreatedTime.ToString("yyyy-MM-dd HH:mm:ss") ); writer.WriteLine(row); @@ -658,6 +671,11 @@ namespace NavisworksTransport routeElement.SetAttribute("name", route.Name); routeElement.SetAttribute("description", route.Description ?? ""); routeElement.SetAttribute("totalLength", route.TotalLength.ToString("F3")); + routeElement.SetAttribute("maxVehicleLength", route.MaxVehicleLength.ToString("F3")); + routeElement.SetAttribute("maxVehicleWidth", route.MaxVehicleWidth.ToString("F3")); + routeElement.SetAttribute("maxVehicleHeight", route.MaxVehicleHeight.ToString("F3")); + routeElement.SetAttribute("safetyMargin", route.SafetyMargin.ToString("F3")); + routeElement.SetAttribute("gridSize", route.GridSize.ToString("F3")); routeElement.SetAttribute("created", route.CreatedTime.ToString("yyyy-MM-ddTHH:mm:ss")); // 添加路径点 @@ -694,7 +712,12 @@ namespace NavisworksTransport { Id = routeNode.Attributes?["id"]?.Value ?? Guid.NewGuid().ToString(), Name = routeNode.Attributes?["name"]?.Value ?? "导入路径", - Description = routeNode.Attributes?["description"]?.Value ?? "" + Description = routeNode.Attributes?["description"]?.Value ?? "", + MaxVehicleLength = double.TryParse(routeNode.Attributes?["maxVehicleLength"]?.Value, out double maxLength) ? maxLength : 1.0, + MaxVehicleWidth = double.TryParse(routeNode.Attributes?["maxVehicleWidth"]?.Value, out double maxWidth) ? maxWidth : 1.0, + MaxVehicleHeight = double.TryParse(routeNode.Attributes?["maxVehicleHeight"]?.Value, out double maxHeight) ? maxHeight : 2.0, + SafetyMargin = double.TryParse(routeNode.Attributes?["safetyMargin"]?.Value, out double safetyMargin) ? safetyMargin : 0.5, + GridSize = double.TryParse(routeNode.Attributes?["gridSize"]?.Value, out double gridSize) ? gridSize : 0.5 }; // 解析创建时间 @@ -788,6 +811,15 @@ namespace NavisworksTransport pathElement.SetAttribute("type", "Transportation"); pathElement.SetAttribute("length", route.TotalLength.ToString("F3")); + // 添加车辆限制元素 + var vehicleConstraints = xmlDoc.CreateElement("VehicleConstraints", _delmiaNamespace); + vehicleConstraints.SetAttribute("maxLength", route.MaxVehicleLength.ToString("F3")); + vehicleConstraints.SetAttribute("maxWidth", route.MaxVehicleWidth.ToString("F3")); + vehicleConstraints.SetAttribute("maxHeight", route.MaxVehicleHeight.ToString("F3")); + vehicleConstraints.SetAttribute("safetyMargin", route.SafetyMargin.ToString("F3")); + vehicleConstraints.SetAttribute("gridSize", route.GridSize.ToString("F3")); + pathElement.AppendChild(vehicleConstraints); + // 添加路径段 var segmentsElement = xmlDoc.CreateElement("Segments", _delmiaNamespace); pathElement.AppendChild(segmentsElement); diff --git a/src/Core/PathDatabase.cs b/src/Core/PathDatabase.cs index 366eda7..ea8dee5 100644 --- a/src/Core/PathDatabase.cs +++ b/src/Core/PathDatabase.cs @@ -68,6 +68,11 @@ namespace NavisworksTransport Name TEXT NOT NULL, TotalLength REAL, EstimatedTime REAL, + MaxVehicleLength REAL, + MaxVehicleWidth REAL, + MaxVehicleHeight REAL, + SafetyMargin REAL, + GridSize REAL, CreatedTime DATETIME, LastModified DATETIME ) @@ -136,8 +141,8 @@ namespace NavisworksTransport // 保存路径基本信息 var sql = @" INSERT OR REPLACE INTO PathRoutes - (Id, Name, TotalLength, EstimatedTime, CreatedTime, LastModified) - VALUES (@id, @name, @length, @time, @created, @modified) + (Id, Name, TotalLength, EstimatedTime, MaxVehicleLength, MaxVehicleWidth, MaxVehicleHeight, SafetyMargin, GridSize, CreatedTime, LastModified) + VALUES (@id, @name, @length, @time, @maxLength, @maxWidth, @maxHeight, @safetyMargin, @gridSize, @created, @modified) "; using (var cmd = new SQLiteCommand(sql, _connection)) @@ -146,6 +151,11 @@ namespace NavisworksTransport cmd.Parameters.AddWithValue("@name", route.Name); cmd.Parameters.AddWithValue("@length", route.TotalLength); cmd.Parameters.AddWithValue("@time", route.EstimatedTime); + cmd.Parameters.AddWithValue("@maxLength", route.MaxVehicleLength); + cmd.Parameters.AddWithValue("@maxWidth", route.MaxVehicleWidth); + cmd.Parameters.AddWithValue("@maxHeight", route.MaxVehicleHeight); + cmd.Parameters.AddWithValue("@safetyMargin", route.SafetyMargin); + cmd.Parameters.AddWithValue("@gridSize", route.GridSize); cmd.Parameters.AddWithValue("@created", route.CreatedTime); cmd.Parameters.AddWithValue("@modified", DateTime.Now); cmd.ExecuteNonQuery(); @@ -393,6 +403,11 @@ namespace NavisworksTransport Name = reader["Name"].ToString(), TotalLength = Convert.ToDouble(reader["TotalLength"]), EstimatedTime = Convert.ToDouble(reader["EstimatedTime"]), + MaxVehicleLength = reader.IsDBNull(reader.GetOrdinal("MaxVehicleLength")) ? 1.0 : Convert.ToDouble(reader["MaxVehicleLength"]), + MaxVehicleWidth = reader.IsDBNull(reader.GetOrdinal("MaxVehicleWidth")) ? 1.0 : Convert.ToDouble(reader["MaxVehicleWidth"]), + MaxVehicleHeight = reader.IsDBNull(reader.GetOrdinal("MaxVehicleHeight")) ? 2.0 : Convert.ToDouble(reader["MaxVehicleHeight"]), + SafetyMargin = reader.IsDBNull(reader.GetOrdinal("SafetyMargin")) ? 0.5 : Convert.ToDouble(reader["SafetyMargin"]), + GridSize = reader.IsDBNull(reader.GetOrdinal("GridSize")) ? 0.5 : Convert.ToDouble(reader["GridSize"]), CreatedTime = Convert.ToDateTime(reader["CreatedTime"]), LastModified = Convert.ToDateTime(reader["LastModified"]) }; diff --git a/src/Core/PathPlanningManager.cs b/src/Core/PathPlanningManager.cs index b90f418..197744d 100644 --- a/src/Core/PathPlanningManager.cs +++ b/src/Core/PathPlanningManager.cs @@ -1029,9 +1029,11 @@ namespace NavisworksTransport // 保存GridMap和参数到PathRoute以便后续恢复网格可视化 autoRoute.AssociatedGridMap = gridMap; autoRoute.GridSize = gridSize; - autoRoute.VehicleSize = vehicleRadius; + // 将vehicleRadius拆分为长宽（暂时使用相同值，后续可以传入更详细的参数） + autoRoute.MaxVehicleLength = vehicleRadius * 2; // 车辆半径转换为长度 + autoRoute.MaxVehicleWidth = vehicleRadius * 2; // 车辆半径转换为宽度 + autoRoute.MaxVehicleHeight = vehicleHeight; autoRoute.SafetyMargin = safetyMargin; - autoRoute.VehicleHeight = vehicleHeight; LogManager.Info($"已保存GridMap到路径: {routeName}, 网格大小: {gridSize}米"); // 7. 添加到路径集合 diff --git a/src/Core/PathPlanningModels.cs b/src/Core/PathPlanningModels.cs index 0c2ca47..c9093ef 100644 --- a/src/Core/PathPlanningModels.cs +++ b/src/Core/PathPlanningModels.cs @@ -394,20 +394,25 @@ namespace NavisworksTransport public double GridSize { get; set; } = 0.5; /// - /// 车辆尺寸（米） + /// 最大车辆长度（米） - 路径适用的车辆长度限制 /// - public double VehicleSize { get; set; } = 1.0; + public double MaxVehicleLength { get; set; } = 1.0; + + /// + /// 最大车辆宽度（米） - 路径适用的车辆宽度限制 + /// + public double MaxVehicleWidth { get; set; } = 1.0; + + /// + /// 最大车辆高度（米） - 路径适用的车辆高度限制 + /// + public double MaxVehicleHeight { get; set; } = 2.0; /// /// 安全间隙（米） /// public double SafetyMargin { get; set; } = 0.5; - /// - /// 车辆高度（米） - /// - public double VehicleHeight { get; set; } = 2.0; - // 数据库分析相关属性 /// /// 碰撞数量（从数据库加载） @@ -617,9 +622,10 @@ namespace NavisworksTransport CompletionPercentage = CompletionPercentage, AssociatedGridMap = AssociatedGridMap, // 共享引用，不深拷贝 GridSize = GridSize, - VehicleSize = VehicleSize, - SafetyMargin = SafetyMargin, - VehicleHeight = VehicleHeight + MaxVehicleLength = MaxVehicleLength, + MaxVehicleWidth = MaxVehicleWidth, + MaxVehicleHeight = MaxVehicleHeight, + SafetyMargin = SafetyMargin }; // 克隆所有路径点