Initial commit

srcs/RobotNet10/Tests/RobotNet10.GlobalPathPlanner.Test/BidirectionalEdgeTests.cs

@@ -0,0 +1,78 @@
+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);
+    }
+}