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-10-04 21:26:49 +08:00
parent 6091b794de
commit e46931311f
6 changed files with 71 additions and 940 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
AStarTestRunner.csproj
View File

@ -1,57 +0,0 @@
<?xml version="1.0" encoding="utf-8"?>
<Project ToolsVersion="15.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
<Import Project="$(MSBuildExtensionsPath)\$(MSBuildToolsVersion)\Microsoft.Common.props" Condition="Exists('$(MSBuildExtensionsPath)\$(MSBuildToolsVersion)\Microsoft.Common.props')" />
<PropertyGroup>
<Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>
<Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform>
<ProjectGuid>{F2B4A8C1-2DEF-4765-8392-E67AD8372FG2}</ProjectGuid>
<OutputType>Exe</OutputType>
<AppDesignerFolder>Properties</AppDesignerFolder>
<RootNamespace>TestRunner</RootNamespace>
<AssemblyName>AStarTestRunner</AssemblyName>
<TargetFrameworkVersion>v4.8</TargetFrameworkVersion>
<FileAlignment>512</FileAlignment>
<Deterministic>true</Deterministic>
<PlatformTarget>x64</PlatformTarget>
</PropertyGroup>
<PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' ">
<DebugSymbols>true</DebugSymbols>
<DebugType>full</DebugType>
<Optimize>false</Optimize>
<OutputPath>bin\Debug\</OutputPath>
<DefineConstants>DEBUG;TRACE</DefineConstants>
<ErrorReport>prompt</ErrorReport>
<WarningLevel>4</WarningLevel>
</PropertyGroup>
<ItemGroup>
<Reference Include="System" />
<Reference Include="System.Core" />
<!-- Roy_T.AStar for A* algorithm testing -->
<Reference Include="Roy-T.AStar">
<HintPath>packages\RoyT.AStar.2.1.0\lib\netstandard2.0\Roy-T.AStar.dll</HintPath>
<Private>True</Private>
</Reference>
<!-- 引用单元测试DLL -->
<Reference Include="NavisworksTransport.UnitTests">
<HintPath>bin\Debug\NavisworksTransport.UnitTests.dll</HintPath>
<Private>True</Private>
</Reference>
<!-- 引用主项目DLL -->
<Reference Include="NavisworksTransportPlugin">
<HintPath>bin\Debug\NavisworksTransportPlugin.dll</HintPath>
<Private>True</Private>
</Reference>
</ItemGroup>
<ItemGroup>
<Compile Include="TestRunner.cs" />
</ItemGroup>
<Import Project="$(MSBuildToolsPath)\Microsoft.CSharp.targets" />
</Project>

867
NavisworksTransport.UnitTests/AStarDebuggingTest.cs
View File

@ -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
{
/// <summary>
/// 简化的3D点结构体（用于测试）
/// </summary>
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})";
}
/// <summary>
/// 简化的2D点结构体（用于测试）
/// </summary>
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})";
}
/// <summary>
/// 简化的网格点结构体（用于测试）
/// </summary>
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})";
}
/// <summary>
/// 简化的边界框结构体（用于测试）
/// </summary>
public struct BoundingBox3D
{
public Point3D Min { get; set; }
public Point3D Max { get; set; }
public BoundingBox3D(Point3D min, Point3D max)
{
Min = min;
Max = max;
}
}
/// <summary>
/// A*算法问题检测测试用例
/// 专门用于调试A*路径穿越障碍物网格的问题
/// </summary>
[TestClass]
public class AStarDebuggingTest
{
/// <summary>
/// 基础障碍物避让测试
/// 创建一个3x3网格，中间设置障碍物，测试A*是否能正确绕过
/// </summary>
[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<GridPosition> { 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})");
}
/// <summary>
/// 坐标映射验证测试
/// 验证GridPosition和Node.Position之间的对应关系
/// </summary>
[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}");
}
}
/// <summary>
/// 实际问题场景重现测试
/// 重现日志中出现的49,25到62,25障碍物路径问题
/// </summary>
[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<GridPosition>();
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<GridPosition> { 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<GridPosition>();
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类型的路径");
}
}
/// <summary>
/// 网格连接状态详细检查测试
/// 验证断开节点后，相邻节点的连接状态
/// </summary>
[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})不应该连接到已断开的中心节点");
}
}
/// <summary>
/// 使用真实坐标系统的A*算法测试
/// 集成实际系统的坐标转换方法，以复现真实环境中的问题
/// </summary>
[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);
}
/// <summary>
/// 通道路径查找测试
/// 创建一个有明确通道的场景，验证A*返回的路径坐标是否正确
/// </summary>
[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<GridPosition>();
// 上下墙
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<GridPosition> { 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<GridPosition>();
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*算法返回的坐标转换正确");
}
}
/// <summary>
/// 创建模拟的GridMap，使用实际系统的参数
/// </summary>
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;
}
/// <summary>
/// 从GridMap创建对应的Roy_T.AStar网格
/// </summary>
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);
}
/// <summary>
/// 在网格中设置障碍物
/// </summary>
private void SetupObstaclesInMockGridMap(MockGridMap gridMap, Grid astarGrid)
{
// 模拟实际问题场景：在路径中间设置障碍物
var obstaclePositions = new List<GridPoint2D>();
// 设置从网格(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}");
}
}
}
/// <summary>
/// 执行真实的坐标转换和路径查找
/// </summary>
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<Point3D>(),
OriginalEndPoint = endWorld,
IsSuccessful = false
};
}
/// <summary>
/// 转换A*路径为世界坐标（模拟AutoPathFinder的逻辑）
/// </summary>
private List<Point3D> ConvertAStarPathToWorldCoordinates(Path astarPath, MockGridMap gridMap)
{
var worldPath = new List<Point3D>();
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;
}
/// <summary>
/// 验证真实坐标系统下的路径结果
/// </summary>
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<GridPoint2D>();
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<GridPoint2D>();
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("✅ 路径正确避开了所有障碍物");
}
}
/// <summary>
/// 模拟路径查找结果类
/// </summary>
private class PathFindingResult
{
public List<Point3D> PathPoints { get; set; } = new List<Point3D>();
public Point3D OriginalEndPoint { get; set; }
public bool IsSuccessful { get; set; }
}
/// <summary>
/// 模拟UnitsConverter类（用于测试）
/// </summary>
private static class MockUnitsConverter
{
/// <summary>
/// 模拟：将距离从文档单位转换为米
/// 假设文档单位就是米，转换因子为1.0
/// </summary>
public static double ConvertToMeters(double distance)
{
return distance * 1.0; // 假设文档单位就是米
}
}
/// <summary>
/// 模拟GridMap类（用于测试）
/// </summary>
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 };
}
}
}
/// <summary>
/// 世界坐标转换为网格坐标
/// </summary>
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));
}
/// <summary>
/// 网格坐标转换为世界2D坐标（网格左下角）
/// </summary>
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);
}
/// <summary>
/// 检查网格坐标是否有效
/// </summary>
public bool IsValidGridPosition(GridPoint2D gridPosition)
{
return gridPosition.X >= 0 && gridPosition.X < Width &&
gridPosition.Y >= 0 && gridPosition.Y < Height;
}
/// <summary>
/// 设置网格单元
/// </summary>
public void SetCell(GridPoint2D gridPosition, bool isWalkable)
{
if (IsValidGridPosition(gridPosition))
{
cells[gridPosition.X, gridPosition.Y].IsWalkable = isWalkable;
}
}
/// <summary>
/// 获取网格单元
/// </summary>
public MockGridCell GetCell(GridPoint2D gridPosition)
{
if (IsValidGridPosition(gridPosition))
{
return cells[gridPosition.X, gridPosition.Y];
}
return new MockGridCell { IsWalkable = false };
}
}
/// <summary>
/// 模拟GridCell类（用于测试）
/// </summary>
private class MockGridCell
{
public bool IsWalkable { get; set; }
}
}
}

36
src/Core/PathDataManager.cs
View File

@ -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);

19
src/Core/PathDatabase.cs
View File

@ -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"])
};

6
src/Core/PathPlanningManager.cs
View File

@ -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. 添加到路径集合

26
src/Core/PathPlanningModels.cs
View File

@ -394,20 +394,25 @@ namespace NavisworksTransport
public double GridSize { get; set; } = 0.5;
/// <summary>
/// 车辆尺寸（米）
/// 最大车辆长度（米） - 路径适用的车辆长度限制
/// </summary>
public double VehicleSize { get; set; } = 1.0;
public double MaxVehicleLength { get; set; } = 1.0;
/// <summary>
/// 最大车辆宽度（米） - 路径适用的车辆宽度限制
/// </summary>
public double MaxVehicleWidth { get; set; } = 1.0;
/// <summary>
/// 最大车辆高度（米） - 路径适用的车辆高度限制
/// </summary>
public double MaxVehicleHeight { get; set; } = 2.0;
/// <summary>
/// 安全间隙（米）
/// </summary>
public double SafetyMargin { get; set; } = 0.5;
/// <summary>
/// 车辆高度（米）
/// </summary>
public double VehicleHeight { get; set; } = 2.0;
// 数据库分析相关属性
/// <summary>
/// 碰撞数量（从数据库加载）
@ -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
};
// 克隆所有路径点