diff --git a/Uchu.NavMesh.Test/Grid/EdgeTest.cs b/Uchu.NavMesh.Test/Grid/EdgeTest.cs
new file mode 100644
index 00000000..49f0bc5e
--- /dev/null
+++ b/Uchu.NavMesh.Test/Grid/EdgeTest.cs
@@ -0,0 +1,38 @@
+using System.Numerics;
+using NUnit.Framework;
+using Uchu.NavMesh.Grid;
+
+namespace Uchu.NavMesh.Test.Grid;
+
+public class EdgeTest
+{
+ ///
+ /// First test edge used.
+ ///
+ public Edge TestEdge1 = new Edge(Vector3.One, Vector3.Zero);
+
+ ///
+ /// Second test edge used.
+ ///
+ public Edge TestEdge2 = new Edge(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.Test/Shape/OrderedShapeTest.cs b/Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs
new file mode 100644
index 00000000..d8db1b90
--- /dev/null
+++ b/Uchu.NavMesh.Test/Shape/OrderedShapeTest.cs
@@ -0,0 +1,325 @@
+using System.Collections.Generic;
+using System.Numerics;
+using NUnit.Framework;
+using Uchu.NavMesh.Shape;
+
+namespace Uchu.NavMesh.Test.Shape;
+
+public class OrderedShapeTest
+{
+ ///
+ /// Tests the Optimize method.
+ ///
+ [Test]
+ public void TestOptimize()
+ {
+ // Create the test shape.
+ var shape = new OrderedShape()
+ {
+ 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),
+ },
+ };
+
+ // Test optimizing the shape.
+ shape.Optimize();
+ Assert.AreEqual(new List()
+ {
+ new Vector2(0, 3),
+ new Vector2(1, 4),
+ new Vector2(1, -1),
+ 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(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)));
+ 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 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)));
+ }
+
+ ///
+ /// 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.
+ ///
+ [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.
+ ///
+ [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);
+ }
+
+ ///
+ /// 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.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.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/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/Grid/Edge.cs b/Uchu.NavMesh/Grid/Edge.cs
new file mode 100644
index 00000000..5a8b73ea
--- /dev/null
+++ b/Uchu.NavMesh/Grid/Edge.cs
@@ -0,0 +1,61 @@
+using System.Numerics;
+
+namespace Uchu.NavMesh.Grid;
+
+public class Edge : IEquatable
+{
+ ///
+ /// Start of the edge.
+ ///
+ public readonly Vector3 Start;
+
+ ///
+ /// End of the edge.
+ ///
+ public readonly Vector3 End;
+
+ ///
+ /// Creates the edge.
+ ///
+ /// Start of the edge.
+ /// End of the edge.
+ public Edge(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(Edge? 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((Edge) 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
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/Grid/Node.cs b/Uchu.NavMesh/Grid/Node.cs
new file mode 100644
index 00000000..1ddc9d17
--- /dev/null
+++ b/Uchu.NavMesh/Grid/Node.cs
@@ -0,0 +1,25 @@
+using System.Numerics;
+
+namespace Uchu.NavMesh.Grid;
+
+public class Node
+{
+ ///
+ /// 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 Node(Vector3 position)
+ {
+ this.Position = position;
+ }
+}
\ No newline at end of file
diff --git a/Uchu.NavMesh/Shape/OrderedShape.cs b/Uchu.NavMesh/Shape/OrderedShape.cs
new file mode 100644
index 00000000..71b5e61d
--- /dev/null
+++ b/Uchu.NavMesh/Shape/OrderedShape.cs
@@ -0,0 +1,314 @@
+using System.Numerics;
+using RakDotNet.IO;
+using Uchu.NavMesh.Graph;
+
+namespace Uchu.NavMesh.Shape;
+
+public class OrderedShape : ISerializable, IDeserializable
+{
+ ///
+ /// Result for a line intersection test.
+ ///
+ public enum LineIntersectionResult {
+ LineIntersects,
+ NoLineIntersects,
+ PartOfShape,
+ }
+
+ ///
+ /// Points of the ordered shape.
+ ///
+ public List Points { get; set; } = new List();
+
+ ///
+ /// Nodes of the shape.
+ ///
+ 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.
+ ///
+ /// 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.
+ ///
+ 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);
+ }
+ }
+
+ ///
+ /// 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));
+ }
+ 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)
+ {
+ foreach (var otherNode in this.Nodes)
+ {
+ if (node == otherNode) continue;
+ if (!this.LineValid(node.Point, otherNode.Point)) continue;
+ node.Nodes.Add(otherNode);
+ }
+ }
+ }
+
+ ///
+ /// 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.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.
+ ///
+ /// Point to check.
+ /// Whether the point is in the shape.
+ public bool PointInShape(Vector2 point)
+ {
+ // 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)];
+
+ // 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 sum is 360 degrees.
+ // If it is 0, the point is outside the polygon.
+ return Math.Abs(totalAngle) > Math.PI;
+ }
+
+ ///
+ /// 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 intersects the shape.
+ public LineIntersectionResult LineIntersects(Vector2 start, Vector2 end)
+ {
+ // 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 LineIntersectionResult.PartOfShape;
+ 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 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 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;
+
+ // 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
new file mode 100644
index 00000000..44b1b72f
--- /dev/null
+++ b/Uchu.NavMesh/Shape/Solver.cs
@@ -0,0 +1,330 @@
+using System.Numerics;
+using RakDotNet.IO;
+using Uchu.Core;
+using Uchu.NavMesh.Grid;
+using Uchu.World.Client;
+
+namespace Uchu.NavMesh.Shape;
+
+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.
+ ///
+ 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; }
+
+ ///
+ /// Shape that define the boundaries in 2D.
+ ///
+ public OrderedShape BoundingShape { get; private set; }
+
+ ///
+ /// Creates a solver for a zone. The zone may be cached.
+ ///
+ /// Zone info with a terrain file to read.
+ 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)
+ {
+ // 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);
+ }
+
+ ///
+ /// 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.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);
+ }
+
+ // Generate the nodes for each shape.
+ tasks = new List();
+ foreach (var shape in shapes)
+ {
+ tasks.Add(Task.Run(() =>
+ {
+ shape.GenerateGraph();
+ }));
+ }
+ 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
diff --git a/Uchu.NavMesh/Shape/UnorderedShape.cs b/Uchu.NavMesh/Shape/UnorderedShape.cs
new file mode 100644
index 00000000..a41414b5
--- /dev/null
+++ b/Uchu.NavMesh/Shape/UnorderedShape.cs
@@ -0,0 +1,183 @@
+using System.Numerics;
+using Uchu.NavMesh.Grid;
+
+namespace Uchu.NavMesh.Shape;
+
+public class UnorderedShape
+{
+ ///
+ /// Edges of the shape.
+ ///
+ public HashSet Edges { get; set; } = new HashSet();
+
+ ///
+ /// Returns a shape from a set of nodes.
+ ///
+ /// 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 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 UnorderedShape()
+ {
+ Edges = {
+ 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 shape.
+ if (node1.Neighbors.Contains(node2) && node2.Neighbors.Contains(node3) && node3.Neighbors.Contains(node1))
+ {
+ // Return a shape without node 4.
+ return new UnorderedShape()
+ {
+ Edges = {
+ 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 shape without node 1.
+ return new UnorderedShape()
+ {
+ Edges = {
+ 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 shape without node 2.
+ return new UnorderedShape()
+ {
+ Edges = {
+ 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 shape without node 3.
+ return new UnorderedShape()
+ {
+ Edges = {
+ new Edge(node1.Position, node2.Position),
+ new Edge(node2.Position, node4.Position),
+ new Edge(node4.Position, node1.Position),
+ },
+ };
+ }
+
+ // Return null (not valid).
+ return null;
+ }
+
+ ///
+ /// Returns if a shape can merge.
+ ///
+ /// Shape to check merging.
+ /// Whether the merge can be done.
+ public bool CanMerge(UnorderedShape shape)
+ {
+ 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 shape.
+ ///
+ /// Shape to merge.
+ public void Merge(UnorderedShape shape)
+ {
+ foreach (var edge in shape.Edges)
+ {
+ if (this.Edges.Contains(edge))
+ {
+ this.Edges.Remove(edge);
+ }
+ else
+ {
+ this.Edges.Add(edge);
+ }
+ }
+ }
+
+ ///
+ /// Returns a list of ordered shapes for the current shape.
+ ///
+ /// Ordered shapes from the current edges.
+ public List GetOrderedShapes()
+ {
+ // Iterate over the edges.
+ var orderedShapes = 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.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);
+
+ // 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));
+ }
+ orderedShapes.Add(new OrderedShape()
+ {
+ Points = newPoints,
+ });
+ currentPoints.RemoveRange(startIndex, currentPoints.Count - startIndex);
+ }
+
+ // Add the point.
+ currentPoints.Add(nextPoint);
+ }
+ }
+
+ // Return the ordered shapes.
+ return orderedShapes;
+ }
+}
\ 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..8f2722bf
--- /dev/null
+++ b/Uchu.NavMesh/Uchu.NavMesh.csproj
@@ -0,0 +1,13 @@
+
+
+
+ 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