From 64a6d04ab8e76346ee4cd3ea8bf179178c640848 Mon Sep 17 00:00:00 2001 From: TheNexusAvenger <13441476+TheNexusAvenger@users.noreply.github.com> Date: Mon, 28 Feb 2022 21:27:57 -0500 Subject: [PATCH 01/22] Create helper node class. --- Uchu.NavMesh.Test/Graph/GridNodeTest.cs | 124 ++++++++++++++++++++ Uchu.NavMesh.Test/Uchu.NavMesh.Test.csproj | 21 ++++ Uchu.NavMesh/Graph/GridNode.cs | 128 +++++++++++++++++++++ Uchu.NavMesh/Uchu.NavMesh.csproj | 9 ++ Uchu.sln | 12 ++ 5 files changed, 294 insertions(+) create mode 100644 Uchu.NavMesh.Test/Graph/GridNodeTest.cs create mode 100644 Uchu.NavMesh.Test/Uchu.NavMesh.Test.csproj create mode 100644 Uchu.NavMesh/Graph/GridNode.cs create mode 100644 Uchu.NavMesh/Uchu.NavMesh.csproj diff --git a/Uchu.NavMesh.Test/Graph/GridNodeTest.cs b/Uchu.NavMesh.Test/Graph/GridNodeTest.cs new file mode 100644 index 00000000..022a5300 --- /dev/null +++ b/Uchu.NavMesh.Test/Graph/GridNodeTest.cs @@ -0,0 +1,124 @@ +using System.Collections.Generic; +using System.Numerics; +using NUnit.Framework; +using Uchu.NavMesh.Graph; + +namespace Uchu.NavMesh.Test.Graph; + +public class GridNodeTest +{ + /// + /// Test node used with most of the tests. + /// + public GridNode TestNode { get; set; } + + /// + /// Sets up the test node. + /// + [SetUp] + public void SetUp() + { + // Create the test node. + this.TestNode = new GridNode(new Vector3(2, 0, 2)) + { + Neighbors = new List() + { + new GridNode(new Vector3(2, 4, 3)), + new GridNode(new Vector3(3, -2, 3)), + new GridNode(new Vector3(3, 0, 2)), + new GridNode(new Vector3(1, 1, 1)), + new GridNode(new Vector3(1, 1, 2)), + } + }; + + // Set up the neighbors. + foreach (var neighbor in this.TestNode.Neighbors) + { + neighbor.Neighbors.Add(this.TestNode); + } + } + + /// + /// Tests the RotationTo method. + /// + [Test] + public void TestRotationTo() + { + // Test with itself. + Assert.AreEqual(0, this.TestNode.RotationTo(this.TestNode.Position)); + + // Test the eight rotation multipliers. + Assert.AreEqual(0, this.TestNode.RotationTo(new Vector3(2, 0, 3))); + Assert.AreEqual(1, this.TestNode.RotationTo(new Vector3(3, 0, 3))); + Assert.AreEqual(2, this.TestNode.RotationTo(new Vector3(3, 0, 2))); + Assert.AreEqual(3, this.TestNode.RotationTo(new Vector3(3, 0, 1))); + Assert.AreEqual(4, this.TestNode.RotationTo(new Vector3(2, 0, 1))); + Assert.AreEqual(5, this.TestNode.RotationTo(new Vector3(1, 0, 1))); + Assert.AreEqual(6, this.TestNode.RotationTo(new Vector3(1, 0, 2))); + Assert.AreEqual(7, this.TestNode.RotationTo(new Vector3(1, 0, 3))); + } + + /// + /// Tests the GetNodeByRotation method. + /// + [Test] + public void TestGetNodeByRotation() + { + Assert.AreEqual(this.TestNode.Neighbors[0], this.TestNode.GetNodeByRotation(0)); + Assert.AreEqual(this.TestNode.Neighbors[1], this.TestNode.GetNodeByRotation(1)); + Assert.AreEqual(this.TestNode.Neighbors[2], this.TestNode.GetNodeByRotation(2)); + Assert.IsNull(this.TestNode.GetNodeByRotation(3)); + Assert.IsNull(this.TestNode.GetNodeByRotation(4)); + Assert.AreEqual(this.TestNode.Neighbors[3], this.TestNode.GetNodeByRotation(5)); + Assert.AreEqual(this.TestNode.Neighbors[4], this.TestNode.GetNodeByRotation(6)); + Assert.IsNull(this.TestNode.GetNodeByRotation(7)); + } + + /// + /// Tests the SplitNode method with 2 shapes. + /// + [Test] + public void TestSplitNodeTwoShapes() + { + // Split the nodes and make sure 2 were created. + var createdNodes = this.TestNode.SplitNode(); + Assert.AreEqual(createdNodes.Count, 2); + + // Check the neighbors. + Assert.AreEqual(createdNodes[0], this.TestNode.Neighbors[0].Neighbors[0]); + Assert.AreEqual(createdNodes[0], this.TestNode.Neighbors[1].Neighbors[0]); + Assert.AreEqual(createdNodes[0], this.TestNode.Neighbors[2].Neighbors[0]); + Assert.AreEqual(createdNodes[1], this.TestNode.Neighbors[3].Neighbors[0]); + Assert.AreEqual(createdNodes[1], this.TestNode.Neighbors[4].Neighbors[0]); + } + + /// + /// Tests the SplitNode method with 1 shape and 1 extra edge. + /// + [Test] + public void TestSplitNodeOneShapeOneExtraEdge() + { + // Split the nodes and make sure 1 was created. + this.TestNode.Neighbors.RemoveAt(4); + var createdNodes = this.TestNode.SplitNode(); + Assert.AreEqual(createdNodes.Count, 1); + + // Check the neighbors. + Assert.AreEqual(createdNodes[0], this.TestNode.Neighbors[0].Neighbors[0]); + Assert.AreEqual(createdNodes[0], this.TestNode.Neighbors[1].Neighbors[0]); + Assert.AreEqual(createdNodes[0], this.TestNode.Neighbors[2].Neighbors[0]); + Assert.AreEqual(0, this.TestNode.Neighbors[3].Neighbors.Count); + } + + /// + /// Tests the SplitNode method with the optimization for 8 all edges. + /// + [Test] + public void TestSplitNodeAllEdges() + { + this.TestNode.Neighbors.Add(new GridNode(new Vector3(1, 0, 3))); + this.TestNode.Neighbors.Add(new GridNode(new Vector3(2, 0, 1))); + this.TestNode.Neighbors.Add(new GridNode(new Vector3(3, 0, 1))); + Assert.AreEqual(this.TestNode, this.TestNode.SplitNode()[0]); + } +} \ No newline at end of file diff --git a/Uchu.NavMesh.Test/Uchu.NavMesh.Test.csproj b/Uchu.NavMesh.Test/Uchu.NavMesh.Test.csproj new file mode 100644 index 00000000..11c3f294 --- /dev/null +++ b/Uchu.NavMesh.Test/Uchu.NavMesh.Test.csproj @@ -0,0 +1,21 @@ + + + + net6.0 + enable + + false + + + + + + + + + + + + + + diff --git a/Uchu.NavMesh/Graph/GridNode.cs b/Uchu.NavMesh/Graph/GridNode.cs new file mode 100644 index 00000000..845cbdff --- /dev/null +++ b/Uchu.NavMesh/Graph/GridNode.cs @@ -0,0 +1,128 @@ +using System.Numerics; + +namespace Uchu.NavMesh.Graph; + +public class GridNode +{ + /// + /// Position of the grid node. + /// + public Vector3 Position { get; set; } + + /// + /// Neighbors of the node. + /// + public List Neighbors { get; set; } = new List(8); + + /// + /// Creates the node. + /// + /// Position of the node. + public GridNode(Vector3 position) + { + this.Position = position; + } + + /// + /// Returns the "rotation" on the XZ plane to a given target. Each increase in 1 represents 45 degrees and is + /// used since the nodes are on a 2D grid with every neighbor being +/-1 on the X and Z on the grid. + /// + /// Position to target. The Y value is ignored. + /// The byte multiplier of the angle. + public byte RotationTo(Vector3 target) + { + // Return 0 if the position is 0 to avoid a divide-by-zero error. + if (this.Position.X == target.X && this.Position.Z == target.Z) + { + return 0; + } + + // Return the angle. + var angle = Math.Atan2(target.X - this.Position.X, target.Z - this.Position.Z); + if (angle < 0) + { + angle += (2 * Math.PI); + } + return (byte) Math.Round(angle / (Math.PI * 0.25)); + } + + /// + /// Returns the neighbor node that is a rotation * 45 degrees from the node. + /// + /// Rotation multiplier to use. + /// Node at the given rotation. + public GridNode? GetNodeByRotation(byte rotation) + { + return this.Neighbors.FirstOrDefault(node => this.RotationTo(node.Position) == rotation); + } + + /// + /// Splits the node so that no 2 shapes share the same node instance. + /// + /// The nodes that were created. + public List SplitNode() + { + // Get the nodes for each angle. + var nodesAtAngles = new GridNode?[8]; + var totalNodes = 0; + for (byte i = 0; i < 8; i++) + { + var node = this.GetNodeByRotation(i); + if (node == null) continue; + nodesAtAngles[i] = node; + totalNodes += 1; + } + + // Return if all 8 angles are covered, or there are no nodes to operate on. + if (totalNodes == 0 || totalNodes == 8) + { + return new List(1) { this }; + } + + // Determine a starting offset where the first index does not need to be replaced. + // This ensures that the replaced nodes do not start at the middle. + var startOffset = 0; + for (var i = 0; i < 8; i++) + { + if (nodesAtAngles[i] != null) continue; + startOffset = i; + } + + // Replaces the node references of the neighbors. + var createdNodes = new List(3); + var newNodesForNeighbor = new GridNode?[8]; + for (var i = 0; i < 8; i++) + { + // Get the current neighbor and continue if there is no neighbor to act on. + var currentIndex = (i + startOffset) % 8; + var currentNeighbor = nodesAtAngles[currentIndex]; + if (currentNeighbor == null) continue; + + // Get the previous and next neighbors. + // The previous index calculation uses +7 instead of -1 to ensure a positive result. + var previousIndex = (currentIndex + 7) % 8; + var nextIndex = (currentIndex + 1) % 8; + var previousNeighbor = nodesAtAngles[previousIndex]; + var nextNeighbor = nodesAtAngles[nextIndex]; + + // Remove the edge and continue if there is no previous or next neighbor. + currentNeighbor.Neighbors.Remove(this); + if (previousNeighbor == null && nextNeighbor == null) continue; + + // Get the new node to use. + var newNode = newNodesForNeighbor[previousIndex]; + if (newNode == null) + { + newNode = new GridNode(this.Position); + createdNodes.Add(newNode); + } + newNodesForNeighbor[currentIndex] = newNode; + + // Replace the neighbor. + currentNeighbor.Neighbors.Add(newNode); + } + + // Return the created nodes. + return createdNodes; + } +} \ No newline at end of file diff --git a/Uchu.NavMesh/Uchu.NavMesh.csproj b/Uchu.NavMesh/Uchu.NavMesh.csproj new file mode 100644 index 00000000..eb2460e9 --- /dev/null +++ b/Uchu.NavMesh/Uchu.NavMesh.csproj @@ -0,0 +1,9 @@ + + + + net6.0 + enable + enable + + + diff --git a/Uchu.sln b/Uchu.sln index a7f022b5..86301f24 100644 --- a/Uchu.sln +++ b/Uchu.sln @@ -47,6 +47,10 @@ Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Uchu.Physics.Test", "Uchu.P EndProject Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Nexus.Logging", "NexusLogging\Nexus.Logging\Nexus.Logging.csproj", "{0282F87A-8F2A-4385-9ACD-7DC647CAD1B9}" EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Uchu.NavMesh", "Uchu.NavMesh\Uchu.NavMesh.csproj", "{E9EB9299-5BF3-49B5-B06C-0C3426478360}" +EndProject +Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Uchu.NavMesh.Test", "Uchu.NavMesh.Test\Uchu.NavMesh.Test.csproj", "{CF14933A-2B86-4966-B370-97B5BFEDE247}" +EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution Debug|Any CPU = Debug|Any CPU @@ -145,6 +149,14 @@ Global {0282F87A-8F2A-4385-9ACD-7DC647CAD1B9}.Debug|Any CPU.Build.0 = Debug|Any CPU {0282F87A-8F2A-4385-9ACD-7DC647CAD1B9}.Release|Any CPU.ActiveCfg = Release|Any CPU {0282F87A-8F2A-4385-9ACD-7DC647CAD1B9}.Release|Any CPU.Build.0 = Release|Any CPU + {E9EB9299-5BF3-49B5-B06C-0C3426478360}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {E9EB9299-5BF3-49B5-B06C-0C3426478360}.Debug|Any CPU.Build.0 = Debug|Any CPU + {E9EB9299-5BF3-49B5-B06C-0C3426478360}.Release|Any CPU.ActiveCfg = Release|Any CPU + {E9EB9299-5BF3-49B5-B06C-0C3426478360}.Release|Any CPU.Build.0 = Release|Any CPU + {CF14933A-2B86-4966-B370-97B5BFEDE247}.Debug|Any CPU.ActiveCfg = Debug|Any CPU + {CF14933A-2B86-4966-B370-97B5BFEDE247}.Debug|Any CPU.Build.0 = Debug|Any CPU + {CF14933A-2B86-4966-B370-97B5BFEDE247}.Release|Any CPU.ActiveCfg = Release|Any CPU + {CF14933A-2B86-4966-B370-97B5BFEDE247}.Release|Any CPU.Build.0 = Release|Any CPU EndGlobalSection GlobalSection(MonoDevelopProperties) = preSolution Policies = $0 From 72031a1fc7b84aee00b1e15d55ce03eb1f21e463 Mon Sep 17 00:00:00 2001 From: TheNexusAvenger <13441476+TheNexusAvenger@users.noreply.github.com> Date: Mon, 28 Feb 2022 22:29:26 -0500 Subject: [PATCH 02/22] Remove neighbors to self when split. --- Uchu.NavMesh.Test/Graph/GridNodeTest.cs | 24 +++++++++++++++--------- Uchu.NavMesh/Graph/GridNode.cs | 1 + 2 files changed, 16 insertions(+), 9 deletions(-) diff --git a/Uchu.NavMesh.Test/Graph/GridNodeTest.cs b/Uchu.NavMesh.Test/Graph/GridNodeTest.cs index 022a5300..5030f7ab 100644 --- a/Uchu.NavMesh.Test/Graph/GridNodeTest.cs +++ b/Uchu.NavMesh.Test/Graph/GridNodeTest.cs @@ -1,4 +1,5 @@ using System.Collections.Generic; +using System.Linq; using System.Numerics; using NUnit.Framework; using Uchu.NavMesh.Graph; @@ -81,15 +82,17 @@ public void TestGetNodeByRotation() public void TestSplitNodeTwoShapes() { // Split the nodes and make sure 2 were created. + var neighbors = this.TestNode.Neighbors.ToList(); var createdNodes = this.TestNode.SplitNode(); Assert.AreEqual(createdNodes.Count, 2); + Assert.AreEqual(this.TestNode.Neighbors.Count, 0); // Check the neighbors. - Assert.AreEqual(createdNodes[0], this.TestNode.Neighbors[0].Neighbors[0]); - Assert.AreEqual(createdNodes[0], this.TestNode.Neighbors[1].Neighbors[0]); - Assert.AreEqual(createdNodes[0], this.TestNode.Neighbors[2].Neighbors[0]); - Assert.AreEqual(createdNodes[1], this.TestNode.Neighbors[3].Neighbors[0]); - Assert.AreEqual(createdNodes[1], this.TestNode.Neighbors[4].Neighbors[0]); + Assert.AreEqual(createdNodes[0], neighbors[0].Neighbors[0]); + Assert.AreEqual(createdNodes[0], neighbors[1].Neighbors[0]); + Assert.AreEqual(createdNodes[0], neighbors[2].Neighbors[0]); + Assert.AreEqual(createdNodes[1], neighbors[3].Neighbors[0]); + Assert.AreEqual(createdNodes[1], neighbors[4].Neighbors[0]); } /// @@ -100,14 +103,16 @@ public void TestSplitNodeOneShapeOneExtraEdge() { // Split the nodes and make sure 1 was created. this.TestNode.Neighbors.RemoveAt(4); + var neighbors = this.TestNode.Neighbors.ToList(); var createdNodes = this.TestNode.SplitNode(); Assert.AreEqual(createdNodes.Count, 1); + Assert.AreEqual(this.TestNode.Neighbors.Count, 0); // Check the neighbors. - Assert.AreEqual(createdNodes[0], this.TestNode.Neighbors[0].Neighbors[0]); - Assert.AreEqual(createdNodes[0], this.TestNode.Neighbors[1].Neighbors[0]); - Assert.AreEqual(createdNodes[0], this.TestNode.Neighbors[2].Neighbors[0]); - Assert.AreEqual(0, this.TestNode.Neighbors[3].Neighbors.Count); + Assert.AreEqual(createdNodes[0], neighbors[0].Neighbors[0]); + Assert.AreEqual(createdNodes[0], neighbors[1].Neighbors[0]); + Assert.AreEqual(createdNodes[0], neighbors[2].Neighbors[0]); + Assert.AreEqual(0, neighbors[3].Neighbors.Count); } /// @@ -120,5 +125,6 @@ public void TestSplitNodeAllEdges() this.TestNode.Neighbors.Add(new GridNode(new Vector3(2, 0, 1))); this.TestNode.Neighbors.Add(new GridNode(new Vector3(3, 0, 1))); Assert.AreEqual(this.TestNode, this.TestNode.SplitNode()[0]); + Assert.AreEqual(this.TestNode.Neighbors.Count, 8); } } \ No newline at end of file diff --git a/Uchu.NavMesh/Graph/GridNode.cs b/Uchu.NavMesh/Graph/GridNode.cs index 845cbdff..dd3131ae 100644 --- a/Uchu.NavMesh/Graph/GridNode.cs +++ b/Uchu.NavMesh/Graph/GridNode.cs @@ -123,6 +123,7 @@ public List SplitNode() } // Return the created nodes. + this.Neighbors.Clear(); return createdNodes; } } \ No newline at end of file From 75fa18b8184e768641d559064372f0398116a7e7 Mon Sep 17 00:00:00 2001 From: TheNexusAvenger <13441476+TheNexusAvenger@users.noreply.github.com> Date: Mon, 28 Feb 2022 22:40:06 -0500 Subject: [PATCH 03/22] Add GetOuterNeighbors helper method. --- Uchu.NavMesh.Test/Graph/GridNodeTest.cs | 15 +++++++ Uchu.NavMesh/Graph/GridNode.cs | 53 +++++++++++++++++++++++-- 2 files changed, 64 insertions(+), 4 deletions(-) diff --git a/Uchu.NavMesh.Test/Graph/GridNodeTest.cs b/Uchu.NavMesh.Test/Graph/GridNodeTest.cs index 5030f7ab..6132e29a 100644 --- a/Uchu.NavMesh.Test/Graph/GridNodeTest.cs +++ b/Uchu.NavMesh.Test/Graph/GridNodeTest.cs @@ -127,4 +127,19 @@ public void TestSplitNodeAllEdges() Assert.AreEqual(this.TestNode, this.TestNode.SplitNode()[0]); Assert.AreEqual(this.TestNode.Neighbors.Count, 8); } + + /// + /// Tests the GetOuterNeighbors method. + /// + [Test] + public void TestGetOuterNeighbors() + { + Assert.AreEqual(new List() + { + this.TestNode.Neighbors[0], + this.TestNode.Neighbors[2], + this.TestNode.Neighbors[3], + this.TestNode.Neighbors[4], + }, this.TestNode.GetOuterNeighbors()); + } } \ No newline at end of file diff --git a/Uchu.NavMesh/Graph/GridNode.cs b/Uchu.NavMesh/Graph/GridNode.cs index dd3131ae..6b3f1813 100644 --- a/Uchu.NavMesh/Graph/GridNode.cs +++ b/Uchu.NavMesh/Graph/GridNode.cs @@ -56,6 +56,22 @@ public byte RotationTo(Vector3 target) return this.Neighbors.FirstOrDefault(node => this.RotationTo(node.Position) == rotation); } + /// + /// Returns the neighbors where the index is the rotation multiple of 45 degrees. + /// + /// The neighbors where the index is the rotation multiple of 45 degrees. + private GridNode?[] GetNodesAtRotation() + { + var nodesAtAngles = new GridNode?[8]; + for (byte i = 0; i < 8; i++) + { + var node = this.GetNodeByRotation(i); + if (node == null) continue; + nodesAtAngles[i] = node; + } + return nodesAtAngles; + } + /// /// Splits the node so that no 2 shapes share the same node instance. /// @@ -63,13 +79,11 @@ public byte RotationTo(Vector3 target) public List SplitNode() { // Get the nodes for each angle. - var nodesAtAngles = new GridNode?[8]; + var nodesAtAngles = this.GetNodesAtRotation(); var totalNodes = 0; for (byte i = 0; i < 8; i++) { - var node = this.GetNodeByRotation(i); - if (node == null) continue; - nodesAtAngles[i] = node; + if (nodesAtAngles[i] == null) continue; totalNodes += 1; } @@ -126,4 +140,35 @@ public List SplitNode() this.Neighbors.Clear(); return createdNodes; } + + /// + /// Returns the neighbors that do not have a neighbor on the left or right. + /// + /// the neighbors that do not have a neighbor on the left or right. + public List GetOuterNeighbors() + { + // Get the neighbors that only have a left or right neighbor (not both). + var nodesAtAngles = this.GetNodesAtRotation(); + var neighbors = new List(4); + for (var i = 0; i < 8; i++) + { + // Get the current neighbor and continue if there is no neighbor to act on. + var currentNeighbor = nodesAtAngles[i]; + if (currentNeighbor == null) continue; + + // Get the previous and next neighbors. + // The previous index calculation uses +7 instead of -1 to ensure a positive result. + var previousIndex = (i + 7) % 8; + var nextIndex = (i + 1) % 8; + var previousNeighbor = nodesAtAngles[previousIndex]; + var nextNeighbor = nodesAtAngles[nextIndex]; + + // Add the neighbor if isn't both a left and right neighbor. + if (previousNeighbor != null && nextNeighbor != null) continue; + neighbors.Add(currentNeighbor); + } + + // Return the neighbors. + return neighbors; + } } \ No newline at end of file From ca3d6de7d4a8a1ceac7c42da6a05670585b60069 Mon Sep 17 00:00:00 2001 From: TheNexusAvenger <13441476+TheNexusAvenger@users.noreply.github.com> Date: Mon, 28 Feb 2022 22:51:09 -0500 Subject: [PATCH 04/22] Create edge helper class. --- Uchu.NavMesh.Test/Graph/GridEdgeTest.cs | 38 +++++++++++++++ Uchu.NavMesh/Graph/GridEdge.cs | 61 +++++++++++++++++++++++++ 2 files changed, 99 insertions(+) create mode 100644 Uchu.NavMesh.Test/Graph/GridEdgeTest.cs create mode 100644 Uchu.NavMesh/Graph/GridEdge.cs diff --git a/Uchu.NavMesh.Test/Graph/GridEdgeTest.cs b/Uchu.NavMesh.Test/Graph/GridEdgeTest.cs new file mode 100644 index 00000000..50f64aad --- /dev/null +++ b/Uchu.NavMesh.Test/Graph/GridEdgeTest.cs @@ -0,0 +1,38 @@ +using System.Numerics; +using NUnit.Framework; +using Uchu.NavMesh.Graph; + +namespace Uchu.NavMesh.Test.Graph; + +public class GridEdgeTest +{ + /// + /// First test edge used. + /// + public GridEdge TestEdge1 = new GridEdge(Vector3.One, Vector3.Zero); + + /// + /// Second test edge used. + /// + public GridEdge TestEdge2 = new GridEdge(Vector3.Zero, Vector3.One); + + /// + /// Tests the Equals method. + /// + [Test] + public void TestEquals() + { + Assert.AreEqual(this.TestEdge1, this.TestEdge1); + Assert.AreEqual(this.TestEdge2, this.TestEdge2); + Assert.AreEqual(this.TestEdge1, this.TestEdge2); + } + + /// + /// Tests the GetHashCode method. + /// + [Test] + public void TestGetHashCode() + { + Assert.AreEqual(this.TestEdge1.GetHashCode(), this.TestEdge2.GetHashCode()); + } +} \ No newline at end of file diff --git a/Uchu.NavMesh/Graph/GridEdge.cs b/Uchu.NavMesh/Graph/GridEdge.cs new file mode 100644 index 00000000..1a27724a --- /dev/null +++ b/Uchu.NavMesh/Graph/GridEdge.cs @@ -0,0 +1,61 @@ +using System.Numerics; + +namespace Uchu.NavMesh.Graph; + +public class GridEdge : IEquatable +{ + /// + /// Start of the edge. + /// + public readonly Vector3 Start; + + /// + /// End of the edge. + /// + public readonly Vector3 End; + + /// + /// Creates the grid edge. + /// + /// Start of the edge. + /// End of the edge. + public GridEdge(Vector3 start, Vector3 end) + { + this.Start = start; + this.End = end; + } + + /// + /// Returns if another edge is equal. + /// + /// The other edge to compare. + /// If the edges are equal. + public bool Equals(GridEdge? other) + { + if (ReferenceEquals(null, other)) return false; + if (ReferenceEquals(this, other)) return true; + return (this.Start.Equals(other.Start) && End.Equals(other.End)) || (this.Start.Equals(other.End) && End.Equals(other.Start)); + } + + /// + /// Returns if another object is equal. + /// + /// The object to compare. + /// If the objects are equal. + public override bool Equals(object? obj) + { + if (ReferenceEquals(null, obj)) return false; + if (ReferenceEquals(this, obj)) return true; + if (obj.GetType() != this.GetType()) return false; + return Equals((GridEdge) obj); + } + + /// + /// Returns the hash code of the object. + /// + /// The hash code of the object. + public override int GetHashCode() + { + return HashCode.Combine(this.Start, this.End) + HashCode.Combine(this.End, this.Start); + } +} \ No newline at end of file From 5a927c22cf2638e02b375c8236db1ddf1b330868 Mon Sep 17 00:00:00 2001 From: TheNexusAvenger <13441476+TheNexusAvenger@users.noreply.github.com> Date: Tue, 1 Mar 2022 15:48:24 -0500 Subject: [PATCH 05/22] Add polygon helper classes. --- Uchu.NavMesh.Test/Graph/GridPolygonTest.cs | 226 +++++++++++++++++++++ Uchu.NavMesh/Graph/GridPolygon.cs | 177 ++++++++++++++++ Uchu.NavMesh/Graph/OrderedPolygon.cs | 11 + 3 files changed, 414 insertions(+) create mode 100644 Uchu.NavMesh.Test/Graph/GridPolygonTest.cs create mode 100644 Uchu.NavMesh/Graph/GridPolygon.cs create mode 100644 Uchu.NavMesh/Graph/OrderedPolygon.cs diff --git a/Uchu.NavMesh.Test/Graph/GridPolygonTest.cs b/Uchu.NavMesh.Test/Graph/GridPolygonTest.cs new file mode 100644 index 00000000..49b509a0 --- /dev/null +++ b/Uchu.NavMesh.Test/Graph/GridPolygonTest.cs @@ -0,0 +1,226 @@ +using System.Collections.Generic; +using System.Linq; +using System.Numerics; +using NUnit.Framework; +using Uchu.NavMesh.Graph; + +namespace Uchu.NavMesh.Test.Graph; + +public class GridPolygonTest +{ + /// + /// Tests the FromNodes method with connected squares. + /// + [Test] + public void TestFromNodesSquare() + { + // Test with unfilled square. + var node1 = new GridNode(new Vector3(0, 0, 0)); + var node2 = new GridNode(new Vector3(0, 0, 1)); + var node3 = new GridNode(new Vector3(1, 0, 0)); + var node4 = new GridNode(new Vector3(1, 0, 1)); + node1.Neighbors = new List() + { + node2, + node3, + }; + node2.Neighbors = new List() + { + node1, + node4, + }; + node3.Neighbors = new List() + { + node1, + node4, + }; + node4.Neighbors = new List() + { + node2, + node3, + }; + Assert.IsNull(GridPolygon.FromNodes(node1, node2, node3, node4)); + + // Test with a filled square. + node2.Neighbors.Add(node3); + Assert.AreEqual(4, GridPolygon.FromNodes(node1, node2, node3, node4)!.Edges.Count); + } + + /// + /// Tests the FromNodes method with connected triangles. + /// + [Test] + public void TestFromNodesTriangles() + { + var node1 = new GridNode(new Vector3(0, 0, 0)); + var node2 = new GridNode(new Vector3(0, 0, 1)); + var node3 = new GridNode(new Vector3(1, 0, 0)); + var node4 = new GridNode(new Vector3(1, 0, 1)); + node1.Neighbors = new List() + { + node2, + node3, + }; + node2.Neighbors = new List() + { + node1, + node3, + }; + node3.Neighbors = new List() + { + node1, + node2, + }; + + Assert.AreEqual(3, GridPolygon.FromNodes(node1, node2, node3, node4)!.Edges.Count); + Assert.AreEqual(3, GridPolygon.FromNodes(node2, node3, node4, node1)!.Edges.Count); + Assert.AreEqual(3, GridPolygon.FromNodes(node3, node4, node1, node2)!.Edges.Count); + Assert.AreEqual(3, GridPolygon.FromNodes(node4, node1, node2, node3)!.Edges.Count); + } + + /// + /// Tests the CanMerge method. + /// + [Test] + public void TestCanMerge() + { + // Test CanMerge on the same polygon. + var polygon1 = new GridPolygon() + { + Edges = new HashSet() + { + new GridEdge(new Vector3(0, 0, 0), new Vector3(1, 0, 1)), + new GridEdge(new Vector3(1, 0, 1), new Vector3(0, 0, 1)), + new GridEdge(new Vector3(0, 0, 1), new Vector3(0, 0, 0)), + } + }; + Assert.IsFalse(polygon1.CanMerge(polygon1)); + + // Test CanMerge with a common side. + var polygon2 = new GridPolygon() + { + Edges = new HashSet() + { + new GridEdge(new Vector3(1, 0, 0), new Vector3(1, 0, 1)), + new GridEdge(new Vector3(1, 0, 1), new Vector3(0, 0, 1)), + new GridEdge(new Vector3(0, 0, 1), new Vector3(1, 0, 0)), + } + }; + Assert.IsTrue(polygon1.CanMerge(polygon2)); + + // Test CanMerge with no common side. + var polygon3 = new GridPolygon() + { + Edges = new HashSet() + { + new GridEdge(new Vector3(0, 0, 0), new Vector3(2, 0, 2)), + new GridEdge(new Vector3(2, 0, 2), new Vector3(0, 0, 2)), + new GridEdge(new Vector3(0, 0, 2), new Vector3(0, 0, 0)), + } + }; + Assert.IsFalse(polygon1.CanMerge(polygon3)); + } + + /// + /// Tests the Merge method. + /// + [Test] + public void TestMerge() + { + var polygon1 = new GridPolygon() + { + Edges = new HashSet() + { + new GridEdge(new Vector3(0, 0, 0), new Vector3(1, 0, 1)), + new GridEdge(new Vector3(1, 0, 1), new Vector3(0, 0, 1)), + new GridEdge(new Vector3(0, 0, 1), new Vector3(0, 0, 0)), + } + }; + var polygon2 = new GridPolygon() + { + Edges = new HashSet() + { + new GridEdge(new Vector3(1, 0, 0), new Vector3(1, 0, 1)), + new GridEdge(new Vector3(1, 0, 1), new Vector3(0, 0, 1)), + new GridEdge(new Vector3(0, 0, 1), new Vector3(1, 0, 0)), + } + }; + var originalEdges1 = polygon1.Edges.ToList(); + var originalEdges2 = polygon2.Edges.ToList(); + polygon1.Merge(polygon2); + + var edges = polygon1.Edges.ToList(); + Assert.IsTrue(edges.Contains(originalEdges1[0])); + Assert.IsFalse(edges.Contains(originalEdges1[1])); + Assert.IsTrue(edges.Contains(originalEdges1[2])); + Assert.IsTrue(edges.Contains(originalEdges2[0])); + Assert.IsFalse(edges.Contains(originalEdges2[1])); + Assert.IsTrue(edges.Contains(originalEdges2[2])); + } + + /// + /// Tests the GetOrderedPolygonsSingleShape method with a single shape. + /// + [Test] + public void TestGetOrderedPolygonsSingleShape() + { + var polygon = new GridPolygon() + { + Edges = new HashSet() + { + new GridEdge(new Vector3(0, 0, 0), new Vector3(1, 0, 0)), + new GridEdge(new Vector3(1, 0, 1), new Vector3(0, 0, 1)), + new GridEdge(new Vector3(0, 0, 1), new Vector3(0, 0, 0)), + new GridEdge(new Vector3(1, 0, 1), new Vector3(1, 0, 0)), + } + }; + + var orderedPolygons = polygon.GetOrderedPolygons(); + Assert.AreEqual(new List() + { + new Vector2(0, 0), + new Vector2(1, 0), + new Vector2(1, 1), + new Vector2(0, 1), + }, orderedPolygons[0].Points); + } + + /// + /// Tests the GetOrderedPolygonsSingleShape method with a two shapes. + /// + [Test] + public void TestGetOrderedPolygonsTwoShapes() + { + var polygon = new GridPolygon() + { + Edges = new HashSet() + { + // Shape 1. + new GridEdge(new Vector3(0, 0, 0), new Vector3(1, 0, 0)), + new GridEdge(new Vector3(1, 0, 1), new Vector3(0, 0, 1)), + new GridEdge(new Vector3(0, 0, 1), new Vector3(0, 0, 0)), + new GridEdge(new Vector3(1, 0, 1), new Vector3(1, 0, 0)), + + // Shape 2. + new GridEdge(new Vector3(0, 0, 0), new Vector3(-1, 0, 0)), + new GridEdge(new Vector3(-1, 0, 0), new Vector3(-1, 0, -1)), + new GridEdge(new Vector3(-1, 0, -1), new Vector3(0, 0, 0)), + } + }; + + var orderedPolygons = polygon.GetOrderedPolygons(); + Assert.AreEqual(new List() + { + new Vector2(0, 0), + new Vector2(1, 0), + new Vector2(1, 1), + new Vector2(0, 1), + }, orderedPolygons[0].Points); + Assert.AreEqual(new List() + { + new Vector2(0, 0), + new Vector2(-1, 0), + new Vector2(-1, -1), + }, orderedPolygons[1].Points); + } +} \ No newline at end of file diff --git a/Uchu.NavMesh/Graph/GridPolygon.cs b/Uchu.NavMesh/Graph/GridPolygon.cs new file mode 100644 index 00000000..1104c73c --- /dev/null +++ b/Uchu.NavMesh/Graph/GridPolygon.cs @@ -0,0 +1,177 @@ +using System.Numerics; + +namespace Uchu.NavMesh.Graph; + +public class GridPolygon +{ + /// + /// Edges of the polygon. + /// + public HashSet Edges { get; set; } = new HashSet(); + + /// + /// Returns a polygon from a set of nodes. + /// + /// Corner 1 of the polygon. + /// Corner 2 of the polygon. + /// Corner 3 of the polygon. + /// Corner 4 of the polygon. + /// The created polygon. + public static GridPolygon? FromNodes(GridNode node1, GridNode node2, GridNode node3, GridNode node4) + { + // Return either a polygon of the square or null if the square is not filled. + if (node1.Neighbors.Contains(node2) && node1.Neighbors.Contains(node3) && node4.Neighbors.Contains(node2) && node4.Neighbors.Contains(node3)) + { + if (node1.Neighbors.Contains(node4) || node2.Neighbors.Contains(node3)) + { + return new GridPolygon() + { + Edges = { + new GridEdge(node1.Position, node2.Position), + new GridEdge(node1.Position, node3.Position), + new GridEdge(node4.Position, node2.Position), + new GridEdge(node4.Position, node3.Position), + }, + }; + } + return null; + } + + // Return a triangle polygon. + if (node1.Neighbors.Contains(node2) && node2.Neighbors.Contains(node3) && node3.Neighbors.Contains(node1)) + { + // Return a polygon without node 4. + return new GridPolygon() + { + Edges = { + new GridEdge(node1.Position, node2.Position), + new GridEdge(node2.Position, node3.Position), + new GridEdge(node3.Position, node1.Position), + }, + }; + } + if (node2.Neighbors.Contains(node3) && node3.Neighbors.Contains(node4) && node4.Neighbors.Contains(node2)) + { + // Return a polygon without node 1. + return new GridPolygon() + { + Edges = { + new GridEdge(node2.Position, node3.Position), + new GridEdge(node3.Position, node4.Position), + new GridEdge(node4.Position, node2.Position), + }, + }; + } + if (node1.Neighbors.Contains(node3) && node3.Neighbors.Contains(node4) && node4.Neighbors.Contains(node1)) + { + // Return a polygon without node 2. + return new GridPolygon() + { + Edges = { + new GridEdge(node1.Position, node3.Position), + new GridEdge(node3.Position, node4.Position), + new GridEdge(node4.Position, node1.Position), + }, + }; + } + if (node1.Neighbors.Contains(node2) && node2.Neighbors.Contains(node4) && node4.Neighbors.Contains(node1)) + { + // Return a polygon without node 3. + return new GridPolygon() + { + Edges = { + new GridEdge(node1.Position, node2.Position), + new GridEdge(node2.Position, node4.Position), + new GridEdge(node4.Position, node1.Position), + }, + }; + } + + // Return null (not valid). + return null; + } + + /// + /// Returns if a polygon can merge. + /// + /// Polygon to check merging. + /// Whether the merge can be done. + public bool CanMerge(GridPolygon polygon) + { + if (polygon == this) return false; + return (from edge in this.Edges from otherEdge in polygon.Edges where edge.Equals(otherEdge) select edge).Any(); + } + + /// + /// Merges another polygon. + /// + /// Polygon to merge. + public void Merge(GridPolygon polygon) + { + foreach (var edge in polygon.Edges) + { + if (this.Edges.Contains(edge)) + { + this.Edges.Remove(edge); + } + else + { + this.Edges.Add(edge); + } + } + } + + /// + /// Returns a list of ordered polygons for the current polygon. + /// + /// Ordered polygons from the current edges. + public List GetOrderedPolygons() + { + // Iterate over the edges. + var orderedPolygons = new List(); + var remainingEdges = this.Edges.ToList(); + var currentPoints = new List(); + while (remainingEdges.Count != 0) + { + if (currentPoints.Count == 0) + { + // Add the points of the current edge. + var edge = remainingEdges[0]; + remainingEdges.RemoveAt(0); + currentPoints.Add(edge.Start); + currentPoints.Add(edge.End); + } + else + { + // Get the next point. + var lastPoint = currentPoints[^1]; + var nextEdge = remainingEdges.First(edge => edge.Start == lastPoint || edge.End == lastPoint); + remainingEdges.Remove(nextEdge); + var nextPoint = (nextEdge.Start == lastPoint ? nextEdge.End : nextEdge.Start); + + // Add the current points as a shape if a cycle was made. + if (currentPoints.Contains(nextPoint)) + { + var newPoints = new List(); + var startIndex = currentPoints.IndexOf(nextPoint); + for (var i = startIndex; i < currentPoints.Count; i++) + { + var point = currentPoints[i]; + newPoints.Add(new Vector2(point.X, point.Z)); + } + orderedPolygons.Add(new OrderedPolygon() + { + Points = newPoints, + }); + currentPoints.RemoveRange(startIndex, currentPoints.Count - startIndex); + } + + // Add the point. + currentPoints.Add(nextPoint); + } + } + + // Return the ordered polygons. + return orderedPolygons; + } +} \ No newline at end of file diff --git a/Uchu.NavMesh/Graph/OrderedPolygon.cs b/Uchu.NavMesh/Graph/OrderedPolygon.cs new file mode 100644 index 00000000..54d0f084 --- /dev/null +++ b/Uchu.NavMesh/Graph/OrderedPolygon.cs @@ -0,0 +1,11 @@ +using System.Numerics; + +namespace Uchu.NavMesh.Graph; + +public class OrderedPolygon +{ + /// + /// Points of the ordered polygon. + /// + public List Points { get; set; } = new List(); +} \ No newline at end of file From b150cdd2ad973bb56f6db7c41154c56b60209f19 Mon Sep 17 00:00:00 2001 From: TheNexusAvenger <13441476+TheNexusAvenger@users.noreply.github.com> Date: Tue, 1 Mar 2022 16:05:10 -0500 Subject: [PATCH 06/22] Fix edge case with extra edges not part of a shape. --- Uchu.NavMesh/Graph/GridPolygon.cs | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/Uchu.NavMesh/Graph/GridPolygon.cs b/Uchu.NavMesh/Graph/GridPolygon.cs index 1104c73c..29e234c7 100644 --- a/Uchu.NavMesh/Graph/GridPolygon.cs +++ b/Uchu.NavMesh/Graph/GridPolygon.cs @@ -145,7 +145,12 @@ public List GetOrderedPolygons() { // Get the next point. var lastPoint = currentPoints[^1]; - var nextEdge = remainingEdges.First(edge => edge.Start == lastPoint || edge.End == lastPoint); + var nextEdge = remainingEdges.FirstOrDefault(edge => edge.Start == lastPoint || edge.End == lastPoint); + if (nextEdge == null) + { + currentPoints.RemoveAt(currentPoints.Count - 1); + continue; + } remainingEdges.Remove(nextEdge); var nextPoint = (nextEdge.Start == lastPoint ? nextEdge.End : nextEdge.Start); From c765e308c072df19ee145a59cb0049a65d074054 Mon Sep 17 00:00:00 2001 From: TheNexusAvenger <13441476+TheNexusAvenger@users.noreply.github.com> Date: Tue, 1 Mar 2022 16:42:33 -0500 Subject: [PATCH 07/22] Add polygon optimization. --- Uchu.NavMesh.Test/Graph/OrderedPolygonTest.cs | 43 +++++++++++++++++++ Uchu.NavMesh/Graph/OrderedPolygon.cs | 28 ++++++++++++ 2 files changed, 71 insertions(+) create mode 100644 Uchu.NavMesh.Test/Graph/OrderedPolygonTest.cs diff --git a/Uchu.NavMesh.Test/Graph/OrderedPolygonTest.cs b/Uchu.NavMesh.Test/Graph/OrderedPolygonTest.cs new file mode 100644 index 00000000..6bc4b7f3 --- /dev/null +++ b/Uchu.NavMesh.Test/Graph/OrderedPolygonTest.cs @@ -0,0 +1,43 @@ +using System.Collections.Generic; +using System.Numerics; +using NUnit.Framework; +using Uchu.NavMesh.Graph; + +namespace Uchu.NavMesh.Test.Graph; + +public class OrderedPolygonTest +{ + /// + /// Tests the Optimize method. + /// + [Test] + public void TestOptimize() + { + var polygon = new OrderedPolygon() + { + Points = new List() + { + new Vector2(0, 0), + new Vector2(0, 1), + new Vector2(0, 2), + new Vector2(0, 3), + new Vector2(1, 4), + new Vector2(1, 3), + new Vector2(1, 2), + new Vector2(1, 1), + new Vector2(1, 0), + new Vector2(1, -1), + new Vector2(0, -1), + }, + }; + + polygon.Optimize(); + Assert.AreEqual(new List() + { + new Vector2(0, 3), + new Vector2(1, 4), + new Vector2(1, -1), + new Vector2(0, -1), + }, polygon.Points); + } +} \ No newline at end of file diff --git a/Uchu.NavMesh/Graph/OrderedPolygon.cs b/Uchu.NavMesh/Graph/OrderedPolygon.cs index 54d0f084..f5729c8b 100644 --- a/Uchu.NavMesh/Graph/OrderedPolygon.cs +++ b/Uchu.NavMesh/Graph/OrderedPolygon.cs @@ -8,4 +8,32 @@ public class OrderedPolygon /// Points of the ordered polygon. /// public List Points { get; set; } = new List(); + + /// + /// Optimizes the polygon by removing points to make longer lines. + /// + public void Optimize() + { + // Remove points that are in the middle of straight lines. + var currentIndex = 0; + while (currentIndex < Points.Count - 2) + { + var point = this.Points[currentIndex]; + var remainingPoints = this.Points.Count; + for (var i = currentIndex + 1; i < remainingPoints - 2; i++) + { + var middlePoint = this.Points[currentIndex + 1]; + var endPoint = this.Points[currentIndex + 2]; + if (Math.Abs(Math.Atan2(endPoint.Y - middlePoint.Y, endPoint.X - middlePoint.X) - Math.Atan2(middlePoint.Y - point.Y, middlePoint.X - point.X)) > 0.01) break; + this.Points.Remove(middlePoint); + } + currentIndex += 1; + } + + // Remove the last point if the last and first line are collinear. + if (Math.Abs(Math.Atan2(this.Points[^1].Y - this.Points[0].Y, this.Points[^1].X - this.Points[0].X) - Math.Atan2(this.Points[0].Y - this.Points[1].Y, this.Points[0].X - this.Points[1].X)) < 0.01) + { + this.Points.RemoveAt(0); + } + } } \ No newline at end of file From 6818ffa9ed1ae8807774e87f58520f88dbec4c6c Mon Sep 17 00:00:00 2001 From: TheNexusAvenger <13441476+TheNexusAvenger@users.noreply.github.com> Date: Tue, 1 Mar 2022 19:01:54 -0500 Subject: [PATCH 08/22] Move classes. Change Polygons to Shapes. --- Uchu.NavMesh.Test/Graph/GridPolygonTest.cs | 226 ----------------- .../GridEdgeTest.cs => Grid/EdgeTest.cs} | 10 +- .../GridNodeTest.cs => Grid/NodeTest.cs} | 28 +-- .../OrderedShapeTest.cs} | 12 +- Uchu.NavMesh.Test/Shape/UnorderedShapeTest.cs | 227 ++++++++++++++++++ .../{Graph/GridEdge.cs => Grid/Edge.cs} | 12 +- .../{Graph/GridNode.cs => Grid/Node.cs} | 28 +-- .../OrderedShape.cs} | 8 +- .../UnorderedShape.cs} | 109 ++++----- 9 files changed, 331 insertions(+), 329 deletions(-) delete mode 100644 Uchu.NavMesh.Test/Graph/GridPolygonTest.cs rename Uchu.NavMesh.Test/{Graph/GridEdgeTest.cs => Grid/EdgeTest.cs} (75%) rename Uchu.NavMesh.Test/{Graph/GridNodeTest.cs => Grid/NodeTest.cs} (86%) rename Uchu.NavMesh.Test/{Graph/OrderedPolygonTest.cs => Shape/OrderedShapeTest.cs} (82%) create mode 100644 Uchu.NavMesh.Test/Shape/UnorderedShapeTest.cs rename Uchu.NavMesh/{Graph/GridEdge.cs => Grid/Edge.cs} (87%) rename Uchu.NavMesh/{Graph/GridNode.cs => Grid/Node.cs} (89%) rename Uchu.NavMesh/{Graph/OrderedPolygon.cs => Shape/OrderedShape.cs} (88%) rename Uchu.NavMesh/{Graph/GridPolygon.cs => Shape/UnorderedShape.cs} (55%) diff --git a/Uchu.NavMesh.Test/Graph/GridPolygonTest.cs b/Uchu.NavMesh.Test/Graph/GridPolygonTest.cs deleted file mode 100644 index 49b509a0..00000000 --- a/Uchu.NavMesh.Test/Graph/GridPolygonTest.cs +++ /dev/null @@ -1,226 +0,0 @@ -using System.Collections.Generic; -using System.Linq; -using System.Numerics; -using NUnit.Framework; -using Uchu.NavMesh.Graph; - -namespace Uchu.NavMesh.Test.Graph; - -public class GridPolygonTest -{ - /// - /// Tests the FromNodes method with connected squares. - /// - [Test] - public void TestFromNodesSquare() - { - // Test with unfilled square. - var node1 = new GridNode(new Vector3(0, 0, 0)); - var node2 = new GridNode(new Vector3(0, 0, 1)); - var node3 = new GridNode(new Vector3(1, 0, 0)); - var node4 = new GridNode(new Vector3(1, 0, 1)); - node1.Neighbors = new List() - { - node2, - node3, - }; - node2.Neighbors = new List() - { - node1, - node4, - }; - node3.Neighbors = new List() - { - node1, - node4, - }; - node4.Neighbors = new List() - { - node2, - node3, - }; - Assert.IsNull(GridPolygon.FromNodes(node1, node2, node3, node4)); - - // Test with a filled square. - node2.Neighbors.Add(node3); - Assert.AreEqual(4, GridPolygon.FromNodes(node1, node2, node3, node4)!.Edges.Count); - } - - /// - /// Tests the FromNodes method with connected triangles. - /// - [Test] - public void TestFromNodesTriangles() - { - var node1 = new GridNode(new Vector3(0, 0, 0)); - var node2 = new GridNode(new Vector3(0, 0, 1)); - var node3 = new GridNode(new Vector3(1, 0, 0)); - var node4 = new GridNode(new Vector3(1, 0, 1)); - node1.Neighbors = new List() - { - node2, - node3, - }; - node2.Neighbors = new List() - { - node1, - node3, - }; - node3.Neighbors = new List() - { - node1, - node2, - }; - - Assert.AreEqual(3, GridPolygon.FromNodes(node1, node2, node3, node4)!.Edges.Count); - Assert.AreEqual(3, GridPolygon.FromNodes(node2, node3, node4, node1)!.Edges.Count); - Assert.AreEqual(3, GridPolygon.FromNodes(node3, node4, node1, node2)!.Edges.Count); - Assert.AreEqual(3, GridPolygon.FromNodes(node4, node1, node2, node3)!.Edges.Count); - } - - /// - /// Tests the CanMerge method. - /// - [Test] - public void TestCanMerge() - { - // Test CanMerge on the same polygon. - var polygon1 = new GridPolygon() - { - Edges = new HashSet() - { - new GridEdge(new Vector3(0, 0, 0), new Vector3(1, 0, 1)), - new GridEdge(new Vector3(1, 0, 1), new Vector3(0, 0, 1)), - new GridEdge(new Vector3(0, 0, 1), new Vector3(0, 0, 0)), - } - }; - Assert.IsFalse(polygon1.CanMerge(polygon1)); - - // Test CanMerge with a common side. - var polygon2 = new GridPolygon() - { - Edges = new HashSet() - { - new GridEdge(new Vector3(1, 0, 0), new Vector3(1, 0, 1)), - new GridEdge(new Vector3(1, 0, 1), new Vector3(0, 0, 1)), - new GridEdge(new Vector3(0, 0, 1), new Vector3(1, 0, 0)), - } - }; - Assert.IsTrue(polygon1.CanMerge(polygon2)); - - // Test CanMerge with no common side. - var polygon3 = new GridPolygon() - { - Edges = new HashSet() - { - new GridEdge(new Vector3(0, 0, 0), new Vector3(2, 0, 2)), - new GridEdge(new Vector3(2, 0, 2), new Vector3(0, 0, 2)), - new GridEdge(new Vector3(0, 0, 2), new Vector3(0, 0, 0)), - } - }; - Assert.IsFalse(polygon1.CanMerge(polygon3)); - } - - /// - /// Tests the Merge method. - /// - [Test] - public void TestMerge() - { - var polygon1 = new GridPolygon() - { - Edges = new HashSet() - { - new GridEdge(new Vector3(0, 0, 0), new Vector3(1, 0, 1)), - new GridEdge(new Vector3(1, 0, 1), new Vector3(0, 0, 1)), - new GridEdge(new Vector3(0, 0, 1), new Vector3(0, 0, 0)), - } - }; - var polygon2 = new GridPolygon() - { - Edges = new HashSet() - { - new GridEdge(new Vector3(1, 0, 0), new Vector3(1, 0, 1)), - new GridEdge(new Vector3(1, 0, 1), new Vector3(0, 0, 1)), - new GridEdge(new Vector3(0, 0, 1), new Vector3(1, 0, 0)), - } - }; - var originalEdges1 = polygon1.Edges.ToList(); - var originalEdges2 = polygon2.Edges.ToList(); - polygon1.Merge(polygon2); - - var edges = polygon1.Edges.ToList(); - Assert.IsTrue(edges.Contains(originalEdges1[0])); - Assert.IsFalse(edges.Contains(originalEdges1[1])); - Assert.IsTrue(edges.Contains(originalEdges1[2])); - Assert.IsTrue(edges.Contains(originalEdges2[0])); - Assert.IsFalse(edges.Contains(originalEdges2[1])); - Assert.IsTrue(edges.Contains(originalEdges2[2])); - } - - /// - /// Tests the GetOrderedPolygonsSingleShape method with a single shape. - /// - [Test] - public void TestGetOrderedPolygonsSingleShape() - { - var polygon = new GridPolygon() - { - Edges = new HashSet() - { - new GridEdge(new Vector3(0, 0, 0), new Vector3(1, 0, 0)), - new GridEdge(new Vector3(1, 0, 1), new Vector3(0, 0, 1)), - new GridEdge(new Vector3(0, 0, 1), new Vector3(0, 0, 0)), - new GridEdge(new Vector3(1, 0, 1), new Vector3(1, 0, 0)), - } - }; - - var orderedPolygons = polygon.GetOrderedPolygons(); - Assert.AreEqual(new List() - { - new Vector2(0, 0), - new Vector2(1, 0), - new Vector2(1, 1), - new Vector2(0, 1), - }, orderedPolygons[0].Points); - } - - /// - /// Tests the GetOrderedPolygonsSingleShape method with a two shapes. - /// - [Test] - public void TestGetOrderedPolygonsTwoShapes() - { - var polygon = new GridPolygon() - { - Edges = new HashSet() - { - // Shape 1. - new GridEdge(new Vector3(0, 0, 0), new Vector3(1, 0, 0)), - new GridEdge(new Vector3(1, 0, 1), new Vector3(0, 0, 1)), - new GridEdge(new Vector3(0, 0, 1), new Vector3(0, 0, 0)), - new GridEdge(new Vector3(1, 0, 1), new Vector3(1, 0, 0)), - - // Shape 2. - new GridEdge(new Vector3(0, 0, 0), new Vector3(-1, 0, 0)), - new GridEdge(new Vector3(-1, 0, 0), new Vector3(-1, 0, -1)), - new GridEdge(new Vector3(-1, 0, -1), new Vector3(0, 0, 0)), - } - }; - - var orderedPolygons = polygon.GetOrderedPolygons(); - Assert.AreEqual(new List() - { - new Vector2(0, 0), - new Vector2(1, 0), - new Vector2(1, 1), - new Vector2(0, 1), - }, orderedPolygons[0].Points); - Assert.AreEqual(new List() - { - new Vector2(0, 0), - new Vector2(-1, 0), - new Vector2(-1, -1), - }, orderedPolygons[1].Points); - } -} \ No newline at end of file diff --git a/Uchu.NavMesh.Test/Graph/GridEdgeTest.cs b/Uchu.NavMesh.Test/Grid/EdgeTest.cs similarity index 75% rename from Uchu.NavMesh.Test/Graph/GridEdgeTest.cs rename to Uchu.NavMesh.Test/Grid/EdgeTest.cs index 50f64aad..49f0bc5e 100644 --- a/Uchu.NavMesh.Test/Graph/GridEdgeTest.cs +++ b/Uchu.NavMesh.Test/Grid/EdgeTest.cs @@ -1,20 +1,20 @@ using System.Numerics; using NUnit.Framework; -using Uchu.NavMesh.Graph; +using Uchu.NavMesh.Grid; -namespace Uchu.NavMesh.Test.Graph; +namespace Uchu.NavMesh.Test.Grid; -public class GridEdgeTest +public class EdgeTest { /// /// First test edge used. /// - public GridEdge TestEdge1 = new GridEdge(Vector3.One, Vector3.Zero); + public Edge TestEdge1 = new Edge(Vector3.One, Vector3.Zero); /// /// Second test edge used. /// - public GridEdge TestEdge2 = new GridEdge(Vector3.Zero, Vector3.One); + public Edge TestEdge2 = new Edge(Vector3.Zero, Vector3.One); /// /// Tests the Equals method. diff --git a/Uchu.NavMesh.Test/Graph/GridNodeTest.cs b/Uchu.NavMesh.Test/Grid/NodeTest.cs similarity index 86% rename from Uchu.NavMesh.Test/Graph/GridNodeTest.cs rename to Uchu.NavMesh.Test/Grid/NodeTest.cs index 6132e29a..989d5328 100644 --- a/Uchu.NavMesh.Test/Graph/GridNodeTest.cs +++ b/Uchu.NavMesh.Test/Grid/NodeTest.cs @@ -2,16 +2,16 @@ using System.Linq; using System.Numerics; using NUnit.Framework; -using Uchu.NavMesh.Graph; +using Uchu.NavMesh.Grid; -namespace Uchu.NavMesh.Test.Graph; +namespace Uchu.NavMesh.Test.Grid; public class GridNodeTest { /// /// Test node used with most of the tests. /// - public GridNode TestNode { get; set; } + public Node TestNode { get; set; } /// /// Sets up the test node. @@ -20,15 +20,15 @@ public class GridNodeTest public void SetUp() { // Create the test node. - this.TestNode = new GridNode(new Vector3(2, 0, 2)) + this.TestNode = new Node(new Vector3(2, 0, 2)) { - Neighbors = new List() + Neighbors = new List() { - new GridNode(new Vector3(2, 4, 3)), - new GridNode(new Vector3(3, -2, 3)), - new GridNode(new Vector3(3, 0, 2)), - new GridNode(new Vector3(1, 1, 1)), - new GridNode(new Vector3(1, 1, 2)), + new Node(new Vector3(2, 4, 3)), + new Node(new Vector3(3, -2, 3)), + new Node(new Vector3(3, 0, 2)), + new Node(new Vector3(1, 1, 1)), + new Node(new Vector3(1, 1, 2)), } }; @@ -121,9 +121,9 @@ public void TestSplitNodeOneShapeOneExtraEdge() [Test] public void TestSplitNodeAllEdges() { - this.TestNode.Neighbors.Add(new GridNode(new Vector3(1, 0, 3))); - this.TestNode.Neighbors.Add(new GridNode(new Vector3(2, 0, 1))); - this.TestNode.Neighbors.Add(new GridNode(new Vector3(3, 0, 1))); + this.TestNode.Neighbors.Add(new Node(new Vector3(1, 0, 3))); + this.TestNode.Neighbors.Add(new Node(new Vector3(2, 0, 1))); + this.TestNode.Neighbors.Add(new Node(new Vector3(3, 0, 1))); Assert.AreEqual(this.TestNode, this.TestNode.SplitNode()[0]); Assert.AreEqual(this.TestNode.Neighbors.Count, 8); } @@ -134,7 +134,7 @@ public void TestSplitNodeAllEdges() [Test] public void TestGetOuterNeighbors() { - Assert.AreEqual(new List() + Assert.AreEqual(new List() { this.TestNode.Neighbors[0], this.TestNode.Neighbors[2], diff --git a/Uchu.NavMesh.Test/Graph/OrderedPolygonTest.cs b/Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs similarity index 82% rename from Uchu.NavMesh.Test/Graph/OrderedPolygonTest.cs rename to Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs index 6bc4b7f3..d0dcb099 100644 --- a/Uchu.NavMesh.Test/Graph/OrderedPolygonTest.cs +++ b/Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs @@ -1,11 +1,11 @@ using System.Collections.Generic; using System.Numerics; using NUnit.Framework; -using Uchu.NavMesh.Graph; +using Uchu.NavMesh.Shape; -namespace Uchu.NavMesh.Test.Graph; +namespace Uchu.NavMesh.Test.Shape; -public class OrderedPolygonTest +public class OrderedShapeTest { /// /// Tests the Optimize method. @@ -13,7 +13,7 @@ public class OrderedPolygonTest [Test] public void TestOptimize() { - var polygon = new OrderedPolygon() + var shape = new OrderedShape() { Points = new List() { @@ -31,13 +31,13 @@ public void TestOptimize() }, }; - polygon.Optimize(); + shape.Optimize(); Assert.AreEqual(new List() { new Vector2(0, 3), new Vector2(1, 4), new Vector2(1, -1), new Vector2(0, -1), - }, polygon.Points); + }, shape.Points); } } \ No newline at end of file diff --git a/Uchu.NavMesh.Test/Shape/UnorderedShapeTest.cs b/Uchu.NavMesh.Test/Shape/UnorderedShapeTest.cs new file mode 100644 index 00000000..a12975ec --- /dev/null +++ b/Uchu.NavMesh.Test/Shape/UnorderedShapeTest.cs @@ -0,0 +1,227 @@ +using System.Collections.Generic; +using System.Linq; +using System.Numerics; +using NUnit.Framework; +using Uchu.NavMesh.Grid; +using Uchu.NavMesh.Shape; + +namespace Uchu.NavMesh.Test.Shape; + +public class UnorderedShapeTest +{ + /// + /// Tests the FromNodes method with connected squares. + /// + [Test] + public void TestFromNodesSquare() + { + // Test with unfilled square. + var node1 = new Node(new Vector3(0, 0, 0)); + var node2 = new Node(new Vector3(0, 0, 1)); + var node3 = new Node(new Vector3(1, 0, 0)); + var node4 = new Node(new Vector3(1, 0, 1)); + node1.Neighbors = new List() + { + node2, + node3, + }; + node2.Neighbors = new List() + { + node1, + node4, + }; + node3.Neighbors = new List() + { + node1, + node4, + }; + node4.Neighbors = new List() + { + node2, + node3, + }; + Assert.IsNull(UnorderedShape.FromNodes(node1, node2, node3, node4)); + + // Test with a filled square. + node2.Neighbors.Add(node3); + Assert.AreEqual(4, UnorderedShape.FromNodes(node1, node2, node3, node4)!.Edges.Count); + } + + /// + /// Tests the FromNodes method with connected triangles. + /// + [Test] + public void TestFromNodesTriangles() + { + var node1 = new Node(new Vector3(0, 0, 0)); + var node2 = new Node(new Vector3(0, 0, 1)); + var node3 = new Node(new Vector3(1, 0, 0)); + var node4 = new Node(new Vector3(1, 0, 1)); + node1.Neighbors = new List() + { + node2, + node3, + }; + node2.Neighbors = new List() + { + node1, + node3, + }; + node3.Neighbors = new List() + { + node1, + node2, + }; + + Assert.AreEqual(3, UnorderedShape.FromNodes(node1, node2, node3, node4)!.Edges.Count); + Assert.AreEqual(3, UnorderedShape.FromNodes(node2, node3, node4, node1)!.Edges.Count); + Assert.AreEqual(3, UnorderedShape.FromNodes(node3, node4, node1, node2)!.Edges.Count); + Assert.AreEqual(3, UnorderedShape.FromNodes(node4, node1, node2, node3)!.Edges.Count); + } + + /// + /// Tests the CanMerge method. + /// + [Test] + public void TestCanMerge() + { + // Test CanMerge on the same shape. + var shape1 = new UnorderedShape() + { + Edges = new HashSet() + { + new Edge(new Vector3(0, 0, 0), new Vector3(1, 0, 1)), + new Edge(new Vector3(1, 0, 1), new Vector3(0, 0, 1)), + new Edge(new Vector3(0, 0, 1), new Vector3(0, 0, 0)), + } + }; + Assert.IsFalse(shape1.CanMerge(shape1)); + + // Test CanMerge with a common side. + var shape2 = new UnorderedShape() + { + Edges = new HashSet() + { + new Edge(new Vector3(1, 0, 0), new Vector3(1, 0, 1)), + new Edge(new Vector3(1, 0, 1), new Vector3(0, 0, 1)), + new Edge(new Vector3(0, 0, 1), new Vector3(1, 0, 0)), + } + }; + Assert.IsTrue(shape1.CanMerge(shape2)); + + // Test CanMerge with no common side. + var shape3 = new UnorderedShape() + { + Edges = new HashSet() + { + new Edge(new Vector3(0, 0, 0), new Vector3(2, 0, 2)), + new Edge(new Vector3(2, 0, 2), new Vector3(0, 0, 2)), + new Edge(new Vector3(0, 0, 2), new Vector3(0, 0, 0)), + } + }; + Assert.IsFalse(shape1.CanMerge(shape3)); + } + + /// + /// Tests the Merge method. + /// + [Test] + public void TestMerge() + { + var shape1 = new UnorderedShape() + { + Edges = new HashSet() + { + new Edge(new Vector3(0, 0, 0), new Vector3(1, 0, 1)), + new Edge(new Vector3(1, 0, 1), new Vector3(0, 0, 1)), + new Edge(new Vector3(0, 0, 1), new Vector3(0, 0, 0)), + } + }; + var shape2 = new UnorderedShape() + { + Edges = new HashSet() + { + new Edge(new Vector3(1, 0, 0), new Vector3(1, 0, 1)), + new Edge(new Vector3(1, 0, 1), new Vector3(0, 0, 1)), + new Edge(new Vector3(0, 0, 1), new Vector3(1, 0, 0)), + } + }; + var originalEdges1 = shape1.Edges.ToList(); + var originalEdges2 = shape2.Edges.ToList(); + shape1.Merge(shape2); + + var edges = shape1.Edges.ToList(); + Assert.IsTrue(edges.Contains(originalEdges1[0])); + Assert.IsFalse(edges.Contains(originalEdges1[1])); + Assert.IsTrue(edges.Contains(originalEdges1[2])); + Assert.IsTrue(edges.Contains(originalEdges2[0])); + Assert.IsFalse(edges.Contains(originalEdges2[1])); + Assert.IsTrue(edges.Contains(originalEdges2[2])); + } + + /// + /// Tests the GetOrderedShapes method with a single shape. + /// + [Test] + public void TestGetOrderedShapesSingleShape() + { + var shape = new UnorderedShape() + { + Edges = new HashSet() + { + new Edge(new Vector3(0, 0, 0), new Vector3(1, 0, 0)), + new Edge(new Vector3(1, 0, 1), new Vector3(0, 0, 1)), + new Edge(new Vector3(0, 0, 1), new Vector3(0, 0, 0)), + new Edge(new Vector3(1, 0, 1), new Vector3(1, 0, 0)), + } + }; + + var orderedShapes = shape.GetOrderedShapes(); + Assert.AreEqual(new List() + { + new Vector2(0, 0), + new Vector2(1, 0), + new Vector2(1, 1), + new Vector2(0, 1), + }, orderedShapes[0].Points); + } + + /// + /// Tests the GetOrderedShapes method with a two shapes. + /// + [Test] + public void TestGetOrderedShapesTwoShapes() + { + var shape = new UnorderedShape() + { + Edges = new HashSet() + { + // Shape 1. + new Edge(new Vector3(0, 0, 0), new Vector3(1, 0, 0)), + new Edge(new Vector3(1, 0, 1), new Vector3(0, 0, 1)), + new Edge(new Vector3(0, 0, 1), new Vector3(0, 0, 0)), + new Edge(new Vector3(1, 0, 1), new Vector3(1, 0, 0)), + + // Shape 2. + new Edge(new Vector3(0, 0, 0), new Vector3(-1, 0, 0)), + new Edge(new Vector3(-1, 0, 0), new Vector3(-1, 0, -1)), + new Edge(new Vector3(-1, 0, -1), new Vector3(0, 0, 0)), + } + }; + + var orderedShapes = shape.GetOrderedShapes(); + Assert.AreEqual(new List() + { + new Vector2(0, 0), + new Vector2(1, 0), + new Vector2(1, 1), + new Vector2(0, 1), + }, orderedShapes[0].Points); + Assert.AreEqual(new List() + { + new Vector2(0, 0), + new Vector2(-1, 0), + new Vector2(-1, -1), + }, orderedShapes[1].Points); + } +} \ No newline at end of file diff --git a/Uchu.NavMesh/Graph/GridEdge.cs b/Uchu.NavMesh/Grid/Edge.cs similarity index 87% rename from Uchu.NavMesh/Graph/GridEdge.cs rename to Uchu.NavMesh/Grid/Edge.cs index 1a27724a..5a8b73ea 100644 --- a/Uchu.NavMesh/Graph/GridEdge.cs +++ b/Uchu.NavMesh/Grid/Edge.cs @@ -1,8 +1,8 @@ using System.Numerics; -namespace Uchu.NavMesh.Graph; +namespace Uchu.NavMesh.Grid; -public class GridEdge : IEquatable +public class Edge : IEquatable { /// /// Start of the edge. @@ -15,11 +15,11 @@ public class GridEdge : IEquatable public readonly Vector3 End; /// - /// Creates the grid edge. + /// Creates the edge. /// /// Start of the edge. /// End of the edge. - public GridEdge(Vector3 start, Vector3 end) + public Edge(Vector3 start, Vector3 end) { this.Start = start; this.End = end; @@ -30,7 +30,7 @@ public GridEdge(Vector3 start, Vector3 end) /// /// The other edge to compare. /// If the edges are equal. - public bool Equals(GridEdge? other) + public bool Equals(Edge? other) { if (ReferenceEquals(null, other)) return false; if (ReferenceEquals(this, other)) return true; @@ -47,7 +47,7 @@ public override bool Equals(object? obj) if (ReferenceEquals(null, obj)) return false; if (ReferenceEquals(this, obj)) return true; if (obj.GetType() != this.GetType()) return false; - return Equals((GridEdge) obj); + return Equals((Edge) obj); } /// diff --git a/Uchu.NavMesh/Graph/GridNode.cs b/Uchu.NavMesh/Grid/Node.cs similarity index 89% rename from Uchu.NavMesh/Graph/GridNode.cs rename to Uchu.NavMesh/Grid/Node.cs index 6b3f1813..2db120f3 100644 --- a/Uchu.NavMesh/Graph/GridNode.cs +++ b/Uchu.NavMesh/Grid/Node.cs @@ -1,8 +1,8 @@ using System.Numerics; -namespace Uchu.NavMesh.Graph; +namespace Uchu.NavMesh.Grid; -public class GridNode +public class Node { /// /// Position of the grid node. @@ -12,13 +12,13 @@ public class GridNode /// /// Neighbors of the node. /// - public List Neighbors { get; set; } = new List(8); + public List Neighbors { get; set; } = new List(8); /// /// Creates the node. /// /// Position of the node. - public GridNode(Vector3 position) + public Node(Vector3 position) { this.Position = position; } @@ -51,7 +51,7 @@ public byte RotationTo(Vector3 target) /// /// Rotation multiplier to use. /// Node at the given rotation. - public GridNode? GetNodeByRotation(byte rotation) + public Node? GetNodeByRotation(byte rotation) { return this.Neighbors.FirstOrDefault(node => this.RotationTo(node.Position) == rotation); } @@ -60,9 +60,9 @@ public byte RotationTo(Vector3 target) /// Returns the neighbors where the index is the rotation multiple of 45 degrees. /// /// The neighbors where the index is the rotation multiple of 45 degrees. - private GridNode?[] GetNodesAtRotation() + private Node?[] GetNodesAtRotation() { - var nodesAtAngles = new GridNode?[8]; + var nodesAtAngles = new Node?[8]; for (byte i = 0; i < 8; i++) { var node = this.GetNodeByRotation(i); @@ -76,7 +76,7 @@ public byte RotationTo(Vector3 target) /// Splits the node so that no 2 shapes share the same node instance. /// /// The nodes that were created. - public List SplitNode() + public List SplitNode() { // Get the nodes for each angle. var nodesAtAngles = this.GetNodesAtRotation(); @@ -90,7 +90,7 @@ public List SplitNode() // Return if all 8 angles are covered, or there are no nodes to operate on. if (totalNodes == 0 || totalNodes == 8) { - return new List(1) { this }; + return new List(1) { this }; } // Determine a starting offset where the first index does not need to be replaced. @@ -103,8 +103,8 @@ public List SplitNode() } // Replaces the node references of the neighbors. - var createdNodes = new List(3); - var newNodesForNeighbor = new GridNode?[8]; + var createdNodes = new List(3); + var newNodesForNeighbor = new Node?[8]; for (var i = 0; i < 8; i++) { // Get the current neighbor and continue if there is no neighbor to act on. @@ -127,7 +127,7 @@ public List SplitNode() var newNode = newNodesForNeighbor[previousIndex]; if (newNode == null) { - newNode = new GridNode(this.Position); + newNode = new Node(this.Position); createdNodes.Add(newNode); } newNodesForNeighbor[currentIndex] = newNode; @@ -145,11 +145,11 @@ public List SplitNode() /// Returns the neighbors that do not have a neighbor on the left or right. /// /// the neighbors that do not have a neighbor on the left or right. - public List GetOuterNeighbors() + public List GetOuterNeighbors() { // Get the neighbors that only have a left or right neighbor (not both). var nodesAtAngles = this.GetNodesAtRotation(); - var neighbors = new List(4); + var neighbors = new List(4); for (var i = 0; i < 8; i++) { // Get the current neighbor and continue if there is no neighbor to act on. diff --git a/Uchu.NavMesh/Graph/OrderedPolygon.cs b/Uchu.NavMesh/Shape/OrderedShape.cs similarity index 88% rename from Uchu.NavMesh/Graph/OrderedPolygon.cs rename to Uchu.NavMesh/Shape/OrderedShape.cs index f5729c8b..026e2ba3 100644 --- a/Uchu.NavMesh/Graph/OrderedPolygon.cs +++ b/Uchu.NavMesh/Shape/OrderedShape.cs @@ -1,16 +1,16 @@ using System.Numerics; -namespace Uchu.NavMesh.Graph; +namespace Uchu.NavMesh.Shape; -public class OrderedPolygon +public class OrderedShape { /// - /// Points of the ordered polygon. + /// Points of the ordered shape. /// public List Points { get; set; } = new List(); /// - /// Optimizes the polygon by removing points to make longer lines. + /// Optimizes the shape by removing points to make longer lines. /// public void Optimize() { diff --git a/Uchu.NavMesh/Graph/GridPolygon.cs b/Uchu.NavMesh/Shape/UnorderedShape.cs similarity index 55% rename from Uchu.NavMesh/Graph/GridPolygon.cs rename to Uchu.NavMesh/Shape/UnorderedShape.cs index 29e234c7..a41414b5 100644 --- a/Uchu.NavMesh/Graph/GridPolygon.cs +++ b/Uchu.NavMesh/Shape/UnorderedShape.cs @@ -1,88 +1,89 @@ using System.Numerics; +using Uchu.NavMesh.Grid; -namespace Uchu.NavMesh.Graph; +namespace Uchu.NavMesh.Shape; -public class GridPolygon +public class UnorderedShape { /// - /// Edges of the polygon. + /// Edges of the shape. /// - public HashSet Edges { get; set; } = new HashSet(); + public HashSet Edges { get; set; } = new HashSet(); /// - /// Returns a polygon from a set of nodes. + /// Returns a shape from a set of nodes. /// - /// Corner 1 of the polygon. - /// Corner 2 of the polygon. - /// Corner 3 of the polygon. - /// Corner 4 of the polygon. - /// The created polygon. - public static GridPolygon? FromNodes(GridNode node1, GridNode node2, GridNode node3, GridNode node4) + /// Corner 1 of the shape. + /// Corner 2 of the shape. + /// Corner 3 of the shape. + /// Corner 4 of the shape. + /// The created shape. + public static UnorderedShape? FromNodes(Node node1, Node node2, Node node3, Node node4) { - // Return either a polygon of the square or null if the square is not filled. + // Return either a shape of the square or null if the square is not filled. if (node1.Neighbors.Contains(node2) && node1.Neighbors.Contains(node3) && node4.Neighbors.Contains(node2) && node4.Neighbors.Contains(node3)) { if (node1.Neighbors.Contains(node4) || node2.Neighbors.Contains(node3)) { - return new GridPolygon() + return new UnorderedShape() { Edges = { - new GridEdge(node1.Position, node2.Position), - new GridEdge(node1.Position, node3.Position), - new GridEdge(node4.Position, node2.Position), - new GridEdge(node4.Position, node3.Position), + new Edge(node1.Position, node2.Position), + new Edge(node1.Position, node3.Position), + new Edge(node4.Position, node2.Position), + new Edge(node4.Position, node3.Position), }, }; } return null; } - // Return a triangle polygon. + // Return a triangle shape. if (node1.Neighbors.Contains(node2) && node2.Neighbors.Contains(node3) && node3.Neighbors.Contains(node1)) { - // Return a polygon without node 4. - return new GridPolygon() + // Return a shape without node 4. + return new UnorderedShape() { Edges = { - new GridEdge(node1.Position, node2.Position), - new GridEdge(node2.Position, node3.Position), - new GridEdge(node3.Position, node1.Position), + new Edge(node1.Position, node2.Position), + new Edge(node2.Position, node3.Position), + new Edge(node3.Position, node1.Position), }, }; } if (node2.Neighbors.Contains(node3) && node3.Neighbors.Contains(node4) && node4.Neighbors.Contains(node2)) { - // Return a polygon without node 1. - return new GridPolygon() + // Return a shape without node 1. + return new UnorderedShape() { Edges = { - new GridEdge(node2.Position, node3.Position), - new GridEdge(node3.Position, node4.Position), - new GridEdge(node4.Position, node2.Position), + new Edge(node2.Position, node3.Position), + new Edge(node3.Position, node4.Position), + new Edge(node4.Position, node2.Position), }, }; } if (node1.Neighbors.Contains(node3) && node3.Neighbors.Contains(node4) && node4.Neighbors.Contains(node1)) { - // Return a polygon without node 2. - return new GridPolygon() + // Return a shape without node 2. + return new UnorderedShape() { Edges = { - new GridEdge(node1.Position, node3.Position), - new GridEdge(node3.Position, node4.Position), - new GridEdge(node4.Position, node1.Position), + new Edge(node1.Position, node3.Position), + new Edge(node3.Position, node4.Position), + new Edge(node4.Position, node1.Position), }, }; } if (node1.Neighbors.Contains(node2) && node2.Neighbors.Contains(node4) && node4.Neighbors.Contains(node1)) { - // Return a polygon without node 3. - return new GridPolygon() + // Return a shape without node 3. + return new UnorderedShape() { Edges = { - new GridEdge(node1.Position, node2.Position), - new GridEdge(node2.Position, node4.Position), - new GridEdge(node4.Position, node1.Position), + new Edge(node1.Position, node2.Position), + new Edge(node2.Position, node4.Position), + new Edge(node4.Position, node1.Position), }, }; } @@ -92,23 +93,23 @@ public class GridPolygon } /// - /// Returns if a polygon can merge. + /// Returns if a shape can merge. /// - /// Polygon to check merging. + /// Shape to check merging. /// Whether the merge can be done. - public bool CanMerge(GridPolygon polygon) + public bool CanMerge(UnorderedShape shape) { - if (polygon == this) return false; - return (from edge in this.Edges from otherEdge in polygon.Edges where edge.Equals(otherEdge) select edge).Any(); + if (shape == this) return false; + return (from edge in this.Edges from otherEdge in shape.Edges where edge.Equals(otherEdge) select edge).Any(); } /// - /// Merges another polygon. + /// Merges another shape. /// - /// Polygon to merge. - public void Merge(GridPolygon polygon) + /// Shape to merge. + public void Merge(UnorderedShape shape) { - foreach (var edge in polygon.Edges) + foreach (var edge in shape.Edges) { if (this.Edges.Contains(edge)) { @@ -122,13 +123,13 @@ public void Merge(GridPolygon polygon) } /// - /// Returns a list of ordered polygons for the current polygon. + /// Returns a list of ordered shapes for the current shape. /// - /// Ordered polygons from the current edges. - public List GetOrderedPolygons() + /// Ordered shapes from the current edges. + public List GetOrderedShapes() { // Iterate over the edges. - var orderedPolygons = new List(); + var orderedShapes = new List(); var remainingEdges = this.Edges.ToList(); var currentPoints = new List(); while (remainingEdges.Count != 0) @@ -164,7 +165,7 @@ public List GetOrderedPolygons() var point = currentPoints[i]; newPoints.Add(new Vector2(point.X, point.Z)); } - orderedPolygons.Add(new OrderedPolygon() + orderedShapes.Add(new OrderedShape() { Points = newPoints, }); @@ -176,7 +177,7 @@ public List GetOrderedPolygons() } } - // Return the ordered polygons. - return orderedPolygons; + // Return the ordered shapes. + return orderedShapes; } } \ No newline at end of file From 1fe8a8b71bfc34dcd63b9eb8693b5c5941935e73 Mon Sep 17 00:00:00 2001 From: TheNexusAvenger <13441476+TheNexusAvenger@users.noreply.github.com> Date: Tue, 1 Mar 2022 19:05:30 -0500 Subject: [PATCH 09/22] Remove unused methods. --- Uchu.NavMesh.Test/Grid/NodeTest.cs | 145 ---------------------------- Uchu.NavMesh/Grid/Node.cs | 149 ----------------------------- 2 files changed, 294 deletions(-) delete mode 100644 Uchu.NavMesh.Test/Grid/NodeTest.cs diff --git a/Uchu.NavMesh.Test/Grid/NodeTest.cs b/Uchu.NavMesh.Test/Grid/NodeTest.cs deleted file mode 100644 index 989d5328..00000000 --- a/Uchu.NavMesh.Test/Grid/NodeTest.cs +++ /dev/null @@ -1,145 +0,0 @@ -using System.Collections.Generic; -using System.Linq; -using System.Numerics; -using NUnit.Framework; -using Uchu.NavMesh.Grid; - -namespace Uchu.NavMesh.Test.Grid; - -public class GridNodeTest -{ - /// - /// Test node used with most of the tests. - /// - public Node TestNode { get; set; } - - /// - /// Sets up the test node. - /// - [SetUp] - public void SetUp() - { - // Create the test node. - this.TestNode = new Node(new Vector3(2, 0, 2)) - { - Neighbors = new List() - { - new Node(new Vector3(2, 4, 3)), - new Node(new Vector3(3, -2, 3)), - new Node(new Vector3(3, 0, 2)), - new Node(new Vector3(1, 1, 1)), - new Node(new Vector3(1, 1, 2)), - } - }; - - // Set up the neighbors. - foreach (var neighbor in this.TestNode.Neighbors) - { - neighbor.Neighbors.Add(this.TestNode); - } - } - - /// - /// Tests the RotationTo method. - /// - [Test] - public void TestRotationTo() - { - // Test with itself. - Assert.AreEqual(0, this.TestNode.RotationTo(this.TestNode.Position)); - - // Test the eight rotation multipliers. - Assert.AreEqual(0, this.TestNode.RotationTo(new Vector3(2, 0, 3))); - Assert.AreEqual(1, this.TestNode.RotationTo(new Vector3(3, 0, 3))); - Assert.AreEqual(2, this.TestNode.RotationTo(new Vector3(3, 0, 2))); - Assert.AreEqual(3, this.TestNode.RotationTo(new Vector3(3, 0, 1))); - Assert.AreEqual(4, this.TestNode.RotationTo(new Vector3(2, 0, 1))); - Assert.AreEqual(5, this.TestNode.RotationTo(new Vector3(1, 0, 1))); - Assert.AreEqual(6, this.TestNode.RotationTo(new Vector3(1, 0, 2))); - Assert.AreEqual(7, this.TestNode.RotationTo(new Vector3(1, 0, 3))); - } - - /// - /// Tests the GetNodeByRotation method. - /// - [Test] - public void TestGetNodeByRotation() - { - Assert.AreEqual(this.TestNode.Neighbors[0], this.TestNode.GetNodeByRotation(0)); - Assert.AreEqual(this.TestNode.Neighbors[1], this.TestNode.GetNodeByRotation(1)); - Assert.AreEqual(this.TestNode.Neighbors[2], this.TestNode.GetNodeByRotation(2)); - Assert.IsNull(this.TestNode.GetNodeByRotation(3)); - Assert.IsNull(this.TestNode.GetNodeByRotation(4)); - Assert.AreEqual(this.TestNode.Neighbors[3], this.TestNode.GetNodeByRotation(5)); - Assert.AreEqual(this.TestNode.Neighbors[4], this.TestNode.GetNodeByRotation(6)); - Assert.IsNull(this.TestNode.GetNodeByRotation(7)); - } - - /// - /// Tests the SplitNode method with 2 shapes. - /// - [Test] - public void TestSplitNodeTwoShapes() - { - // Split the nodes and make sure 2 were created. - var neighbors = this.TestNode.Neighbors.ToList(); - var createdNodes = this.TestNode.SplitNode(); - Assert.AreEqual(createdNodes.Count, 2); - Assert.AreEqual(this.TestNode.Neighbors.Count, 0); - - // Check the neighbors. - Assert.AreEqual(createdNodes[0], neighbors[0].Neighbors[0]); - Assert.AreEqual(createdNodes[0], neighbors[1].Neighbors[0]); - Assert.AreEqual(createdNodes[0], neighbors[2].Neighbors[0]); - Assert.AreEqual(createdNodes[1], neighbors[3].Neighbors[0]); - Assert.AreEqual(createdNodes[1], neighbors[4].Neighbors[0]); - } - - /// - /// Tests the SplitNode method with 1 shape and 1 extra edge. - /// - [Test] - public void TestSplitNodeOneShapeOneExtraEdge() - { - // Split the nodes and make sure 1 was created. - this.TestNode.Neighbors.RemoveAt(4); - var neighbors = this.TestNode.Neighbors.ToList(); - var createdNodes = this.TestNode.SplitNode(); - Assert.AreEqual(createdNodes.Count, 1); - Assert.AreEqual(this.TestNode.Neighbors.Count, 0); - - // Check the neighbors. - Assert.AreEqual(createdNodes[0], neighbors[0].Neighbors[0]); - Assert.AreEqual(createdNodes[0], neighbors[1].Neighbors[0]); - Assert.AreEqual(createdNodes[0], neighbors[2].Neighbors[0]); - Assert.AreEqual(0, neighbors[3].Neighbors.Count); - } - - /// - /// Tests the SplitNode method with the optimization for 8 all edges. - /// - [Test] - public void TestSplitNodeAllEdges() - { - this.TestNode.Neighbors.Add(new Node(new Vector3(1, 0, 3))); - this.TestNode.Neighbors.Add(new Node(new Vector3(2, 0, 1))); - this.TestNode.Neighbors.Add(new Node(new Vector3(3, 0, 1))); - Assert.AreEqual(this.TestNode, this.TestNode.SplitNode()[0]); - Assert.AreEqual(this.TestNode.Neighbors.Count, 8); - } - - /// - /// Tests the GetOuterNeighbors method. - /// - [Test] - public void TestGetOuterNeighbors() - { - Assert.AreEqual(new List() - { - this.TestNode.Neighbors[0], - this.TestNode.Neighbors[2], - this.TestNode.Neighbors[3], - this.TestNode.Neighbors[4], - }, this.TestNode.GetOuterNeighbors()); - } -} \ No newline at end of file diff --git a/Uchu.NavMesh/Grid/Node.cs b/Uchu.NavMesh/Grid/Node.cs index 2db120f3..1ddc9d17 100644 --- a/Uchu.NavMesh/Grid/Node.cs +++ b/Uchu.NavMesh/Grid/Node.cs @@ -22,153 +22,4 @@ public Node(Vector3 position) { this.Position = position; } - - /// - /// Returns the "rotation" on the XZ plane to a given target. Each increase in 1 represents 45 degrees and is - /// used since the nodes are on a 2D grid with every neighbor being +/-1 on the X and Z on the grid. - /// - /// Position to target. The Y value is ignored. - /// The byte multiplier of the angle. - public byte RotationTo(Vector3 target) - { - // Return 0 if the position is 0 to avoid a divide-by-zero error. - if (this.Position.X == target.X && this.Position.Z == target.Z) - { - return 0; - } - - // Return the angle. - var angle = Math.Atan2(target.X - this.Position.X, target.Z - this.Position.Z); - if (angle < 0) - { - angle += (2 * Math.PI); - } - return (byte) Math.Round(angle / (Math.PI * 0.25)); - } - - /// - /// Returns the neighbor node that is a rotation * 45 degrees from the node. - /// - /// Rotation multiplier to use. - /// Node at the given rotation. - public Node? GetNodeByRotation(byte rotation) - { - return this.Neighbors.FirstOrDefault(node => this.RotationTo(node.Position) == rotation); - } - - /// - /// Returns the neighbors where the index is the rotation multiple of 45 degrees. - /// - /// The neighbors where the index is the rotation multiple of 45 degrees. - private Node?[] GetNodesAtRotation() - { - var nodesAtAngles = new Node?[8]; - for (byte i = 0; i < 8; i++) - { - var node = this.GetNodeByRotation(i); - if (node == null) continue; - nodesAtAngles[i] = node; - } - return nodesAtAngles; - } - - /// - /// Splits the node so that no 2 shapes share the same node instance. - /// - /// The nodes that were created. - public List SplitNode() - { - // Get the nodes for each angle. - var nodesAtAngles = this.GetNodesAtRotation(); - var totalNodes = 0; - for (byte i = 0; i < 8; i++) - { - if (nodesAtAngles[i] == null) continue; - totalNodes += 1; - } - - // Return if all 8 angles are covered, or there are no nodes to operate on. - if (totalNodes == 0 || totalNodes == 8) - { - return new List(1) { this }; - } - - // Determine a starting offset where the first index does not need to be replaced. - // This ensures that the replaced nodes do not start at the middle. - var startOffset = 0; - for (var i = 0; i < 8; i++) - { - if (nodesAtAngles[i] != null) continue; - startOffset = i; - } - - // Replaces the node references of the neighbors. - var createdNodes = new List(3); - var newNodesForNeighbor = new Node?[8]; - for (var i = 0; i < 8; i++) - { - // Get the current neighbor and continue if there is no neighbor to act on. - var currentIndex = (i + startOffset) % 8; - var currentNeighbor = nodesAtAngles[currentIndex]; - if (currentNeighbor == null) continue; - - // Get the previous and next neighbors. - // The previous index calculation uses +7 instead of -1 to ensure a positive result. - var previousIndex = (currentIndex + 7) % 8; - var nextIndex = (currentIndex + 1) % 8; - var previousNeighbor = nodesAtAngles[previousIndex]; - var nextNeighbor = nodesAtAngles[nextIndex]; - - // Remove the edge and continue if there is no previous or next neighbor. - currentNeighbor.Neighbors.Remove(this); - if (previousNeighbor == null && nextNeighbor == null) continue; - - // Get the new node to use. - var newNode = newNodesForNeighbor[previousIndex]; - if (newNode == null) - { - newNode = new Node(this.Position); - createdNodes.Add(newNode); - } - newNodesForNeighbor[currentIndex] = newNode; - - // Replace the neighbor. - currentNeighbor.Neighbors.Add(newNode); - } - - // Return the created nodes. - this.Neighbors.Clear(); - return createdNodes; - } - - /// - /// Returns the neighbors that do not have a neighbor on the left or right. - /// - /// the neighbors that do not have a neighbor on the left or right. - public List GetOuterNeighbors() - { - // Get the neighbors that only have a left or right neighbor (not both). - var nodesAtAngles = this.GetNodesAtRotation(); - var neighbors = new List(4); - for (var i = 0; i < 8; i++) - { - // Get the current neighbor and continue if there is no neighbor to act on. - var currentNeighbor = nodesAtAngles[i]; - if (currentNeighbor == null) continue; - - // Get the previous and next neighbors. - // The previous index calculation uses +7 instead of -1 to ensure a positive result. - var previousIndex = (i + 7) % 8; - var nextIndex = (i + 1) % 8; - var previousNeighbor = nodesAtAngles[previousIndex]; - var nextNeighbor = nodesAtAngles[nextIndex]; - - // Add the neighbor if isn't both a left and right neighbor. - if (previousNeighbor != null && nextNeighbor != null) continue; - neighbors.Add(currentNeighbor); - } - - // Return the neighbors. - return neighbors; - } } \ No newline at end of file From 775f273a5f355222818d9c115694ba6031ac7db3 Mon Sep 17 00:00:00 2001 From: TheNexusAvenger <13441476+TheNexusAvenger@users.noreply.github.com> Date: Tue, 1 Mar 2022 19:56:19 -0500 Subject: [PATCH 10/22] Implement height map to shapes solver. --- Uchu.NavMesh/Grid/HeightMap.cs | 58 +++++++++ Uchu.NavMesh/Shape/Solver.cs | 202 +++++++++++++++++++++++++++++++ Uchu.NavMesh/Uchu.NavMesh.csproj | 4 + 3 files changed, 264 insertions(+) create mode 100644 Uchu.NavMesh/Grid/HeightMap.cs create mode 100644 Uchu.NavMesh/Shape/Solver.cs diff --git a/Uchu.NavMesh/Grid/HeightMap.cs b/Uchu.NavMesh/Grid/HeightMap.cs new file mode 100644 index 00000000..362e3ff3 --- /dev/null +++ b/Uchu.NavMesh/Grid/HeightMap.cs @@ -0,0 +1,58 @@ +using System.Numerics; +using Uchu.World.Client; + +namespace Uchu.NavMesh.Grid; + +public class HeightMap +{ + /// + /// Scale to apply to the positions. + /// + public const float Scale = 3.125f; + + /// + /// Height values of the height map. + /// + public float[,] Heights { get; private set; } + + /// + /// Width of the heightmap. + /// + public int SizeX => Heights.GetLength(0); + + /// + /// Depth of the heightmap. + /// + public int SizeY => Heights.GetLength(1); + + /// + /// Generates the height map for a zone. + /// + /// Zone info to use. + /// The height map for the zone. + public static HeightMap FromZoneInfo(ZoneInfo zoneInfo) + { + // Generate the heightmap. + var terrain = zoneInfo.TerrainFile; + var heightMap = new HeightMap() + { + Heights = terrain.GenerateHeightMap(), + }; + + // Return the heightmap. + return heightMap; + } + + /// + /// Returns the position in the world for a given grid position. + /// + /// X position in the grid. + /// Y position in the grid. + /// Position in the world. + public Vector3 GetPosition(int x, int y) + { + var centerX = (this.Heights.GetLength(0) - 1) / 2; + var centerY = (this.Heights.GetLength(1) - 1) / 2; + return new Vector3((x - centerX) * Scale, this.Heights[x, y], (y - centerY) * Scale); + } +} \ No newline at end of file diff --git a/Uchu.NavMesh/Shape/Solver.cs b/Uchu.NavMesh/Shape/Solver.cs new file mode 100644 index 00000000..aacf73f2 --- /dev/null +++ b/Uchu.NavMesh/Shape/Solver.cs @@ -0,0 +1,202 @@ +using System.Numerics; +using Uchu.NavMesh.Grid; +using Uchu.World.Client; + +namespace Uchu.NavMesh.Shape; + +public class Solver +{ + /// + /// Maximum distance 2 nodes on the heightmap can be before being considered to steep to connect. + /// + public const int MaximumNodeDistance = 6; + + /// + /// Minimum distance the nodes must be from the lowest node to be used for generating the shapes. + /// + public const int MinimumDistanceFromBottom = 5; + + /// + /// Height map of the solver. + /// + public HeightMap HeightMap { get; private set; } + + /// + /// Shapes that define the boundaries in 2D. + /// + public List BoundingShapes { get; private set; } + + /// + /// Initializes the solver. + /// + /// Zone info with a terrain file to read. + public async Task Initialize(ZoneInfo zoneInfo) + { + this.HeightMap = HeightMap.FromZoneInfo(zoneInfo); + // TODO: Consider caching the results somehow. These can take time to generate. + await this.GenerateShapesAsync(); + } + + /// + /// Generates the shapes of the zone. + /// + private async Task GenerateShapesAsync() + { + // Create the nodes. + var minimumHeight = float.MaxValue; + var nodes = new Node[this.HeightMap.SizeX, this.HeightMap.SizeY]; + for (var x = 0; x < this.HeightMap.SizeX; x++) + { + for (var y = 0; y < this.HeightMap.SizeY; y++) + { + // Get the position. + var position = this.HeightMap.GetPosition(x, y); + if (position.Y < minimumHeight) + minimumHeight = position.Y; + + // Add the node. + nodes[x, y] = new Node(position); + } + } + + // Populate the edges. + // This can be done in parallel. + var tasks = new List(); + for (var x = 0; x < this.HeightMap.SizeX; x++) + { + for (var y = 0; y < this.HeightMap.SizeY; y++) + { + var currentX = x; + var currentY = y; + var currentNode = nodes[x, y]; + if (Math.Abs(currentNode.Position.Y - minimumHeight) < MinimumDistanceFromBottom) continue; + tasks.Add(Task.Run(() => + { + for (var offsetX = -1; offsetX <= 1; offsetX++) + { + var otherX = currentX + offsetX; + if (otherX < 0 || otherX >= this.HeightMap.SizeX) continue; + for (var offsetY = -1; offsetY <= 1; offsetY++) + { + if (offsetX == 0 && offsetY == 0) continue; + var otherY = currentY + offsetY; + if (otherY < 0 || otherY >= this.HeightMap.SizeY) continue; + var otherNode = nodes[otherX, otherY]; + if (Vector3.Distance(otherNode.Position, currentNode.Position) > MaximumNodeDistance) continue; + currentNode.Neighbors.Add(otherNode); + } + } + })); + } + } + await Task.WhenAll(tasks); + + // Create the rows of shapes. + var shapeRows = new List[this.HeightMap.SizeX - 1]; + tasks = new List(); + for (var x = 0; x < this.HeightMap.SizeX - 1; x++) + { + var currentX = x; + tasks.Add(Task.Run(() => + { + // Create and merge the shapes for the row. + var rowShapes = new List(); + for (var y = 0; y < this.HeightMap.SizeY - 1; y++) + { + var shape = UnorderedShape.FromNodes(nodes[currentX, y], nodes[currentX + 1, y], nodes[currentX, y + 1], nodes[currentX + 1, y + 1]); + if (shape == null) continue; + + if (rowShapes.Count > 0 && rowShapes[^1].CanMerge(shape)) + { + rowShapes[^1].Merge(shape); + continue; + } + rowShapes.Add(shape); + } + + // Store the row. + lock (shapeRows) + { + shapeRows[currentX] = rowShapes; + } + })); + } + await Task.WhenAll(tasks); + + // Merge the rows. + // This is done by constantly merging pairs of rows in parallel until every row is merged. + while (shapeRows.Length > 1) + { + // Create the list for the merged rows and add the last row if it is odd. + var totalNewShapeRows = (int) Math.Ceiling((shapeRows.Length / 2.0)); + var newShapeRows = new List[totalNewShapeRows]; + if (shapeRows.Length % 2 == 1) + { + newShapeRows[totalNewShapeRows - 1] = shapeRows[^1]; + } + + // Create tasks to merge evert set of 2 rows. + tasks = new List(); + for (var x = 0; x < Math.Floor(shapeRows.Length / 2.0) * 2; x += 2) + { + // Merge the 2 rows. + var shapesToMerge = shapeRows[x].ToList(); + shapesToMerge.AddRange(shapeRows[x + 1]); + var rowShapes = new List(); + newShapeRows[x / 2] = rowShapes; + tasks.Add(Task.Run(() => + { + while (shapesToMerge.Count > 0) { + var changesMade = false; + var shapeToMerge = shapesToMerge[0]; + foreach (var otherShape in shapesToMerge.ToList()) + { + if (!shapeToMerge.CanMerge(otherShape)) continue; + shapeToMerge.Merge(otherShape); + shapesToMerge.Remove(otherShape); + changesMade = true; + } + + if (changesMade) continue; + shapesToMerge.Remove(shapeToMerge); + rowShapes.Add(shapeToMerge); + } + })); + } + + // Wait for the rows to complete and prepare for the next step. + await Task.WhenAll(tasks); + shapeRows = newShapeRows; + } + + // Separate the shapes and make them 2D. + var shapes = new List(); + tasks = new List(); + foreach (var shape in shapeRows[0]) + { + tasks.Add(Task.Run(() => + { + var newShapes = shape.GetOrderedShapes(); + lock (shapes) + { + shapes.AddRange(newShapes); + } + })); + } + await Task.WhenAll(tasks); + + // Optimize the shapes. + tasks = new List(); + foreach (var shape in shapes) + { + tasks.Add(Task.Run(() => + { + shape.Optimize(); + })); + } + await Task.WhenAll(tasks); + + // Store the shapes. + this.BoundingShapes = shapes; + } +} \ No newline at end of file diff --git a/Uchu.NavMesh/Uchu.NavMesh.csproj b/Uchu.NavMesh/Uchu.NavMesh.csproj index eb2460e9..8f2722bf 100644 --- a/Uchu.NavMesh/Uchu.NavMesh.csproj +++ b/Uchu.NavMesh/Uchu.NavMesh.csproj @@ -6,4 +6,8 @@ enable + + + + From 3e5c5620304065f94a35aaebdf85dbafa286d286 Mon Sep 17 00:00:00 2001 From: TheNexusAvenger <13441476+TheNexusAvenger@users.noreply.github.com> Date: Tue, 15 Mar 2022 23:35:33 -0400 Subject: [PATCH 11/22] Add point in shape helper method. --- Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs | 32 +++++++++++++++++++++ Uchu.NavMesh/Shape/OrderedShape.cs | 25 ++++++++++++++++ 2 files changed, 57 insertions(+) diff --git a/Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs b/Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs index d0dcb099..425bed20 100644 --- a/Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs +++ b/Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs @@ -13,6 +13,7 @@ public class OrderedShapeTest [Test] public void TestOptimize() { + // Create the test shape. var shape = new OrderedShape() { Points = new List() @@ -31,6 +32,7 @@ public void TestOptimize() }, }; + // Test optimizing the shape. shape.Optimize(); Assert.AreEqual(new List() { @@ -40,4 +42,34 @@ public void TestOptimize() new Vector2(0, -1), }, shape.Points); } + + /// + /// Tests the PointInShape method. + /// + [Test] + public void TestPointInShape() + { + // Create the test shape. + var shape = new OrderedShape() + { + Points = new List() + { + new Vector2(0, 0), + new Vector2(-2, -2), + new Vector2(-2, 2), + new Vector2(2, 2), + new Vector2(2, -2), + }, + }; + + // Test that various parts are in the shape. + Assert.IsTrue(shape.PointInShape(new Vector2(0, 0))); + Assert.IsTrue(shape.PointInShape(new Vector2(1, 1))); + Assert.IsTrue(shape.PointInShape(new Vector2(1, -1))); + Assert.IsTrue(shape.PointInShape(new Vector2(0, 1))); + Assert.IsFalse(shape.PointInShape(new Vector2(0, -1))); + Assert.IsFalse(shape.PointInShape(new Vector2(-3, -1))); + Assert.IsFalse(shape.PointInShape(new Vector2(3, -1))); + Assert.IsFalse(shape.PointInShape(new Vector2(0, 3))); + } } \ No newline at end of file diff --git a/Uchu.NavMesh/Shape/OrderedShape.cs b/Uchu.NavMesh/Shape/OrderedShape.cs index 026e2ba3..2344bef2 100644 --- a/Uchu.NavMesh/Shape/OrderedShape.cs +++ b/Uchu.NavMesh/Shape/OrderedShape.cs @@ -36,4 +36,29 @@ public void Optimize() this.Points.RemoveAt(0); } } + + /// + /// Returns if a point is in the shape. + /// + /// Point to check. + /// Whether the point is in the shape. + public bool PointInShape(Vector2 point) + { + // Get the lines that are left of the point. + var linesLeftOfPoint = 0; + for (var i = 0; i < this.Points.Count; i++) + { + var currentPoint = this.Points[i]; + if (point == currentPoint) return true; + var lastPoint = this.Points[i == 0 ? this.Points.Count - 1 : (i - 1)]; + if (!((currentPoint.Y > point.Y && lastPoint.Y < point.Y) || (currentPoint.Y < point.Y && lastPoint.Y > point.Y))) continue; + var lineRatio = (point.Y - currentPoint.Y) / (lastPoint.Y - currentPoint.Y); + var lineX = currentPoint.X + ((lastPoint.X - currentPoint.X) * lineRatio); + if (lineX > point.X) continue; + linesLeftOfPoint += 1; + } + + // Return if the points to the left is odd. + return linesLeftOfPoint % 2 == 1; + } } \ No newline at end of file From c26ba08071103857c87923972ac8853f47db8d3d Mon Sep 17 00:00:00 2001 From: TheNexusAvenger <13441476+TheNexusAvenger@users.noreply.github.com> Date: Wed, 16 Mar 2022 00:09:54 -0400 Subject: [PATCH 12/22] Add line intersection tester. --- Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs | 32 ++++++++++++++ Uchu.NavMesh/Shape/OrderedShape.cs | 49 +++++++++++++++++++++ 2 files changed, 81 insertions(+) diff --git a/Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs b/Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs index 425bed20..51e52808 100644 --- a/Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs +++ b/Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs @@ -72,4 +72,36 @@ public void TestPointInShape() Assert.IsFalse(shape.PointInShape(new Vector2(3, -1))); Assert.IsFalse(shape.PointInShape(new Vector2(0, 3))); } + + /// + /// Tests teh LineValid method. + /// + [Test] + public void TestLineValid() + { + // Create the test shape. + var shape = new OrderedShape() + { + Points = new List() + { + new Vector2(0, 0), + new Vector2(-2, -2), + new Vector2(-2, 2), + new Vector2(2, 2), + new Vector2(2, -2), + }, + }; + + // Test with lines that make up the shape. + Assert.IsTrue(shape.LineValid(new Vector2(0, 0), new Vector2(-2, -2))); + Assert.IsTrue(shape.LineValid(new Vector2(2, 2), new Vector2(-2, 2))); + + // Test with lines completely inside or outside the shape. + Assert.IsTrue(shape.LineValid(new Vector2(-1, 1), new Vector2(1, 1))); + Assert.IsFalse(shape.LineValid(new Vector2(-2, -2), new Vector2(2, -2))); + + // Test with intersections. + Assert.IsFalse(shape.LineValid(new Vector2(-1, -1), new Vector2(1, -1))); + Assert.IsFalse(shape.LineValid(new Vector2(-2, -2), new Vector2(2, 2))); + } } \ No newline at end of file diff --git a/Uchu.NavMesh/Shape/OrderedShape.cs b/Uchu.NavMesh/Shape/OrderedShape.cs index 2344bef2..3d51c41a 100644 --- a/Uchu.NavMesh/Shape/OrderedShape.cs +++ b/Uchu.NavMesh/Shape/OrderedShape.cs @@ -9,6 +9,17 @@ public class OrderedShape /// public List Points { get; set; } = new List(); + /// + /// Returns the cross product of 2 2D vectors. + /// + /// The first point. + /// The second point. + /// The cross product of the 2 vectors. + private static double Cross(Vector2 point1, Vector2 point2) + { + return (point1.X * point2.Y) - (point1.Y * point2.X); + } + /// /// Optimizes the shape by removing points to make longer lines. /// @@ -61,4 +72,42 @@ public bool PointInShape(Vector2 point) // Return if the points to the left is odd. return linesLeftOfPoint % 2 == 1; } + + + + /// + /// Returns if a line is valid for the shape. A line is considered valid if + /// + /// Start point of the line. + /// End point of the line. + /// Whether the line is valid. + public bool LineValid(Vector2 start, Vector2 end) + { + // Return false if at least 1 line intersects. + var lineDelta1 = end - start; + for (var i = 0; i < this.Points.Count; i++) + { + // Get the start and end. Ignore if the start or end of the line match the start or end of the parameters. + var currentPoint = this.Points[i]; + var lastPoint = this.Points[i == 0 ? this.Points.Count - 1 : (i - 1)]; + if ((currentPoint == start && lastPoint == end) || (lastPoint == start && currentPoint == end)) return true; + if (currentPoint == start || currentPoint == end) continue; + if (lastPoint == start || lastPoint == end) continue; + + // Return false if the lines intersect. + var lineDelta2 = lastPoint - currentPoint; + var mainCross = Cross(lineDelta1, lineDelta2); + var coefficient1 = Cross(currentPoint - start, lineDelta1) / mainCross; + var coefficient2 = Cross(currentPoint - start, lineDelta2) / mainCross; + if (coefficient1 >= 0 && coefficient1 <= 1 && coefficient2 >= 0 && coefficient2 <= 1) + return false; + } + + // Return false if the middle of the line is not in the shape. + if (!this.PointInShape(new Vector2(start.X + ((end.X - start.X) / 2), start.Y + ((end.Y - start.Y) / 2)))) + return false; + + // Return true (valid). + return true; + } } \ No newline at end of file From 832a0ca13acda1064681c681c0269d7936dfe12c Mon Sep 17 00:00:00 2001 From: TheNexusAvenger <13441476+TheNexusAvenger@users.noreply.github.com> Date: Wed, 16 Mar 2022 00:25:25 -0400 Subject: [PATCH 13/22] Add generating nodes from shape. --- Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs | 37 ++++++++++++++++++++- Uchu.NavMesh/Graph/Node.cs | 25 ++++++++++++++ Uchu.NavMesh/Shape/OrderedShape.cs | 31 +++++++++++++++-- 3 files changed, 90 insertions(+), 3 deletions(-) create mode 100644 Uchu.NavMesh/Graph/Node.cs diff --git a/Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs b/Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs index 51e52808..482c81f9 100644 --- a/Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs +++ b/Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs @@ -74,7 +74,7 @@ public void TestPointInShape() } /// - /// Tests teh LineValid method. + /// Tests the LineValid method. /// [Test] public void TestLineValid() @@ -104,4 +104,39 @@ public void TestLineValid() Assert.IsFalse(shape.LineValid(new Vector2(-1, -1), new Vector2(1, -1))); Assert.IsFalse(shape.LineValid(new Vector2(-2, -2), new Vector2(2, 2))); } + + /// + /// Tests the GenerateGraph method. + /// + [Test] + public void TestGenerateGraph() + { + // Create the test shape. + var shape = new OrderedShape() + { + Points = new List() + { + new Vector2(0, -1), + new Vector2(-2, -2), + new Vector2(-2, 2), + new Vector2(2, 2), + new Vector2(2, -2), + }, + }; + shape.GenerateGraph(); + + // Test that the connected nodes are correct. + Assert.AreEqual(4, shape.Nodes[0].Nodes.Count); + Assert.AreEqual(new Vector2(-2, -2), shape.Nodes[0].Nodes[0].Point); + Assert.AreEqual(new Vector2(-2, 2), shape.Nodes[0].Nodes[1].Point); + Assert.AreEqual(new Vector2(2, 2), shape.Nodes[0].Nodes[2].Point); + Assert.AreEqual(new Vector2(2, -2), shape.Nodes[0].Nodes[3].Point); + Assert.AreEqual(3, shape.Nodes[1].Nodes.Count); + Assert.AreEqual(new Vector2(0, -1), shape.Nodes[1].Nodes[0].Point); + Assert.AreEqual(new Vector2(-2, 2), shape.Nodes[1].Nodes[1].Point); + Assert.AreEqual(new Vector2(2, 2), shape.Nodes[1].Nodes[2].Point); + Assert.AreEqual(4, shape.Nodes[2].Nodes.Count); + Assert.AreEqual(4, shape.Nodes[3].Nodes.Count); + Assert.AreEqual(3, shape.Nodes[4].Nodes.Count); + } } \ No newline at end of file diff --git a/Uchu.NavMesh/Graph/Node.cs b/Uchu.NavMesh/Graph/Node.cs new file mode 100644 index 00000000..2b2274ef --- /dev/null +++ b/Uchu.NavMesh/Graph/Node.cs @@ -0,0 +1,25 @@ +using System.Numerics; + +namespace Uchu.NavMesh.Graph; + +public class Node +{ + /// + /// Point of the node. + /// + public Vector2 Point { get; set; } + + /// + /// Nodes that are connected. + /// + public List Nodes { get; set; } = new List(); + + /// + /// Creates the node. + /// + /// Point of the node. + public Node(Vector2 point) + { + this.Point = point; + } +} \ No newline at end of file diff --git a/Uchu.NavMesh/Shape/OrderedShape.cs b/Uchu.NavMesh/Shape/OrderedShape.cs index 3d51c41a..8e111286 100644 --- a/Uchu.NavMesh/Shape/OrderedShape.cs +++ b/Uchu.NavMesh/Shape/OrderedShape.cs @@ -1,4 +1,5 @@ using System.Numerics; +using Uchu.NavMesh.Graph; namespace Uchu.NavMesh.Shape; @@ -9,6 +10,11 @@ public class OrderedShape /// public List Points { get; set; } = new List(); + /// + /// Nodes of the shape. + /// + public List Nodes { get; set; } = new List(); + /// /// Returns the cross product of 2 2D vectors. /// @@ -48,6 +54,29 @@ public void Optimize() } } + /// + /// Generates the connected nodes of the shape. + /// + public void GenerateGraph() + { + // Create the nodes. + foreach (var point in this.Points) + { + this.Nodes.Add(new Node(point)); + } + + // Connect the nodes. + foreach (var node in this.Nodes) + { + foreach (var otherNode in this.Nodes) + { + if (node == otherNode) continue; + if (!this.LineValid(node.Point, otherNode.Point)) continue; + node.Nodes.Add(otherNode); + } + } + } + /// /// Returns if a point is in the shape. /// @@ -73,8 +102,6 @@ public bool PointInShape(Vector2 point) return linesLeftOfPoint % 2 == 1; } - - /// /// Returns if a line is valid for the shape. A line is considered valid if /// From c46000a7ac9a2e2cf364ddf35230f1d70baa812d Mon Sep 17 00:00:00 2001 From: TheNexusAvenger <13441476+TheNexusAvenger@users.noreply.github.com> Date: Wed, 16 Mar 2022 01:30:37 -0400 Subject: [PATCH 14/22] Add shape nesting. --- Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs | 64 +++++++++++++++++++++ Uchu.NavMesh/Shape/OrderedShape.cs | 36 ++++++++++++ 2 files changed, 100 insertions(+) diff --git a/Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs b/Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs index 482c81f9..d4f288e5 100644 --- a/Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs +++ b/Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs @@ -105,6 +105,70 @@ public void TestLineValid() Assert.IsFalse(shape.LineValid(new Vector2(-2, -2), new Vector2(2, 2))); } + /// + /// Tests the TryAddShape method. + /// + [Test] + public void TestTryAddShape() + { + // Create several rectangles. + var shape1 = new OrderedShape() + { + Points = new List() + { + new Vector2(-1, 0), + new Vector2(-1, 1), + new Vector2(1, 1), + new Vector2(1, 0), + }, + }; + var shape2 = new OrderedShape() + { + Points = new List() + { + new Vector2(-1, 0), + new Vector2(-1, -1), + new Vector2(1, -1), + new Vector2(1, 0), + }, + }; + var shape3 = new OrderedShape() + { + Points = new List() + { + new Vector2(-2, -2), + new Vector2(-2, 2), + new Vector2(2, 2), + new Vector2(2, -2), + }, + }; + var shape4 = new OrderedShape() + { + Points = new List() + { + new Vector2(-3, -3), + new Vector2(-3, 3), + new Vector2(3, 3), + new Vector2(3, -3), + }, + }; + + // Assert certain shapes that can't be added. + Assert.IsFalse(shape1.TryAddShape(shape2)); + Assert.IsFalse(shape1.TryAddShape(shape3)); + + // Assert adding shapes. + Assert.IsTrue(shape4.TryAddShape(shape1)); + Assert.IsTrue(shape4.TryAddShape(shape3)); + Assert.IsTrue(shape4.TryAddShape(shape2)); + + // Assert the correct shapes are stored. + Assert.AreEqual(new List(), shape1.Shapes); + Assert.AreEqual(new List(), shape2.Shapes); + Assert.AreEqual(new List() {shape1, shape2}, shape3.Shapes); + Assert.AreEqual(new List() {shape3}, shape4.Shapes); + } + /// /// Tests the GenerateGraph method. /// diff --git a/Uchu.NavMesh/Shape/OrderedShape.cs b/Uchu.NavMesh/Shape/OrderedShape.cs index 8e111286..ff1be4d4 100644 --- a/Uchu.NavMesh/Shape/OrderedShape.cs +++ b/Uchu.NavMesh/Shape/OrderedShape.cs @@ -15,6 +15,11 @@ public class OrderedShape /// public List Nodes { get; set; } = new List(); + /// + /// Shapes that are contained in the shape. + /// + public List Shapes { get; set; } = new List(); + /// /// Returns the cross product of 2 2D vectors. /// @@ -77,6 +82,37 @@ public void GenerateGraph() } } + /// + /// Tries to add a child shape. + /// + /// Shape to try to add. + /// Whether the shape was added. + public bool TryAddShape(OrderedShape shape) + { + // Return false if there is a point not in the shape. + foreach (var point in shape.Points) + { + if (this.Points.Contains(point)) continue; + if (!this.PointInShape(point)) return false; + } + + // Return true if it can be added directly to a child shape. + foreach (var otherShape in this.Shapes) + { + if (!otherShape.TryAddShape(shape)) continue; + return true; + } + + // Add the child shape directly and remove the child shapes that are contained in the new shape. + foreach (var otherShape in this.Shapes.ToList()) + { + if (!shape.TryAddShape(otherShape)) continue; + this.Shapes.Remove(otherShape); + } + this.Shapes.Add(shape); + return true; + } + /// /// Returns if a point is in the shape. /// From 2bc1a7e8886a9cdd8ff4619f06a4cab474dbede0 Mon Sep 17 00:00:00 2001 From: TheNexusAvenger <13441476+TheNexusAvenger@users.noreply.github.com> Date: Wed, 23 Mar 2022 00:21:07 -0400 Subject: [PATCH 15/22] Add additional test cases. --- Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs | 31 +++++++++++++++++++++ 1 file changed, 31 insertions(+) diff --git a/Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs b/Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs index d4f288e5..c58f9507 100644 --- a/Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs +++ b/Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs @@ -71,6 +71,37 @@ public void TestPointInShape() Assert.IsFalse(shape.PointInShape(new Vector2(-3, -1))); Assert.IsFalse(shape.PointInShape(new Vector2(3, -1))); Assert.IsFalse(shape.PointInShape(new Vector2(0, 3))); + + // Test edges cases where the point is inline with the lines. + Assert.IsTrue(shape.PointInShape(new Vector2(-1, 0))); + Assert.IsTrue(shape.PointInShape(new Vector2(1, 0))); + Assert.IsFalse(shape.PointInShape(new Vector2(-3, 0))); + Assert.IsFalse(shape.PointInShape(new Vector2(3, 0))); + } + + /// + /// Tests the PointInShape method with a horizontal line. + /// + [Test] + public void TestPointHorizontalLineEdgeCaseInShape() + { + // Create the test shape. + var shape = new OrderedShape() + { + Points = new List() + { + new Vector2(0, 0), + new Vector2(0, -2), + new Vector2(-2, 2), + new Vector2(2, 2), + new Vector2(2, -2), + }, + }; + + // Test the edge case points. + Assert.IsTrue(shape.PointInShape(new Vector2(1, 0))); + Assert.IsFalse(shape.PointInShape(new Vector2(-3, 0))); + Assert.IsFalse(shape.PointInShape(new Vector2(3, 0))); } /// From 3ca1a7fe94032d1ccf1c9f9228f6739f258235cf Mon Sep 17 00:00:00 2001 From: TheNexusAvenger <13441476+TheNexusAvenger@users.noreply.github.com> Date: Wed, 23 Mar 2022 01:11:31 -0400 Subject: [PATCH 16/22] Fix edge cases with point in shape. --- Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs | 29 ++---------------- Uchu.NavMesh/Shape/OrderedShape.cs | 34 ++++++++++++++------- 2 files changed, 25 insertions(+), 38 deletions(-) diff --git a/Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs b/Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs index c58f9507..351518da 100644 --- a/Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs +++ b/Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs @@ -64,8 +64,8 @@ public void TestPointInShape() // Test that various parts are in the shape. Assert.IsTrue(shape.PointInShape(new Vector2(0, 0))); - Assert.IsTrue(shape.PointInShape(new Vector2(1, 1))); - Assert.IsTrue(shape.PointInShape(new Vector2(1, -1))); + Assert.IsTrue(shape.PointInShape(new Vector2(0.5f, 1))); + Assert.IsTrue(shape.PointInShape(new Vector2(1.5f, -1))); Assert.IsTrue(shape.PointInShape(new Vector2(0, 1))); Assert.IsFalse(shape.PointInShape(new Vector2(0, -1))); Assert.IsFalse(shape.PointInShape(new Vector2(-3, -1))); @@ -78,31 +78,6 @@ public void TestPointInShape() Assert.IsFalse(shape.PointInShape(new Vector2(-3, 0))); Assert.IsFalse(shape.PointInShape(new Vector2(3, 0))); } - - /// - /// Tests the PointInShape method with a horizontal line. - /// - [Test] - public void TestPointHorizontalLineEdgeCaseInShape() - { - // Create the test shape. - var shape = new OrderedShape() - { - Points = new List() - { - new Vector2(0, 0), - new Vector2(0, -2), - new Vector2(-2, 2), - new Vector2(2, 2), - new Vector2(2, -2), - }, - }; - - // Test the edge case points. - Assert.IsTrue(shape.PointInShape(new Vector2(1, 0))); - Assert.IsFalse(shape.PointInShape(new Vector2(-3, 0))); - Assert.IsFalse(shape.PointInShape(new Vector2(3, 0))); - } /// /// Tests the LineValid method. diff --git a/Uchu.NavMesh/Shape/OrderedShape.cs b/Uchu.NavMesh/Shape/OrderedShape.cs index ff1be4d4..1e5eb2eb 100644 --- a/Uchu.NavMesh/Shape/OrderedShape.cs +++ b/Uchu.NavMesh/Shape/OrderedShape.cs @@ -92,7 +92,6 @@ public bool TryAddShape(OrderedShape shape) // Return false if there is a point not in the shape. foreach (var point in shape.Points) { - if (this.Points.Contains(point)) continue; if (!this.PointInShape(point)) return false; } @@ -120,22 +119,35 @@ public bool TryAddShape(OrderedShape shape) /// Whether the point is in the shape. public bool PointInShape(Vector2 point) { - // Get the lines that are left of the point. - var linesLeftOfPoint = 0; + // Get the sum of the angles of the point to every pair of points that form the lines. + var totalAngle = 0d; for (var i = 0; i < this.Points.Count; i++) { + // Get the current point and last point. var currentPoint = this.Points[i]; if (point == currentPoint) return true; var lastPoint = this.Points[i == 0 ? this.Points.Count - 1 : (i - 1)]; - if (!((currentPoint.Y > point.Y && lastPoint.Y < point.Y) || (currentPoint.Y < point.Y && lastPoint.Y > point.Y))) continue; - var lineRatio = (point.Y - currentPoint.Y) / (lastPoint.Y - currentPoint.Y); - var lineX = currentPoint.X + ((lastPoint.X - currentPoint.X) * lineRatio); - if (lineX > point.X) continue; - linesLeftOfPoint += 1; + + // Determine the angles to each point and determine the angle difference. + var theta1 = Math.Atan2(currentPoint.Y - point.Y, currentPoint.X - point.X); + var theta2 = Math.Atan2(lastPoint.Y - point.Y, lastPoint.X - point.X); + var thetaDelta = theta2 - theta1; + while (thetaDelta > Math.PI) + { + thetaDelta += -(2 * Math.PI); + } + while (thetaDelta < -Math.PI) + { + thetaDelta += (2 * Math.PI); + } + + // Add the difference. + totalAngle += thetaDelta; } - - // Return if the points to the left is odd. - return linesLeftOfPoint % 2 == 1; + + // Return if the sum is 360 degrees. + // If it is 0, the point is outside the polygon. + return Math.Abs(totalAngle) > Math.PI; } /// From 57526baa55a947e62d8eeb88ddfd3242d783e93c Mon Sep 17 00:00:00 2001 From: TheNexusAvenger <13441476+TheNexusAvenger@users.noreply.github.com> Date: Wed, 23 Mar 2022 01:14:15 -0400 Subject: [PATCH 17/22] Store bounding shape with inner shapes. --- Uchu.NavMesh/Shape/Solver.cs | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) diff --git a/Uchu.NavMesh/Shape/Solver.cs b/Uchu.NavMesh/Shape/Solver.cs index aacf73f2..93ea73bf 100644 --- a/Uchu.NavMesh/Shape/Solver.cs +++ b/Uchu.NavMesh/Shape/Solver.cs @@ -22,9 +22,9 @@ public class Solver public HeightMap HeightMap { get; private set; } /// - /// Shapes that define the boundaries in 2D. + /// Shape that define the boundaries in 2D. /// - public List BoundingShapes { get; private set; } + public OrderedShape BoundingShape { get; private set; } /// /// Initializes the solver. @@ -197,6 +197,19 @@ private async Task GenerateShapesAsync() await Task.WhenAll(tasks); // Store the shapes. - this.BoundingShapes = shapes; + this.BoundingShape = new OrderedShape() + { + Points = new List() + { + new Vector2(float.MaxValue, float.MaxValue), + new Vector2(float.MinValue, float.MaxValue), + new Vector2(float.MinValue, float.MinValue), + new Vector2(float.MaxValue, float.MinValue), + } + }; + foreach (var shape in shapes) + { + this.BoundingShape.TryAddShape(shape); + } } } \ No newline at end of file From c8cc170dd78ccea820d0fc4e6dda35b6ab5abb23 Mon Sep 17 00:00:00 2001 From: TheNexusAvenger <13441476+TheNexusAvenger@users.noreply.github.com> Date: Wed, 23 Mar 2022 01:24:27 -0400 Subject: [PATCH 18/22] Split line intersection test. --- Uchu.NavMesh/Shape/OrderedShape.cs | 41 +++++++++++++++++++++++++----- 1 file changed, 35 insertions(+), 6 deletions(-) diff --git a/Uchu.NavMesh/Shape/OrderedShape.cs b/Uchu.NavMesh/Shape/OrderedShape.cs index 1e5eb2eb..06079f67 100644 --- a/Uchu.NavMesh/Shape/OrderedShape.cs +++ b/Uchu.NavMesh/Shape/OrderedShape.cs @@ -5,6 +5,15 @@ namespace Uchu.NavMesh.Shape; public class OrderedShape { + /// + /// Result for a line intersection test. + /// + public enum LineIntersectionResult { + LineIntersects, + NoLineIntersects, + PartOfShape, + } + /// /// Points of the ordered shape. /// @@ -151,21 +160,22 @@ public bool PointInShape(Vector2 point) } /// - /// Returns if a line is valid for the shape. A line is considered valid if + /// Returns if the given line intersects the shape. + /// Inner shapes are not checked. /// /// Start point of the line. /// End point of the line. - /// Whether the line is valid. - public bool LineValid(Vector2 start, Vector2 end) + /// Whether the line intersects the shape. + public LineIntersectionResult LineIntersects(Vector2 start, Vector2 end) { - // Return false if at least 1 line intersects. + // Return true if at least 1 line intersects. var lineDelta1 = end - start; for (var i = 0; i < this.Points.Count; i++) { // Get the start and end. Ignore if the start or end of the line match the start or end of the parameters. var currentPoint = this.Points[i]; var lastPoint = this.Points[i == 0 ? this.Points.Count - 1 : (i - 1)]; - if ((currentPoint == start && lastPoint == end) || (lastPoint == start && currentPoint == end)) return true; + if ((currentPoint == start && lastPoint == end) || (lastPoint == start && currentPoint == end)) return LineIntersectionResult.PartOfShape; if (currentPoint == start || currentPoint == end) continue; if (lastPoint == start || lastPoint == end) continue; @@ -175,8 +185,27 @@ public bool LineValid(Vector2 start, Vector2 end) var coefficient1 = Cross(currentPoint - start, lineDelta1) / mainCross; var coefficient2 = Cross(currentPoint - start, lineDelta2) / mainCross; if (coefficient1 >= 0 && coefficient1 <= 1 && coefficient2 >= 0 && coefficient2 <= 1) - return false; + return LineIntersectionResult.LineIntersects; } + + // Return false (doesn't intersect). + return LineIntersectionResult.NoLineIntersects; + } + + /// + /// Returns if a line is valid for the shape. A line is considered valid if + /// + /// Start point of the line. + /// End point of the line. + /// Whether the line is valid. + public bool LineValid(Vector2 start, Vector2 end) + { + // Return false if at least 1 line intersects. + var lineIntersectResult = this.LineIntersects(start, end); + if (lineIntersectResult == LineIntersectionResult.PartOfShape) + return true; + if (lineIntersectResult == LineIntersectionResult.LineIntersects) + return false; // Return false if the middle of the line is not in the shape. if (!this.PointInShape(new Vector2(start.X + ((end.X - start.X) / 2), start.Y + ((end.Y - start.Y) / 2)))) From 8fda7b5a93030b2330d35123079dbed7da5b246c Mon Sep 17 00:00:00 2001 From: TheNexusAvenger <13441476+TheNexusAvenger@users.noreply.github.com> Date: Wed, 23 Mar 2022 01:37:08 -0400 Subject: [PATCH 19/22] Test for inner shapes with checking for valid lines. --- Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs | 55 +++++++++++++++++++++ Uchu.NavMesh/Shape/OrderedShape.cs | 12 +++++ 2 files changed, 67 insertions(+) diff --git a/Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs b/Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs index 351518da..f2106a81 100644 --- a/Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs +++ b/Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs @@ -111,6 +111,61 @@ public void TestLineValid() Assert.IsFalse(shape.LineValid(new Vector2(-2, -2), new Vector2(2, 2))); } + /// + /// Tests the LineValid method with a containing shape.. + /// + [Test] + public void TestLineValidContainingShape() + { + // Create the test shape. + var shape = new OrderedShape() + { + Points = new List() + { + new Vector2(-2, -2), + new Vector2(-2, 2), + new Vector2(2, 2), + new Vector2(2, -2), + }, + Shapes = new List() + { + new OrderedShape() + { + Points = new List() + { + new Vector2(-1, -1), + new Vector2(-1, 1), + new Vector2(1, 1), + new Vector2(1, -1), + }, + } + } + }; + + // Test with lines that make up the shape. + Assert.IsTrue(shape.LineValid(new Vector2(-2, 2), new Vector2(-2, -2))); + Assert.IsTrue(shape.LineValid(new Vector2(2, 2), new Vector2(-2, 2))); + + // Test with lines completely inside or outside the shape. + Assert.IsTrue(shape.LineValid(new Vector2(-1, 1.5f), new Vector2(1, 1.5f))); + Assert.IsFalse(shape.LineValid(new Vector2(-3, -3), new Vector2(3, -3))); + + // Test with lines that are part of the inner shape. + Assert.IsTrue(shape.LineValid(new Vector2(-1, -1), new Vector2(-1, 1))); + Assert.IsTrue(shape.LineValid(new Vector2(1, -1), new Vector2(1, 1))); + + // Test with lines that intersect the inner shape. + Assert.IsFalse(shape.LineValid(new Vector2(-2, -2), new Vector2(2, 2))); + Assert.IsFalse(shape.LineValid(new Vector2(-2, 2), new Vector2(2, -2))); + + // Test with lines inside the inner shape. + Assert.IsFalse(shape.LineValid(new Vector2(-1, -1), new Vector2(1, 1))); + Assert.IsFalse(shape.LineValid(new Vector2(-1, 1), new Vector2(-1, 1))); + + // Test with intersections. + Assert.IsFalse(shape.LineValid(new Vector2(-3, -1), new Vector2(-1, -1))); + } + /// /// Tests the TryAddShape method. /// diff --git a/Uchu.NavMesh/Shape/OrderedShape.cs b/Uchu.NavMesh/Shape/OrderedShape.cs index 06079f67..d2c91590 100644 --- a/Uchu.NavMesh/Shape/OrderedShape.cs +++ b/Uchu.NavMesh/Shape/OrderedShape.cs @@ -207,6 +207,18 @@ public bool LineValid(Vector2 start, Vector2 end) if (lineIntersectResult == LineIntersectionResult.LineIntersects) return false; + // Return false if a contained shape intersects or the center of the line is inside the contained shape. + foreach (var shape in this.Shapes) + { + var containedLineIntersectResult = shape.LineIntersects(start, end); + if (containedLineIntersectResult == LineIntersectionResult.PartOfShape) + return true; + if (containedLineIntersectResult == LineIntersectionResult.LineIntersects) + return false; + if (shape.PointInShape(new Vector2(start.X + ((end.X - start.X) * 0.5f), start.Y + ((end.Y - start.Y) * 0.5f)))) + return false; + } + // Return false if the middle of the line is not in the shape. if (!this.PointInShape(new Vector2(start.X + ((end.X - start.X) / 2), start.Y + ((end.Y - start.Y) / 2)))) return false; From 23317e24f55574e9f13618fd417e9b3c57f09bae Mon Sep 17 00:00:00 2001 From: TheNexusAvenger <13441476+TheNexusAvenger@users.noreply.github.com> Date: Wed, 23 Mar 2022 01:48:47 -0400 Subject: [PATCH 20/22] Generate nodes to contained shapes. --- Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs | 58 +++++++++++++++++++++ Uchu.NavMesh/Shape/OrderedShape.cs | 8 +++ 2 files changed, 66 insertions(+) diff --git a/Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs b/Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs index f2106a81..d8db1b90 100644 --- a/Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs +++ b/Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs @@ -264,4 +264,62 @@ public void TestGenerateGraph() Assert.AreEqual(4, shape.Nodes[3].Nodes.Count); Assert.AreEqual(3, shape.Nodes[4].Nodes.Count); } + + /// + /// Tests the GenerateGraph method with contained shapes. + /// + [Test] + public void TestGenerateGraphContainedShapes() + { + // Create the test shape. + var shape = new OrderedShape() + { + Points = new List() + { + new Vector2(-4, -4), + new Vector2(-4, 4), + new Vector2(4, 4), + new Vector2(4, -4), + }, + Shapes = new List() + { + new OrderedShape() + { + Points = new List() + { + new Vector2(-2, -2), + new Vector2(-2, -1), + new Vector2(2, -1), + new Vector2(2, -2), + }, + }, + new OrderedShape() + { + Points = new List() + { + new Vector2(-2, 2), + new Vector2(-2, 1), + new Vector2(2, 1), + new Vector2(2, 2), + }, + }, + }, + }; + shape.GenerateGraph(); + + // Test that the connected nodes are correct. + // Due to how many nodes there are, only the totals are checked. + Assert.AreEqual(7, shape.Nodes[0].Nodes.Count); + Assert.AreEqual(7, shape.Nodes[1].Nodes.Count); + Assert.AreEqual(7, shape.Nodes[2].Nodes.Count); + Assert.AreEqual(7, shape.Nodes[3].Nodes.Count); + Assert.AreEqual(5, shape.Nodes[4].Nodes.Count); + Assert.AreEqual(6, shape.Nodes[5].Nodes.Count); + Assert.AreEqual(6, shape.Nodes[6].Nodes.Count); + Assert.AreEqual(5, shape.Nodes[7].Nodes.Count); + Assert.AreEqual(5, shape.Nodes[8].Nodes.Count); + Assert.AreEqual(6, shape.Nodes[9].Nodes.Count); + Assert.AreEqual(6, shape.Nodes[10].Nodes.Count); + Assert.AreEqual(5, shape.Nodes[11].Nodes.Count); + } } \ No newline at end of file diff --git a/Uchu.NavMesh/Shape/OrderedShape.cs b/Uchu.NavMesh/Shape/OrderedShape.cs index d2c91590..8be13832 100644 --- a/Uchu.NavMesh/Shape/OrderedShape.cs +++ b/Uchu.NavMesh/Shape/OrderedShape.cs @@ -78,6 +78,14 @@ public void GenerateGraph() { this.Nodes.Add(new Node(point)); } + foreach (var shape in this.Shapes) + { + foreach (var point in shape.Points) + { + if (this.Nodes.FirstOrDefault(node => node.Point == point) != null) continue; + this.Nodes.Add(new Node(point)); + } + } // Connect the nodes. foreach (var node in this.Nodes) From 5bced6658078719ad0afeb7e4c46cbc873a0a242 Mon Sep 17 00:00:00 2001 From: TheNexusAvenger <13441476+TheNexusAvenger@users.noreply.github.com> Date: Wed, 23 Mar 2022 02:31:58 -0400 Subject: [PATCH 21/22] Generate nodes for shapes. --- Uchu.NavMesh/Shape/Solver.cs | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/Uchu.NavMesh/Shape/Solver.cs b/Uchu.NavMesh/Shape/Solver.cs index 93ea73bf..48810410 100644 --- a/Uchu.NavMesh/Shape/Solver.cs +++ b/Uchu.NavMesh/Shape/Solver.cs @@ -211,5 +211,16 @@ private async Task GenerateShapesAsync() { this.BoundingShape.TryAddShape(shape); } + + // Generate the nodes for each shape. + tasks = new List(); + foreach (var shape in shapes) + { + tasks.Add(Task.Run(() => + { + shape.GenerateGraph(); + })); + } + await Task.WhenAll(tasks); } } \ No newline at end of file From fddd5134d1b8e5bcc40816eb6226d578b7f4ada5 Mon Sep 17 00:00:00 2001 From: TheNexusAvenger <13441476+TheNexusAvenger@users.noreply.github.com> Date: Wed, 23 Mar 2022 16:55:14 -0400 Subject: [PATCH 22/22] Add caching for solver results. --- Uchu.NavMesh/Shape/OrderedShape.cs | 79 +++++++++++++++++++- Uchu.NavMesh/Shape/Solver.cs | 116 +++++++++++++++++++++++++++-- 2 files changed, 188 insertions(+), 7 deletions(-) diff --git a/Uchu.NavMesh/Shape/OrderedShape.cs b/Uchu.NavMesh/Shape/OrderedShape.cs index 8be13832..71b5e61d 100644 --- a/Uchu.NavMesh/Shape/OrderedShape.cs +++ b/Uchu.NavMesh/Shape/OrderedShape.cs @@ -1,9 +1,10 @@ using System.Numerics; +using RakDotNet.IO; using Uchu.NavMesh.Graph; namespace Uchu.NavMesh.Shape; -public class OrderedShape +public class OrderedShape : ISerializable, IDeserializable { /// /// Result for a line intersection test. @@ -234,4 +235,80 @@ public bool LineValid(Vector2 start, Vector2 end) // Return true (valid). return true; } + + /// + /// Serializes the object. + /// + /// Writer to write to. + public void Serialize(BitWriter writer) + { + // Write the points. + writer.Write(this.Points.Count); + foreach (var point in this.Points) + { + writer.Write(point); + } + + // Write the nodes. + writer.Write(this.Nodes.Count); + foreach (var node in this.Nodes) + { + writer.Write(node.Point); + } + foreach (var node in this.Nodes) + { + writer.Write(node.Nodes.Count); + foreach (var connectedNode in node.Nodes) + { + writer.Write(this.Nodes.FindIndex((listNode) => listNode == connectedNode)); + } + } + + // Write the shapes. + writer.Write(this.Shapes.Count); + foreach (var shape in this.Shapes) + { + shape.Serialize(writer); + } + } + + /// + /// Deserializes the object. + /// + /// Reader to read to. + public void Deserialize(BitReader reader) + { + // Read the points. + var totalPoints = reader.Read(); + for (var i = 0; i < totalPoints; i++) + { + this.Points.Add(reader.Read()); + } + + // Read the nodes. + var totalNodes = reader.Read(); + for (var i = 0; i < totalNodes; i++) + { + this.Nodes.Add(new Node(reader.Read())); + } + for (var i = 0; i < totalNodes; i++) + { + var node = this.Nodes[i]; + var totalConnections = reader.Read(); + for (var j = 0; j < totalConnections; j++) + { + var connectionIndex = reader.Read(); + node.Nodes.Add(this.Nodes[connectionIndex]); + } + } + + // Read the shapes. + var totalShapes = reader.Read(); + for (var i = 0; i < totalShapes; i++) + { + var shape = new OrderedShape(); + shape.Deserialize(reader); + this.Shapes.Add(shape); + } + } } \ No newline at end of file diff --git a/Uchu.NavMesh/Shape/Solver.cs b/Uchu.NavMesh/Shape/Solver.cs index 48810410..44b1b72f 100644 --- a/Uchu.NavMesh/Shape/Solver.cs +++ b/Uchu.NavMesh/Shape/Solver.cs @@ -1,11 +1,20 @@ using System.Numerics; +using RakDotNet.IO; +using Uchu.Core; using Uchu.NavMesh.Grid; using Uchu.World.Client; namespace Uchu.NavMesh.Shape; -public class Solver +public class Solver : ISerializable, IDeserializable { + /// + /// Version of the solver stored with the cache files. If the version does not match, the cached version of the + /// solver is discarded. If changes are made that change the results of generating the solver, this number + /// should be incremented to invalidate the cache entries of updating servers. + /// + public const int SolverVersion = 0; + /// /// Maximum distance 2 nodes on the heightmap can be before being considered to steep to connect. /// @@ -27,14 +36,90 @@ public class Solver public OrderedShape BoundingShape { get; private set; } /// - /// Initializes the solver. + /// Creates a solver for a zone. The zone may be cached. /// /// Zone info with a terrain file to read. - public async Task Initialize(ZoneInfo zoneInfo) + public static async Task FromZoneAsync(ZoneInfo zoneInfo) + { + // Create the solver with the heightmap. + var solver = new Solver(); + solver.HeightMap = HeightMap.FromZoneInfo(zoneInfo); + + // Return a cached entry. + try + { + await solver.LoadFromCacheAsync(zoneInfo); + return solver; + } + catch (FileNotFoundException) + { + // Cache file not found. + Logger.Information("Cached version of map path solver not found."); + } + catch (InvalidDataException) + { + // Delete the invalid cache file. + Logger.Information(""); + File.Delete(Path.Combine("MapSolverCache", zoneInfo.LuzFile.WorldId + ".bin")); + } + catch (Exception e) + { + Logger.Error(e); + } + + // Load the solver from the heightmap. + await solver.GenerateShapesAsync(); + await solver.SaveToCacheAsync(zoneInfo); + return solver; + } + + /// + /// Saves the file for caching. + /// + /// Zone info to save as. + private async Task SaveToCacheAsync(ZoneInfo zoneInfo) + { + // Create the directory if it does not exist. + if (!Directory.Exists("MapSolverCache")) + { + Directory.CreateDirectory("MapSolverCache"); + } + + // Save the file. + var path = Path.Combine("MapSolverCache", zoneInfo.LuzFile.WorldId + ".bin"); + if (File.Exists(path)) + return; + await using var memoryStream = new MemoryStream(); + var writer = new BitWriter(memoryStream); + writer.Write(SolverVersion); + this.Serialize(writer); + await File.WriteAllBytesAsync(path, memoryStream.ToArray()); + } + + /// + /// Loads the solver from the cache. Throws an exception if the cache is invalid. + /// + /// Zone info to save as. + private async Task LoadFromCacheAsync(ZoneInfo zoneInfo) { - this.HeightMap = HeightMap.FromZoneInfo(zoneInfo); - // TODO: Consider caching the results somehow. These can take time to generate. - await this.GenerateShapesAsync(); + // Throw an exception if the cache file does not exist. + var path = Path.Combine("MapSolverCache", zoneInfo.LuzFile.WorldId + ".bin"); + if (!File.Exists(path)) + throw new FileNotFoundException("Cache file not found"); + + // Start to read the file and throw an exception if the version does not match. + var cacheData = await File.ReadAllBytesAsync(path); + await using var memoryStream = new MemoryStream(cacheData.Length); + memoryStream.Write(cacheData); + await memoryStream.FlushAsync(); + memoryStream.Position = 0; + var reader = new BitReader(memoryStream); + var cacheVersion = reader.Read(); + if (cacheVersion != SolverVersion) + throw new InvalidDataException("Cache version is not current. (Expected " + SolverVersion + ", got " + cacheVersion); + + // Deserialize the file. + this.Deserialize(reader); } /// @@ -223,4 +308,23 @@ private async Task GenerateShapesAsync() } await Task.WhenAll(tasks); } + + /// + /// Serializes the object. + /// + /// Writer to write to. + public void Serialize(BitWriter writer) + { + this.BoundingShape.Serialize(writer); + } + + /// + /// Deserializes the object. + /// + /// Reader to read to. + public void Deserialize(BitReader reader) + { + this.BoundingShape = new OrderedShape(); + this.BoundingShape.Deserialize(reader); + } } \ No newline at end of file