/// <summary> /// Gets the shortest path from the starting Node to the ending Node. /// </summary> /// <returns>The shortest path.</returns> /// <param name="start">Start Node.</param> /// <param name="end">End Node.</param> public virtual FindPath GetShortestPath(Node start, Node end) { // We don't accept null arguments if (start == null || end == null) { throw new ArgumentNullException(); } // The final path FindPath path = new FindPath(); // If the start and end are same node, we can return the start node if (start == end) { path.nodes.Add(start); return(path); } // The list of unvisited nodes List <Node> unvisited = new List <Node>(); // Previous nodes in optimal path from source Dictionary <Node, Node> previous = new Dictionary <Node, Node>(); // The calculated distances, set all to Infinity at start, except the start Node Dictionary <Node, float> distances = new Dictionary <Node, float>(); for (int i = 0; i < m_Nodes.Count; i++) { Node node = m_Nodes[i]; unvisited.Add(node); // Setting the node distance to Infinity distances.Add(node, float.MaxValue); } // Set the starting Node distance to zero distances[start] = 0f; while (unvisited.Count != 0) { // Ordering the unvisited list by distance, smallest distance at start and largest at end unvisited = unvisited.OrderBy(node => distances[node]).ToList(); // Getting the Node with smallest distance Node current = unvisited[0]; // Remove the current node from unvisisted list unvisited.Remove(current); // When the current node is equal to the end node, then we can break and return the path if (current == end) { // Construct the shortest path while (previous.ContainsKey(current)) { // Insert the node onto the final result path.nodes.Insert(0, current); // Traverse from start to end current = previous[current]; } // Insert the source onto the final result path.nodes.Insert(0, current); break; } // Looping through the Node connections (neighbors) and where the connection (neighbor) is available at unvisited list for (int i = 0; i < current.connections.Count; i++) { Node neighbor = current.connections[i]; // Getting the distance between the current node and the connection (neighbor) float length = Vector3.Distance(current.transform.position, neighbor.transform.position); // The distance from start node to this connection (neighbor) of current node float alt = distances[current] + length; // A shorter path to the connection (neighbor) has been found if (alt < distances[neighbor]) { distances[neighbor] = alt; previous[neighbor] = current; } } } path.Bake(); return(path); }