public void ComputeNavmesh() { if (!meshFilter) { return; } GameObject[] obstacles = GameObject.FindGameObjectsWithTag("NavMeshObstacle"); Mesh mesh = meshFilter.mesh; Matrix4x4 matrix = meshFilter.transform.localToWorldMatrix; ClearNavmesh(); Debug.Log("Begin"); float cosSlope = Mathf.Cos(slopeAngle * Mathf.Deg2Rad); for (int i = 0; i < mesh.triangles.Length; i += 3) { int idxTri0 = mesh.triangles[i]; int idxTri1 = mesh.triangles[i + 1]; int idxTri2 = mesh.triangles[i + 2]; Vertex v0 = new Vertex(idxTri0, matrix.MultiplyPoint3x4(mesh.vertices[idxTri0]), matrix.MultiplyVector(mesh.normals[idxTri0])); Vertex v1 = new Vertex(idxTri1, matrix.MultiplyPoint3x4(mesh.vertices[idxTri1]), matrix.MultiplyVector(mesh.normals[idxTri1])); Vertex v2 = new Vertex(idxTri2, matrix.MultiplyPoint3x4(mesh.vertices[idxTri2]), matrix.MultiplyVector(mesh.normals[idxTri2])); //Merge vertices that are too close foreach (KeyValuePair <int, Vertex> vertexPair in uniqueVertices) { if (Vector3.Distance(v0.position, vertexPair.Value.position) <= 0.25f) { idxTri0 = vertexPair.Key; v0 = vertexPair.Value; } else if (Vector3.Distance(v1.position, vertexPair.Value.position) <= 0.25f) { idxTri1 = vertexPair.Key; v1 = vertexPair.Value; } else if (Vector3.Distance(v2.position, vertexPair.Value.position) <= 0.25f) { idxTri2 = vertexPair.Key; v2 = vertexPair.Value; } } v0.position += v0.normal.normalized * meshHeight; v1.position += v1.normal.normalized * meshHeight; v2.position += v2.normal.normalized * meshHeight; Triangle triangle = new Triangle(v0, v1, v2); if (Vector3.Dot(Vector3.up, triangle.normal) >= cosSlope) { allTriangles.Add(new Triangle(v0, v1, v2)); if (!uniqueVertices.ContainsKey(idxTri0)) { uniqueVertices.Add(idxTri0, v0); } if (!uniqueVertices.ContainsKey(idxTri1)) { uniqueVertices.Add(idxTri1, v1); } if (!uniqueVertices.ContainsKey(idxTri2)) { uniqueVertices.Add(idxTri2, v2); } if (!uniqueVertices[idxTri0].neighbours.ContainsKey(idxTri1)) { uniqueVertices[idxTri0].neighbours.Add(idxTri1, v1); } if (!uniqueVertices[idxTri0].neighbours.ContainsKey(idxTri2)) { uniqueVertices[idxTri0].neighbours.Add(idxTri2, v2); } if (!uniqueVertices[idxTri1].neighbours.ContainsKey(idxTri0)) { uniqueVertices[idxTri1].neighbours.Add(idxTri0, v0); } if (!uniqueVertices[idxTri1].neighbours.ContainsKey(idxTri2)) { uniqueVertices[idxTri1].neighbours.Add(idxTri2, v2); } if (!uniqueVertices[idxTri2].neighbours.ContainsKey(idxTri1)) { uniqueVertices[idxTri2].neighbours.Add(idxTri1, v1); } if (!uniqueVertices[idxTri2].neighbours.ContainsKey(idxTri0)) { uniqueVertices[idxTri2].neighbours.Add(idxTri0, v0); } } } Debug.Log("Identify Key Points"); List <int> keyIds = new List <int>(); foreach (KeyValuePair <int, Vertex> pair in uniqueVertices) { Vertex vertex = pair.Value; List <int> invalidId = new List <int>(); foreach (KeyValuePair <int, Vertex> neighbourPair in vertex.neighbours) { Vertex neighbour = neighbourPair.Value; Ray ray = new Ray(vertex.position, (neighbour.position - vertex.position).normalized); float dist = (neighbour.position - vertex.position).magnitude; RaycastHit hitInfo; foreach (GameObject obstacle in obstacles) { Collider collider = obstacle.GetComponent <Collider>(); if (!collider) { continue; } if (collider.ClosestPoint(neighbour.position) == neighbour.position) { invalidId.Add(neighbour.id); keyIds.Add(pair.Key); } else if (collider.Raycast(ray, out hitInfo, dist)) { invalidId.Add(neighbour.id); keyIds.Add(pair.Key); keyIds.Add(neighbourPair.Key); } } } foreach (int id in invalidId) { vertex.neighbours.Remove(id); } } foreach (int id in keyIds) { Vertex vert = uniqueVertices[id]; vert.isKey = true; uniqueVertices[id] = vert; } Debug.Log("Group Vertices"); for (int i = 0; i < allTriangles.Count; ++i) { Triangle triangle = allTriangles[i]; Polygon polygon = new Polygon(triangle); for (int j = allTriangles.Count - 1; j > i; --j) { Triangle neighbour = allTriangles[j]; if (Vector3.Dot(polygon.normal, neighbour.normal) == 1.0f) { if (polygon.IsConnected(neighbour)) { polygon.AddTriangle(neighbour); allTriangles.RemoveAt(j); } } } allPolygons.Add(polygon); } Debug.Log("Group Polygons"); bool change = false; do { change = false; for (int i = 0; i < allPolygons.Count; ++i) { Polygon polygon = allPolygons[i]; for (int j = allPolygons.Count - 1; j > i; --j) { Polygon neighbour = allPolygons[j]; if (Vector3.Dot(polygon.normal, neighbour.normal) == 1.0f) { if (polygon.IsConnected(neighbour)) { polygon.AddPolygon(neighbour); allPolygons.RemoveAt(j); change = true; } } } } }while (change); Debug.Log("Remove redundant vertices"); List <int> idsRemove = new List <int>(); foreach (KeyValuePair <int, Vertex> vertex in uniqueVertices) { int multipier = 0; foreach (Polygon polygon in allPolygons) { if (polygon.IsConnected(vertex.Value)) { ++multipier; } } if (vertex.Value.neighbours.Count > 3 * multipier && !vertex.Value.isKey) { idsRemove.Add(vertex.Key); } } foreach (KeyValuePair <int, Vertex> vertex in uniqueVertices) { if (idsRemove.Contains(vertex.Key)) { continue; } foreach (int id in idsRemove) { if (vertex.Value.neighbours.ContainsKey(id)) { vertex.Value.neighbours.Remove(id); } } } foreach (int id in idsRemove) { uniqueVertices.Remove(id); } //Merge close vertices /*foreach (KeyValuePair<int, Vertex> vertex1 in uniqueVertices) * { * foreach (KeyValuePair<int, Vertex> vertex2 in uniqueVertices) * { * if (vertex1.Key == vertex2.Key) continue; * } * }*/ for (int i = allPolygons.Count - 1; i >= 0; --i) { for (int j = allPolygons[i].vertices.Count - 1; j >= 0; --j) { if (!uniqueVertices.ContainsKey(allPolygons[i].vertices[j].id)) { allPolygons[i].vertices.RemoveAt(j); } } if (allPolygons[i].vertices.Count < 3) { allPolygons.RemoveAt(i); } else { for (int j = 0; j < allPolygons[i].vertices.Count - 1; ++j) { int id = allPolygons[i].vertices[j].id; uniqueVertices[id].neighbours.Clear(); } } } Debug.Log("Gen Navmesh Triangles"); for (int i = allPolygons.Count - 1; i >= 0; --i) { List <Vertex> temp = allPolygons[i].vertices; Triangulate(temp, ref obstacles); } foreach (KeyValuePair <int, NavMeshVertex> navVertexPair in uniqueNavmeshVertices) { Vertex correspondance = uniqueVertices[navVertexPair.Value.id]; foreach (KeyValuePair <int, Vertex> vertexPair in correspondance.neighbours) { navVertexPair.Value.neighbours.Add(uniqueNavmeshVertices[vertexPair.Value.id]); } } for (int i = 0; i < allNavMeshTriangles.Count; ++i) { for (int j = i + 1; j < allNavMeshTriangles.Count; ++j) { if (allNavMeshTriangles[i].IsAdjacent(allNavMeshTriangles[j])) { allNavMeshTriangles[i].AddNeighbour(allNavMeshTriangles[j]); allNavMeshTriangles[j].AddNeighbour(allNavMeshTriangles[i]); } } } }
public static void Perturb(ref Triangle t1, ref Triangle t2) { List <int> shared = t1.SharedIndices(t2); int t1ExclusiveIndex = t1.indices.FindIndex(i => !shared.Contains(i)); int t2ExclusiveIndex = t2.indices.FindIndex(i => !shared.Contains(i)); int t1SwapIndex = t1.indices.FindIndex(i => i == shared[0]); int t2SwapIndex = t2.indices.FindIndex(i => i == shared[1]); int t1Exclusive = t1[t1ExclusiveIndex]; int t2Exclusive = t2[t2ExclusiveIndex]; int t1Swap = t1.indices[t1SwapIndex]; int t2Swap = t2.indices[t2SwapIndex]; // Find Polygons to be affected Polygon t1ExcPoly = t1.polygons.Find(p => p.index == t1Exclusive); Polygon t2ExcPoly = t2.polygons.Find(p => p.index == t2Exclusive); Polygon t1SidePoly = t1.polygons.Find(p => p.index == t1Swap); Polygon t2SidePoly = t2.polygons.Find(p => p.index == t2Swap); // Check For Outcome if ( (t1ExcPoly != null && t1ExcPoly.Triangles.Count == 7) || (t2ExcPoly != null && t2ExcPoly.Triangles.Count == 7) || (t1SidePoly != null && t1SidePoly.Triangles.Count == 5) || (t2SidePoly != null && t2SidePoly.Triangles.Count == 5)) { return; } // Apply Changes To Polygons if (t1ExcPoly != null) { t1ExcPoly.AddTriangle(t2); } if (t2ExcPoly != null) { t2ExcPoly.AddTriangle(t1); } if (t1SidePoly != null) { t1SidePoly.RemoveTriangle(t1); } if (t2SidePoly != null) { t2SidePoly.RemoveTriangle(t2); } //Continue Perturbing t1.indices[t1SwapIndex] = t2Exclusive; t2.indices[t2SwapIndex] = t1Exclusive; // Apply Neighbour Changes Triangle t1Temp = t1; Triangle t2Temp = t2; int t1OldNeighbourIndex = t1.neighbours.FindIndex(n => (!n.Equals(t2Temp) && n.indices.Contains(shared[0]))); int t2OldNeighbourIndex = t2.neighbours.FindIndex(n => (!n.Equals(t1Temp) && n.indices.Contains(shared[1]))); Triangle t1OldNeighbour = t1.neighbours[t1OldNeighbourIndex]; Triangle t2OldNeighbour = t2.neighbours[t2OldNeighbourIndex]; t1.neighbours[t1OldNeighbourIndex] = t2OldNeighbour; t2.neighbours[t2OldNeighbourIndex] = t1OldNeighbour; int t1IndexOnNeighbour = t1OldNeighbour.neighbours.FindIndex(n => n.Equals(t1Temp)); int t2IndexOnNeighbour = t2OldNeighbour.neighbours.FindIndex(n => n.Equals(t2Temp)); t1OldNeighbour.neighbours[t1IndexOnNeighbour] = t2; t2OldNeighbour.neighbours[t2IndexOnNeighbour] = t1; }