public NavMeshVertex GetClosestVertex(int index, Vector3 pos)
{
NavMeshVertex retVertex = null;
float shortestDist = Mathf.Infinity;
if (index < 0)
{
// Check through all triangles
foreach (var tri in nmg.allNavMeshTriangles)
{
float d1 = Vector3.Distance(tri.v0.point, pos);
float d2 = Vector3.Distance(tri.v1.point, pos);
float d3 = Vector3.Distance(tri.v2.point, pos);
if (d1 < shortestDist)
{
shortestDist = d1;
retVertex = tri.v0;
}
if (d2 < shortestDist)
{
shortestDist = d2;
retVertex = tri.v1;
}
if (d3 < shortestDist)
{
shortestDist = d3;
retVertex = tri.v2;
}
}
}
else
{
NavmeshTriangle tri = nmg.allNavMeshTriangles[index];
float d1 = Vector3.Distance(tri.v0.point, pos);
float d2 = Vector3.Distance(tri.v1.point, pos);
float d3 = Vector3.Distance(tri.v2.point, pos);
shortestDist = d1;
retVertex = tri.v0;
if (d2 < shortestDist)
{
shortestDist = d2;
retVertex = tri.v1;
}
if (d3 < shortestDist)
{
shortestDist = d3;
retVertex = tri.v2;
}
}
return(retVertex);
}
public bool IsAdjacent(NavmeshTriangle triangle)
{
int commonEdge = 0;
if (v0.id == triangle.v0.id || v0.id == triangle.v1.id || v0.id == triangle.v2.id)
{
++commonEdge;
}
if (v1.id == triangle.v0.id || v1.id == triangle.v1.id || v1.id == triangle.v2.id)
{
++commonEdge;
}
if (v2.id == triangle.v0.id || v2.id == triangle.v1.id || v2.id == triangle.v2.id)
{
++commonEdge;
}
return(commonEdge == 2);
}
public int GetClosestTriangleIndex(Vector3 point)
{
int index = -1;
float shortestDistance = Mathf.Infinity;
for (int i = 0; i < nmg.allNavMeshTriangles.Count; ++i)
{
NavmeshTriangle tri = nmg.allNavMeshTriangles[i];
Vector3 midPoint = (tri.v0.point + tri.v1.point + tri.v2.point) / 3;
float dist = Vector3.Distance(midPoint, point);
if (dist < shortestDistance)
{
shortestDistance = dist;
index = i;
}
}
return(index);
}
void Triangulate(List <Vertex> vertices, ref GameObject[] obstacles)
{
vertices.Sort((a, b) => a.position.x.CompareTo(b.position.x));
for (int i = 0; i < vertices.Count; ++i)
{
for (int j = i + 1; j < vertices.Count; ++j)
{
for (int k = j + 1; (k % vertices.Count) != i; ++k)
{
Vertex v0 = vertices[i];
Vertex v1 = vertices[j % vertices.Count];
Vertex v2 = vertices[k % vertices.Count];
int idxTri0 = v0.id;
int idxTri1 = v1.id;
int idxTri2 = v2.id;
Ray ray0 = new Ray(v0.position, (v1.position - v0.position).normalized);
Ray ray1 = new Ray(v1.position, (v2.position - v1.position).normalized);
Ray ray2 = new Ray(v2.position, (v0.position - v2.position).normalized);
float dist0 = (v1.position - v0.position).magnitude;
float dist1 = (v2.position - v1.position).magnitude;
float dist2 = (v0.position - v2.position).magnitude;
RaycastHit hitInfo;
bool skip = false;
foreach (GameObject obstacle in obstacles)
{
Collider collider = obstacle.GetComponent <Collider>();
if (!collider)
{
continue;
}
if (collider.Raycast(ray0, out hitInfo, dist0) ||
collider.Raycast(ray1, out hitInfo, dist1) ||
collider.Raycast(ray2, out hitInfo, dist2))
{
skip = true;
break;
}
else
{
Vector3 p0 = collider.ClosestPoint(v0.position);
Vector3 p1 = collider.ClosestPoint(v1.position);
Vector3 p2 = collider.ClosestPoint(v2.position);
if (IsPointInTriangle(v0.position, v1.position, v2.position, p0) ||
IsPointInTriangle(v0.position, v1.position, v2.position, p1) ||
IsPointInTriangle(v0.position, v1.position, v2.position, p2))
{
skip = true;
break;
}
}
}
if (skip)
{
continue;
}
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);
}
NavMeshVertex nv0, nv1, nv2;
if (!uniqueNavmeshVertices.ContainsKey(idxTri0))
{
nv0 = new NavMeshVertex();
nv0.id = v0.id;
nv0.point = v0.position;
nv0.normal = v0.normal;
uniqueNavmeshVertices.Add(idxTri0, nv0);
}
else
{
nv0 = uniqueNavmeshVertices[idxTri0];
}
if (!uniqueNavmeshVertices.ContainsKey(idxTri1))
{
nv1 = new NavMeshVertex();
nv1.id = v1.id;
nv1.point = v1.position;
nv1.normal = v1.normal;
uniqueNavmeshVertices.Add(idxTri1, nv1);
}
else
{
nv1 = uniqueNavmeshVertices[idxTri1];
}
if (!uniqueNavmeshVertices.ContainsKey(idxTri2))
{
nv2 = new NavMeshVertex();
nv2.id = v2.id;
nv2.point = v2.position;
nv2.normal = v2.normal;
uniqueNavmeshVertices.Add(idxTri2, nv2);
}
else
{
nv2 = uniqueNavmeshVertices[idxTri2];
}
NavmeshTriangle triangle = new NavmeshTriangle(nv0, nv1, nv2);
allNavMeshTriangles.Add(triangle);
}
}
}
}
public void AddNeighbour(NavmeshTriangle triangle)
{
neighbours.Add(triangle);
if (triangle.IsConnected(v0))
{
if (v0.id != triangle.v0.id)
{
if (!v0.neighbours.Contains(triangle.v0))
{
v0.neighbours.Add(triangle.v0);
}
}
if (v0.id != triangle.v1.id)
{
if (!v0.neighbours.Contains(triangle.v1))
{
v0.neighbours.Add(triangle.v1);
}
}
if (v0.id != triangle.v2.id)
{
if (!v0.neighbours.Contains(triangle.v2))
{
v0.neighbours.Add(triangle.v2);
}
}
}
if (triangle.IsConnected(v1))
{
if (v1.id != triangle.v0.id)
{
if (!v1.neighbours.Contains(triangle.v0))
{
v1.neighbours.Add(triangle.v0);
}
}
if (v1.id != triangle.v1.id)
{
if (!v1.neighbours.Contains(triangle.v1))
{
v1.neighbours.Add(triangle.v1);
}
}
if (v1.id != triangle.v2.id)
{
if (!v1.neighbours.Contains(triangle.v2))
{
v1.neighbours.Add(triangle.v2);
}
}
}
if (triangle.IsConnected(v2))
{
if (v2.id != triangle.v0.id)
{
if (!v2.neighbours.Contains(triangle.v0))
{
v2.neighbours.Add(triangle.v0);
}
}
if (v2.id != triangle.v1.id)
{
if (!v2.neighbours.Contains(triangle.v1))
{
v2.neighbours.Add(triangle.v1);
}
}
if (v2.id != triangle.v2.id)
{
if (!v2.neighbours.Contains(triangle.v2))
{
v2.neighbours.Add(triangle.v2);
}
}
}
}
public int GetTriangleIndex(Vector3 point)
{
for (int i = 0; i < nmg.allNavMeshTriangles.Count; ++i)
{
NavmeshTriangle tri = nmg.allNavMeshTriangles[i];
Vector3[] vecs = new Vector3[] { tri.v0.point, tri.v1.point, tri.v2.point };
/*if (CheckPointInTriangle(point, vecs, 1.0f))
* {
* return i;
* }*/
// find barycenter
Vector3 midPoint = (tri.v0.point + tri.v1.point + tri.v2.point) / 3;
if (CheckPointInTriangle(midPoint, vecs, 1.0f)) // clockwise
{
if (CheckPointInTriangle(point, vecs, 1.0f))
{
return(i);
}
}
else // anti clockwise
{
if (CheckPointInTriangle(point, vecs, -1.0f))
{
return(i);
}
}
/*
* bool clockWise = false;
*
* // find barycenter
* Vector3 midPoint = (tri.v0.point + tri.v1.point + tri.v2.point) / 3;
*
* // check for outward normal
* Vector3 inward = midPoint - tri.v0.point;
* Vector3 edge = tri.v1.point - tri.v0.point;
* Vector2 edgeNormal = new Vector2(edge.z, -edge.x);
* Vector2 inward2D = new Vector2(inward.x, inward.z);
*
* if (Vector2.Dot(inward2D.normalized, edgeNormal.normalized) > 0.0f)
* {
* clockWise = true;
* }
*
*
* Vector3 diff = tri.v0.point - point;
* Vector3 line = tri.v1.point - tri.v0.point;
*
* Vector2 dir = new Vector2(diff.x, diff.z);
* Vector2 norm = new Vector2(line.z, -line.x);
*
* if (Vector2.Dot(dir.normalized, norm.normalized) < 0.0f)
* continue;
*
* diff = tri.v1.point - point;
* line = tri.v2.point - tri.v1.point;
*
* dir = new Vector2(diff.x, diff.z);
* norm = new Vector2(line.z, -line.x);
*
* if (Vector2.Dot(dir.normalized, norm.normalized) < 0.0f)
* continue;
*
* diff = tri.v2.point - point;
* line = tri.v0.point - tri.v2.point;
*
* dir = new Vector2(diff.x, diff.z);
* norm = new Vector2(line.z, -line.x);
*
* if (Vector2.Dot(dir.normalized, norm.normalized) < 0.0f)
* continue;
*
* return i;
*/
}
return(-1); // Out of bound for all triangles
}
public bool FindPath(Vector3 goal, out List <NavMeshVertex> outPath)
{
outPath = new List <NavMeshVertex>();
foreach (NavmeshTriangle tri in nmg.allNavMeshTriangles)
{
tri.v0.Reset();
tri.v1.Reset();
tri.v2.Reset();
}
// Get origin and goal triangle
int startIndex = GetClosestTriangleIndex(transform.position);//GetTriangleIndex(transform.position);
int goalIndex = GetTriangleIndex(goal);
if (startIndex == -1)
{
Debug.Log("start out of bound");
return(false);
}
else if (goalIndex == -1)
{
Debug.Log("goal out of bound");
return(false);
}
NavmeshTriangle originTri = nmg.allNavMeshTriangles[startIndex];
NavmeshTriangle goalTri = nmg.allNavMeshTriangles[goalIndex];
// If origin and goal share the same triangle
NavMeshVertex v1 = new NavMeshVertex();
NavMeshVertex v2 = new NavMeshVertex();
v1.point = transform.position;
v2.point = goal;
outPath.Add(v1);
outPath.Add(v2);
//if (originTri.v0 == goalTri.v0 && originTri.v1 == goalTri.v1 && originTri.v2 == goalTri.v2)
if (startIndex == goalIndex)
{
Debug.Log("same tri");
return(true);
}
// Calculate origin triangle
NavMeshVertex originTempVertex = new NavMeshVertex();
originTempVertex.point = transform.position;
originTempVertex.parent = null;
originTri.v0.totalCost = Vector3.SqrMagnitude(originTri.v0.point - transform.position);
originTri.v1.totalCost = Vector3.SqrMagnitude(originTri.v1.point - transform.position);
originTri.v2.totalCost = Vector3.SqrMagnitude(originTri.v2.point - transform.position);
originTri.v0.parent = originTempVertex;
originTri.v1.parent = originTempVertex;
originTri.v2.parent = originTempVertex;
// Open list
List <NavMeshVertex> openList = new List <NavMeshVertex>();
openList.Add(originTri.v0);
openList.Add(originTri.v1);
openList.Add(originTri.v2);
// Master list for reset
//List<NavMeshVertex> dumpster = new List<NavMeshVertex>();
//dumpster.AddRange(openList);
while (openList.Count > 0)
{
// Find cheapest node
int cheapestIndex = 0;
for (int i = 1; i < openList.Count; ++i)
{
if (openList[i].totalCost < openList[cheapestIndex].totalCost)
{
cheapestIndex = i;
}
}
NavMeshVertex currPoint = openList[cheapestIndex];
openList.RemoveAt(cheapestIndex);
// Check vertex against goal triangle
if (goalTri.v0 == currPoint || goalTri.v1 == currPoint || goalTri.v2 == currPoint)
{
while (currPoint.parent != null)
{
outPath.Insert(1, currPoint);
currPoint = currPoint.parent;
}
//foreach (NavMeshVertex vtx in dumpster)
// vtx.Reset();
return(true);
}
// Loop neighbours
for (int i = 0; i < currPoint.neighbours.Count; ++i)
{
// Skip visited vertex
if (currPoint.neighbours[i].isVisited)
{
continue;
}
float newCost = currPoint.totalCost + Vector3.SqrMagnitude(currPoint.neighbours[i].point - currPoint.point);
if (newCost < currPoint.neighbours[i].totalCost)
{
currPoint.neighbours[i].totalCost = newCost;
currPoint.neighbours[i].parent = currPoint;
}
if (!openList.Contains(currPoint.neighbours[i]))
{
openList.Add(currPoint.neighbours[i]);
//dumpster.Add(currPoint);
}
}
currPoint.isVisited = true;
}
//foreach (NavMeshVertex vtx in dumpster)
// vtx.Reset();
return(false);
}
