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;
}
