/// <summary> /// Method that takes a node and inserts it into the given list in greatest /// to least order. Insertion inserts in order of greatest f-cost first, /// then greatest h-cost as a tie breaker. /// </summary> /// <param name="list">List into which to insert the node.</param> /// <param name="node">The node to insert into the list.</param> private void sortedAdd(LinkedList <VertexNode> list, VertexNode node) { // Obtain the first node, since have to traverse the list from the start. LinkedListNode <VertexNode> currentNode = list.First; // Can just insert if the list is empty. if (list.Count == 0) { list.AddFirst(node); return; } // Insert into a non-empty list. while (true) { // When the fcosts are equal, have to check hcost to determine insertion position. if (currentNode.Value.fCost == node.fCost) { if (node.hCost >= currentNode.Value.hCost) { list.AddBefore(currentNode, node); break; } } if (node.fCost > currentNode.Value.fCost) { list.AddBefore(currentNode, node); break; } if (currentNode.Next == null) { list.AddAfter(currentNode, node); break; } // If it didn't break by this point, move onto the next list element. currentNode = currentNode.Next; } }
/// <summary> /// Method used to determine the shortest path to the destination /// point from the start point using the A* algorithm. /// </summary> /// <param name="start">The point at which to start the pathfinding.</param> /// <param name="destination">The target point of the pathfinding.</param> /// <returns>Stack containing the vertices that create the shortest path, with the top as the start point.</returns> public Stack <Vertex> findPath(Vertex start, Vertex destination) { Stack <Vertex> path = new Stack <Vertex>(); // Return an empty path when the start and destination are the same. if (start.Equals(destination)) { return(path); } // Create the set of open vertices. LinkedList <VertexNode> open = new LinkedList <VertexNode>(); // Add starting node to the list, and set its parent to null. open.AddFirst(new VertexNode(start, 0, destination.getDistanceTo(start))); open.First.Value.setLinkToPrevious(null); // Create the set of closed vertices. LinkedList <VertexNode> closed = new LinkedList <VertexNode>(); // Create and initialize the vertex that's currently being checked. LinkedListNode <VertexNode> currentNode = open.Last; // G-cost of a neighboring vertex. double cost; // Filled if a node already exists in the open or closed sets. // Should theoretically never be filled together. LinkedListNode <VertexNode> openTemp, closedTemp; // Make sure that the currently checked node is not the destination node. while (!currentNode.Value.associatedLocation.Equals(destination)) { // Remove the node with the smallest f-cost from open and add it to closed. open.RemoveLast(); closed.AddFirst(currentNode); // A node should be placed in open when: // A) It hasn't been visited yet. (Neither in open nor closed.) // B) It's already in open, but the gCost is smaller than that of the existing node in the set. // C) It's in the closed set, but the gCost is better than the original visit. foreach (Vertex neighbor in currentNode.Value.associatedLocation.neighborhood) { // Neighboring vertices that are unnavigable are useless. if (neighbor.toIgnore) { continue; } // Calculate the g-cost of this neighbor and prepare it for use in the lists. cost = currentNode.Value.gCost + currentNode.Value.associatedLocation.getDistanceTo(neighbor); VertexNode neighborNode = new VertexNode(neighbor); // Obtain the equivalent node in open, or null if it's not in the set. openTemp = open.Find(neighborNode); // Check if the node's position shoud change in the list. if (openTemp != null && openTemp.Value.gCost > cost) { open.Remove(openTemp); } // Check if the node exists in closed and should be placed in the open set again. closedTemp = closed.Find(neighborNode); if (closedTemp != null && closedTemp.Value.gCost > cost) { closed.Remove(closedTemp); } // Add the node to the open set if doesn't exist in there. if (!open.Contains(neighborNode) && !closed.Contains(neighborNode)) { // If value already existed in one of sets, replace node with the node // from the set and update the gCost. if (openTemp != null) { neighborNode = openTemp.Value; neighborNode.changeGCost(cost); } else if (closedTemp != null) { neighborNode = closedTemp.Value; neighborNode.changeGCost(cost); } // The node didn't already exist, so need to instantiate the node with new values. else { neighborNode.setCosts(cost, destination.getDistanceTo(neighborNode.associatedLocation)); } // Set this node to the previous node, then add the neighbor to the open set. neighborNode.setLinkToPrevious(currentNode.Value); sortedAdd(open, neighborNode); } } // Set current to the node with the smallest fcost/gcost. currentNode = open.Last; } // A path has been found. Take the associated VertexNode from the LinkedListNode. VertexNode node = currentNode.Value; // Use the parents to rebuild the path back to the start point. while (node != null) { path.Push(node.associatedLocation); node = node.previousOnPath; } // DEBUG: Uncomment if code not working as expected. // Debug.Log("TOTAL NODES PASSED: " + path.Count); return(path); }