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 NavmeshTriangle(NavMeshVertex a, NavMeshVertex b, NavMeshVertex c)
{
v0 = a;
v1 = b;
v2 = c;
normal = Vector3.zero;
neighbours = new List <NavmeshTriangle>();
}
// pre calculate path
// find neighbour
// profit
void Start()
{
vertices = new List <NavMeshVertex>();
NavMeshTriangulation nmt = NavMesh.CalculateTriangulation();
foreach (Vector3 vec in nmt.vertices)
{
bool isAdd = true;
foreach (var vtx in vertices)
{
if (vtx.point == vec)
{
isAdd = false;
break;
}
}
if (isAdd)
{
NavMeshVertex toAdd = new NavMeshVertex();
toAdd.point = vec;
vertices.Add(toAdd);
}
}
Debug.Log("Vertex count: " + vertices.Count);
foreach (NavMeshVertex vtx in vertices)
{
//vtx.neighbours = new List<NavMeshVertex>();
foreach (NavMeshVertex neighbour in vertices)
{
if (vtx.point == neighbour.point)
{
continue;
}
Vector3 dir = (neighbour.point - vtx.point);
float dist = Vector3.Distance(vtx.point, neighbour.point);
if (!Physics.Raycast(vtx.point, dir.normalized, dist))
{
vtx.neighbours.Add(neighbour);
vtx.cost.Add(dist);
}
}
}
}
public bool GetPath(NavMeshVertex origin, NavMeshVertex goal, out List <Vector3> path)
{
path = new List <Vector3>();
List <NavMeshVertex> openList = new List <NavMeshVertex>();
origin.totalCost = 0.0f;
openList.Add(origin);
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;
}
}
var currPoint = openList[cheapestIndex];
openList.RemoveAt(cheapestIndex);
// Reached
if (currPoint == goal)
{
while (currPoint != origin)
{
path.Insert(0, currPoint.point);
currPoint = currPoint.parent;
}
path.Insert(0, origin.point);
Debug.Log(path.Count);
foreach (var vtx in vertices)
{
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 + currPoint.cost[i];
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]);
}
}
currPoint.isVisited = true;
}
foreach (var vtx in vertices)
{
vtx.Reset();
}
return(false);
}
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 bool IsConnected(NavMeshVertex v)
{
return(v0.id == v.id || v1.id == v.id || v2.id == v.id);
}
public void Load(string onvfile)
{
Name = Path.GetFileNameWithoutExtension(onvfile).ToLowerInvariant();
var lines = File.ReadAllLines(onvfile);
bool inverts = false;
bool ininds = false;
bool inedges = false;
bool inpolys = false;
bool insectors = false;
bool insectordata = false;
bool insectorpolyinds = false;
bool insectorbounds = false;
bool inportals = false;
int depth = 0;
int cdepth = 0;
OnvSector csector = SectorTree;
var spacedelim = new[] { ' ' };
var cult = CultureInfo.InvariantCulture;
foreach (var line in lines)
{
string tline = line.Trim();
if (string.IsNullOrEmpty(tline))
{
continue; //blank line
}
//if (tline.StartsWith("#")) continue; //commented out
string[] parts = tline.Split(spacedelim, StringSplitOptions.RemoveEmptyEntries);
if (tline.StartsWith("{"))
{
depth++; continue;
}
if (tline.StartsWith("}"))
{
depth--;
} //need to handle the closing cases
if (inverts)
{
if (depth <= 0)
{
inverts = false;
}
else if (parts.Length == 3)
{
NavMeshVertex v = new NavMeshVertex();
v.X = ushort.Parse(parts[0].Trim(), cult);
v.Y = ushort.Parse(parts[1].Trim(), cult);
v.Z = ushort.Parse(parts[2].Trim(), cult);
Vertices.Add(v);
}
}
else if (ininds)
{
if (depth <= 0)
{
ininds = false;
}
else
{
for (int i = 0; i < parts.Length; i++)
{
int ind = int.Parse(parts[i]);
Indices.Add(ind);
}
}
}
else if (inedges)
{
if (depth <= 0)
{
inedges = false;
}
else
{
OnvEdge edge = new OnvEdge(parts);
Edges.Add(edge);
}
}
else if (inpolys)
{
if (depth <= 0)
{
inpolys = false;
}
else
{
OnvPoly poly = new OnvPoly(parts);
Polys.Add(poly);
}
}
else if (insectors)
{
if (depth <= 0)
{
insectors = false;
insectordata = false;
insectorpolyinds = false;
insectorbounds = false;
}
else if (insectordata)
{
if (depth <= cdepth)
{
insectordata = false;
insectorpolyinds = false;
insectorbounds = false;
}
else if (insectorpolyinds)
{
if (depth <= (cdepth + 1))
{
insectorpolyinds = false;
}
else
{
for (int i = 0; i < parts.Length; i++)
{
int ind = int.Parse(parts[i]);
csector.SectorData.PolyIndices.Add(ind);
}
}
}
else if (insectorbounds)
{
if (depth <= (cdepth + 1))
{
insectorbounds = false;
}
else
{
OnvBounds bounds = new OnvBounds(parts);
csector.SectorData.Bounds.Add(bounds);
}
}
else
{
string idstr = parts[0].Trim();
if (idstr == "PolyIndices")
{
csector.SectorData.PolyIndicesCount = int.Parse(parts[1].Trim(), cult);
insectorpolyinds = csector.SectorData.PolyIndicesCount > 0;
if (insectorpolyinds)
{
csector.SectorData.PolyIndices = new List <int>();
}
}
else if (idstr == "Bounds")
{
csector.SectorData.BoundsCount = int.Parse(parts[1].Trim(), cult);
insectorbounds = csector.SectorData.BoundsCount > 0;
if (insectorbounds)
{
csector.SectorData.Bounds = new List <OnvBounds>();
}
}
}
}
else
{
if (depth < cdepth)
{
csector = csector.Parent;
}
cdepth = depth;
string idstr = parts[0].Trim();
if (idstr == "AABBMin")
{
csector.AABBMin = Util.GetVector3(parts, 1);
}
else if (idstr == "AABBMax")
{
csector.AABBMax = Util.GetVector3(parts, 1);
}
else if ((parts.Length < 2) || (parts[1].Trim() != "null"))
{
if (idstr == "SectorData")
{
csector.SectorData = new OnvSectorData();
insectordata = true;
}
else if (idstr == "SubTree0")
{
csector.SubTree0 = new OnvSector();
csector.SubTree0.Parent = csector;
csector = csector.SubTree0;
}
else if (idstr == "SubTree1")
{
csector.SubTree1 = new OnvSector();
csector.SubTree1.Parent = csector;
csector = csector.SubTree1;
}
else if (idstr == "SubTree2")
{
csector.SubTree2 = new OnvSector();
csector.SubTree2.Parent = csector;
csector = csector.SubTree2;
}
else if (idstr == "SubTree3")
{
csector.SubTree3 = new OnvSector();
csector.SubTree3.Parent = csector;
csector = csector.SubTree3;
}
}
}
}
else if (inportals)
{
if (depth <= 0)
{
inportals = false;
}
else
{
OnvPortal portal = new OnvPortal(parts);
Portals.Add(portal);
}
}
else
{
//at root level, look for identifier
depth = 0; //reset just in case
string idstr = parts[0].Trim();
if (idstr == "Version")
{
VersionMaj = int.Parse(parts[1].Trim(), cult);
VersionMin = int.Parse(parts[2].Trim(), cult);
}
else if (idstr == "Sizes")
{
Sizes = Util.GetVector3(parts, 1);
}
else if (idstr == "Flags")
{
Flags = int.Parse(parts[1].Trim(), cult);
}
else if (idstr == "Vertices")
{
VerticesCount = int.Parse(parts[1].Trim(), cult);
inverts = VerticesCount > 0;
}
else if (idstr == "Indices")
{
IndicesCount = int.Parse(parts[1].Trim(), cult);
ininds = IndicesCount > 0;
}
else if (idstr == "Edges")
{
EdgesCount = int.Parse(parts[1].Trim(), cult);
inedges = EdgesCount > 0;
}
else if (idstr == "Polys")
{
PolysCount = int.Parse(parts[1].Trim(), cult);
inpolys = PolysCount > 0;
}
else if (idstr == "SectorTree")
{
insectors = true;
}
else if (idstr == "Portals")
{
PortalsCount = int.Parse(parts[1].Trim(), cult);
inportals = PortalsCount > 0;
}
else if (idstr == "SectorID")
{
SectorID = int.Parse(parts[1].Trim(), cult);
}
}
}
}
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);
}
