867 lines
34 KiB
C#
867 lines
34 KiB
C#
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; }
|
||
}
|
||
}
|
||
} |