bool CalculatePath(NavNode source, NavNode target, out List <NavNode> outPath) { List <NavNode> openNodes = new List <NavNode>(); HashSet <NavNode> closedNodes = new HashSet <NavNode>(); NavNode[] cameFromTable = new NavNode[nodes.Count]; // Add the source node source.heuristicCost = useDijkstra ? 0 : target.Distance(source); source.travelCost = 0; openNodes.Add(source); // Search for all avaliable paths while (openNodes.Count != 0) { // Find lowest cost open node NavNode current = openNodes[0]; for (int i = 1; i < openNodes.Count; ++i) { float a = openNodes[i].totalCost; float b = current.totalCost; if (a < b || (a == b && openNodes[i].travelCost < current.travelCost)) { current = openNodes[i]; } } // Lock current node's states openNodes.Remove(current); closedNodes.Add(current); // Reached target if (current == target) { // Reconstruct path in reverse then flip outPath = new List <NavNode>(); while (current != null) { outPath.Add(current); current = cameFromTable[current.id]; } outPath.Reverse(); return(true); } // Add neighbours foreach (int neighbourId in current.neighbours) { NavNode neighbour = nodes[neighbourId]; // Ignore locked nodes if (closedNodes.Contains(neighbour)) { continue; } // Check to see if this path is faster for open nodes if (openNodes.Contains(neighbour)) { float h = useDijkstra ? 0 : target.Distance(neighbour); float g = current.Distance(neighbour) + current.totalCost; // New route is better if (neighbour.totalCost > h + g) { neighbour.heuristicCost = h; neighbour.travelCost = g; openNodes.Add(neighbour); cameFromTable[neighbour.id] = current; } } // Not considered yet else { // Workout it's values neighbour.heuristicCost = useDijkstra ? 0 : target.Distance(neighbour); neighbour.travelCost = current.Distance(neighbour) + current.totalCost; openNodes.Add(neighbour); cameFromTable[neighbour.id] = current; } } } // Has failed to reach target if reached here outPath = null; return(false); }