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, name = route.Name,
description = route.Description ?? "", description = route.Description ?? "",
totalLength = Math.Round(route.TotalLength, exportSettings?.Precision ?? 6), 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"), created = route.CreatedTime.ToString("yyyy-MM-ddTHH:mm:ss"),
points = route.GetSortedPoints().Select(point => new points = route.GetSortedPoints().Select(point => new
{ {
@ -253,7 +261,7 @@ namespace NavisworksTransport
using (var writer = new StreamWriter(filePath, false, Encoding.UTF8)) using (var writer = new StreamWriter(filePath, false, Encoding.UTF8))
{ {
// 写入CSV表头 // 写入CSV表头
writer.WriteLine("路径名称,路径描述,点编号,点名称,X坐标,Y坐标,Z坐标,点类型,点索引,创建时间,路径总长度,路径创建时间"); writer.WriteLine("路径名称,路径描述,点编号,点名称,X坐标,Y坐标,Z坐标,点类型,点索引,创建时间,路径总长度,限长,限宽,限高,安全间隙,网格尺寸,路径创建时间");
// 写入数据行 // 写入数据行
foreach (var route in routes.Where(r => r.IsValid())) foreach (var route in routes.Where(r => r.IsValid()))
@ -273,6 +281,11 @@ namespace NavisworksTransport
point.Index.ToString(), point.Index.ToString(),
point.CreatedTime.ToString("yyyy-MM-dd HH:mm:ss"), point.CreatedTime.ToString("yyyy-MM-dd HH:mm:ss"),
route.TotalLength.ToString("F3", CultureInfo.InvariantCulture), 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") route.CreatedTime.ToString("yyyy-MM-dd HH:mm:ss")
); );
writer.WriteLine(row); writer.WriteLine(row);
@ -658,6 +671,11 @@ namespace NavisworksTransport
routeElement.SetAttribute("name", route.Name); routeElement.SetAttribute("name", route.Name);
routeElement.SetAttribute("description", route.Description ?? ""); routeElement.SetAttribute("description", route.Description ?? "");
routeElement.SetAttribute("totalLength", route.TotalLength.ToString("F3")); 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")); 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(), Id = routeNode.Attributes?["id"]?.Value ?? Guid.NewGuid().ToString(),
Name = routeNode.Attributes?["name"]?.Value ?? "导入路径", 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("type", "Transportation");
pathElement.SetAttribute("length", route.TotalLength.ToString("F3")); 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); var segmentsElement = xmlDoc.CreateElement("Segments", _delmiaNamespace);
pathElement.AppendChild(segmentsElement); pathElement.AppendChild(segmentsElement);

19
src/Core/PathDatabase.cs
View File

@ -68,6 +68,11 @@ namespace NavisworksTransport
Name TEXT NOT NULL, Name TEXT NOT NULL,
TotalLength REAL, TotalLength REAL,
EstimatedTime REAL, EstimatedTime REAL,
MaxVehicleLength REAL,
MaxVehicleWidth REAL,
MaxVehicleHeight REAL,
SafetyMargin REAL,
GridSize REAL,
CreatedTime DATETIME, CreatedTime DATETIME,
LastModified DATETIME LastModified DATETIME
) )
@ -136,8 +141,8 @@ namespace NavisworksTransport
// 保存路径基本信息 // 保存路径基本信息
var sql = @" var sql = @"
INSERT OR REPLACE INTO PathRoutes INSERT OR REPLACE INTO PathRoutes
(Id, Name, TotalLength, EstimatedTime, CreatedTime, LastModified) (Id, Name, TotalLength, EstimatedTime, MaxVehicleLength, MaxVehicleWidth, MaxVehicleHeight, SafetyMargin, GridSize, CreatedTime, LastModified)
VALUES (@id, @name, @length, @time, @created, @modified) VALUES (@id, @name, @length, @time, @maxLength, @maxWidth, @maxHeight, @safetyMargin, @gridSize, @created, @modified)
"; ";
using (var cmd = new SQLiteCommand(sql, _connection)) using (var cmd = new SQLiteCommand(sql, _connection))
@ -146,6 +151,11 @@ namespace NavisworksTransport
cmd.Parameters.AddWithValue("@name", route.Name); cmd.Parameters.AddWithValue("@name", route.Name);
cmd.Parameters.AddWithValue("@length", route.TotalLength); cmd.Parameters.AddWithValue("@length", route.TotalLength);
cmd.Parameters.AddWithValue("@time", route.EstimatedTime); 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("@created", route.CreatedTime);
cmd.Parameters.AddWithValue("@modified", DateTime.Now); cmd.Parameters.AddWithValue("@modified", DateTime.Now);
cmd.ExecuteNonQuery(); cmd.ExecuteNonQuery();
@ -393,6 +403,11 @@ namespace NavisworksTransport
Name = reader["Name"].ToString(), Name = reader["Name"].ToString(),
TotalLength = Convert.ToDouble(reader["TotalLength"]), TotalLength = Convert.ToDouble(reader["TotalLength"]),
EstimatedTime = Convert.ToDouble(reader["EstimatedTime"]), 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"]), CreatedTime = Convert.ToDateTime(reader["CreatedTime"]),
LastModified = Convert.ToDateTime(reader["LastModified"]) LastModified = Convert.ToDateTime(reader["LastModified"])
}; };

6
src/Core/PathPlanningManager.cs
View File

@ -1029,9 +1029,11 @@ namespace NavisworksTransport
// 保存GridMap和参数到PathRoute以便后续恢复网格可视化 // 保存GridMap和参数到PathRoute以便后续恢复网格可视化
autoRoute.AssociatedGridMap = gridMap; autoRoute.AssociatedGridMap = gridMap;
autoRoute.GridSize = gridSize; autoRoute.GridSize = gridSize;
autoRoute.VehicleSize = vehicleRadius; // 将vehicleRadius拆分为长宽（暂时使用相同值，后续可以传入更详细的参数）
autoRoute.MaxVehicleLength = vehicleRadius * 2; // 车辆半径转换为长度
autoRoute.MaxVehicleWidth = vehicleRadius * 2; // 车辆半径转换为宽度
autoRoute.MaxVehicleHeight = vehicleHeight;
autoRoute.SafetyMargin = safetyMargin; autoRoute.SafetyMargin = safetyMargin;
autoRoute.VehicleHeight = vehicleHeight;
LogManager.Info($"已保存GridMap到路径: {routeName}, 网格大小: {gridSize}米"); LogManager.Info($"已保存GridMap到路径: {routeName}, 网格大小: {gridSize}米");
// 7. 添加到路径集合 // 7. 添加到路径集合

26
src/Core/PathPlanningModels.cs
View File

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