Navigation.findPath C# (CSharp) Method

findPath() public method

public findPath ( Vector3 start, Vector3 end ) : List
start Vector3
end Vector3
return List
    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);
    }