79 lines
4.5 KiB
C#
79 lines
4.5 KiB
C#
using RobotNet10.GlobalPathPlanner.Differential;
|
|
using RobotNet10.GlobalPathPlanner.Model;
|
|
|
|
namespace RobotNet10.GlobalPathPlanner.UnitTest;
|
|
|
|
public class BidirectionalEdgeTests
|
|
{
|
|
/// <summary>
|
|
/// Test 1: All straight lines (Degree=1)
|
|
/// A(1.6, 1.6) ↔ B(0, 0) ↔ C(0, -4)
|
|
/// Robot at (0.3, -0.4), theta = -25
|
|
/// </summary>
|
|
[Fact]
|
|
public void PathPlanning_AllLinear_RobotNearB_GoalC()
|
|
{
|
|
var mapId = Guid.Parse("00000000-0000-0000-0000-000000000001");
|
|
|
|
var nodeA = new GlobalNode { Id = Guid.Parse("11111111-1111-1111-1111-111111111111"), MapId = mapId, Name = "A", X = 1.6, Y = 1.6 };
|
|
var nodeB = new GlobalNode { Id = Guid.Parse("22222222-2222-2222-2222-222222222222"), MapId = mapId, Name = "B", X = 0.0, Y = 0.0 };
|
|
var nodeC = new GlobalNode { Id = Guid.Parse("33333333-3333-3333-3333-333333333333"), MapId = mapId, Name = "C", X = 0.0, Y = -4.0 };
|
|
|
|
var edgeAB = new GlobalEdge { Id = Guid.Parse("aaaaaaaa-aaaa-aaaa-aaaa-aaaaaaaaaaaa"), MapId = mapId, StartNodeId = nodeA.Id, EndNodeId = nodeB.Id, Degree = 1 };
|
|
var edgeBA = new GlobalEdge { Id = Guid.Parse("aaaaaaaa-aaaa-aaaa-aaaa-aaaaaaaaaaab"), MapId = mapId, StartNodeId = nodeB.Id, EndNodeId = nodeA.Id, Degree = 1 };
|
|
var edgeBC = new GlobalEdge { Id = Guid.Parse("bbbbbbbb-bbbb-bbbb-bbbb-bbbbbbbbbbbb"), MapId = mapId, StartNodeId = nodeB.Id, EndNodeId = nodeC.Id, Degree = 1 };
|
|
var edgeCB = new GlobalEdge { Id = Guid.Parse("bbbbbbbb-bbbb-bbbb-bbbb-bbbbbbbbbbb2"), MapId = mapId, StartNodeId = nodeC.Id, EndNodeId = nodeB.Id, Degree = 1 };
|
|
|
|
var nodes = new[] { nodeA, nodeB, nodeC };
|
|
var edges = new[] { edgeAB, edgeBA, edgeBC, edgeCB };
|
|
|
|
var planner = new DifferentialPlanner();
|
|
planner.SetData(nodes, edges);
|
|
|
|
var (pathNodes, pathEdges) = planner.PathPlanning(0.3, -0.4, -25.0, nodeC.Id);
|
|
|
|
var pathNames = string.Join(" → ", pathNodes.Select(n => $"{n.Name}({n.X},{n.Y})"));
|
|
bool pathIncludesB = pathNodes.Any(n => n.Id == nodeB.Id);
|
|
|
|
Assert.True(pathIncludesB, $"Path should include node B. Got: {pathNames}");
|
|
Assert.Equal(nodeC.Id, pathNodes[^1].Id);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Test 2: AB is Quadratic Bezier (Degree=2) with control point G(1.6, 0)
|
|
/// A(1.6, 1.6) ↔ B(0, 0) with Degree=2, ControlPoint1=(1.6, 0)
|
|
/// B(0, 0) ↔ C(0, -4) with Degree=1 (straight)
|
|
/// Robot at (0.3, -0.4), theta = -25
|
|
/// </summary>
|
|
[Fact]
|
|
public void PathPlanning_AB_Bezier_RobotNearB_GoalC()
|
|
{
|
|
var mapId = Guid.Parse("00000000-0000-0000-0000-000000000001");
|
|
|
|
var nodeA = new GlobalNode { Id = Guid.Parse("11111111-1111-1111-1111-111111111111"), MapId = mapId, Name = "A", X = 1.6, Y = 1.6 };
|
|
var nodeB = new GlobalNode { Id = Guid.Parse("22222222-2222-2222-2222-222222222222"), MapId = mapId, Name = "B", X = 0.0, Y = 0.0 };
|
|
var nodeC = new GlobalNode { Id = Guid.Parse("33333333-3333-3333-3333-333333333333"), MapId = mapId, Name = "C", X = 0.0, Y = -4.0 };
|
|
|
|
// A↔B: Quadratic Bezier, control point G(1.6, 0)
|
|
var edgeAB = new GlobalEdge { Id = Guid.Parse("aaaaaaaa-aaaa-aaaa-aaaa-aaaaaaaaaaaa"), MapId = mapId, StartNodeId = nodeA.Id, EndNodeId = nodeB.Id, Degree = 2, ControlPoint1X = 1.6, ControlPoint1Y = 0.0 };
|
|
var edgeBA = new GlobalEdge { Id = Guid.Parse("aaaaaaaa-aaaa-aaaa-aaaa-aaaaaaaaaaab"), MapId = mapId, StartNodeId = nodeB.Id, EndNodeId = nodeA.Id, Degree = 2, ControlPoint1X = 1.6, ControlPoint1Y = 0.0 };
|
|
// B↔C: Straight line
|
|
var edgeBC = new GlobalEdge { Id = Guid.Parse("bbbbbbbb-bbbb-bbbb-bbbb-bbbbbbbbbbbb"), MapId = mapId, StartNodeId = nodeB.Id, EndNodeId = nodeC.Id, Degree = 1 };
|
|
var edgeCB = new GlobalEdge { Id = Guid.Parse("bbbbbbbb-bbbb-bbbb-bbbb-bbbbbbbbbbb2"), MapId = mapId, StartNodeId = nodeC.Id, EndNodeId = nodeB.Id, Degree = 1 };
|
|
|
|
var nodes = new[] { nodeA, nodeB, nodeC };
|
|
var edges = new[] { edgeAB, edgeBA, edgeBC, edgeCB };
|
|
|
|
var planner = new DifferentialPlanner();
|
|
planner.SetData(nodes, edges);
|
|
|
|
var (pathNodes, pathEdges) = planner.PathPlanning(0.3, -0.4, -25.0, nodeC.Id);
|
|
|
|
var pathNames = string.Join(" → ", pathNodes.Select(n => $"{n.Name}({n.X},{n.Y})"));
|
|
bool pathIncludesB = pathNodes.Any(n => n.Id == nodeB.Id);
|
|
|
|
Assert.True(pathIncludesB, $"[Bezier AB] Path should include node B. Got: {pathNames}");
|
|
Assert.Equal(nodeC.Id, pathNodes[^1].Id);
|
|
}
|
|
}
|