public List<Vector3> findPath(Vector3 start, Vector3 end)
{
List<Vertex> allVertices = buildGraph(start, end);//start and end
List<Vertex> priorityVertices = new List<Vertex>();
Vertex currentVertex = null;
startVertex.distTo = 0;
priorityVertices.Add (startVertex);
while (priorityVertices.Count != 0) { //pop off the element with the smallest priority
currentVertex = priorityVertices[0];
priorityVertices.RemoveAt(0);
foreach (Vertex vert in currentVertex.adj) { //get all adjacent vertices
if (!vert.hasTraversed) { //distance along terrain I can make more advanced,
//In the future, I could precompute all the distances between
//adjacent waypoints and store them at the start or index them
if (vert.distTo > currentVertex.distTo + Vector3.Distance(currentVertex.myvector, vert.myvector)) {
vert.distTo = currentVertex.distTo + Vector3.Distance(currentVertex.myvector, vert.myvector);
vert.prev = currentVertex;
priorityVertices = enqueue(priorityVertices, vert); //add to queue
}
}
}
currentVertex.hasTraversed = true;
}
return finalPath (endVertex);
}