/// <summary> /// Computes the path from => to. /// </summary> /// <param name="from">Start node.</param> /// <param name="to">end node.</param> /// <returns>Path from start to end.</returns> public static List <Node> Compute(Node from, Node to) { var done = new List <Node>(); // A priority queue that will sort our nodes based on the total cost estimate var open = new PriorityQueue <Node>(); foreach (var node in from.ConnectedNodes) { // Add connecting nodes if traversable if (node.Traversable) { // Calculate the Costs node.CurrentCost = from.CurrentCost + from.DistanceTo(node) * node.TraversalCostMultiplier; node.EstimatedCost = from.CurrentCost + node.DistanceTo(to); // Enqueue open.Enqueue(node); } } while (true) { // End Condition( Path not found ) if (open.Count == 0) { ResetNodes(done); ResetNodes(open.GetData()); return(new List <Node>()); } // Selecting next Element from queue var current = open.Dequeue(); // Add it to the done list done.Add(current); current.State = NodeState.CLOSED; // EndCondition( Path was found ) if (current == to) { var ret = GeneratePath(to); // Create the Path // Reset all Nodes that were used. ResetNodes(done); ResetNodes(open.GetData()); return(ret); } else { AddOrUpdateConnected(current, to, open); } } }
private static void AddOrUpdateConnected(Node current, Node to, PriorityQueue <Node> queue) { foreach (var connected in current.ConnectedNodes) { if (!connected.Traversable || connected.State == NodeState.CLOSED) { continue; // Do ignore already checked and not traversable nodes. } // Adds a previously not "seen" node into the Queue if (connected.State == NodeState.UNCONSIDERED) { connected.Parent = current; connected.CurrentCost = current.CurrentCost + current.DistanceTo(connected) * connected.TraversalCostMultiplier; connected.EstimatedCost = connected.CurrentCost + connected.DistanceTo(to); connected.State = NodeState.OPEN; queue.Enqueue(connected); } else if (current != connected) { // Updating the cost of the node if the current way is cheaper than the previous var newCCost = current.CurrentCost + current.DistanceTo(connected); var newTCost = newCCost + current.EstimatedCost; if (newTCost < connected.TotalCost) { connected.Parent = current; connected.CurrentCost = newCCost; } } else { // Codacy made me do it. throw new PathfindingException("Detected the same node twice. Confusion how this could ever happen"); } } }