/// <summary> /// Implementation of A* Algorithm /// Takes the start node and goal node as input and returns the node which matches the goal node /// </summary> /// <param name="startNode"></param> /// <param name="goalNode"></param> /// <returns></returns> public Node AStar(Node startNode, Node goalNode) { Node INIT = startNode; //Create a list to track the visited nodes List <Node> visitedNodes = new List <Node>(); //Create a Queue for the open nodes AlgoImpl.PriorityQueue queue = new AlgoImpl.PriorityQueue(); //Add the start node to the queue INIT.Priority = INIT.HCost(goalNode) + INIT.PathCost; queue.Enqueue(INIT); //Check the queue for nodes, if empty then return null while (queue.nodes.Count != 0) { //Get the best node with minimum f value from the queue Node current = queue.Dequeue(); //Add the node to visited nodes visitedNodes.Add(current); //Check if the node is goal node and return the node if true else expand its child nodes if (current.Equals(goalNode)) { return(current); } else { //Increment the expanded nodes global variable ++noOfNodesExpanded; //Generated the successor nodes List <Node> childNodes = current.ChildrenNodes(); //Traverse the nodes foreach (Node successor in childNodes) { int distanceFromNewNode = successor.HCost(goalNode); //Compute the estimated g-cost of the successor int estGCost = current.GCost() + Math.Abs(distanceFromNewNode - current.HCost(goalNode)); int findVisited = FindNode(visitedNodes, successor); //check if the successor is already visited or not, if visted then discard it if (findVisited == -1) { //check if the successor is already in the queue int found = FindNode(queue.nodes, successor); //if true if (found != -1) { //if f cost of old node is greater than f cost of successor node, delete it from the queue //else discard the successor node if ((queue.nodes.ElementAt(found).PathCost > estGCost + distanceFromNewNode)) { queue.Delete(found); } else { continue; } } //Add the node to the queue and set its parent to curren successor.Parent = current; successor.PathCost = estGCost; successor.Priority = successor.PathCost + distanceFromNewNode; queue.Enqueue(successor); ++noOfNodesGenerated; } else { if (estGCost < successor.GCost()) { visitedNodes.RemoveAt(findVisited); continue; } } } } if (queue.nodes.Count == 100000) { break; } } return(null); }
/// <summary> /// Implementation of A* Algorithm /// Takes the start node and goal node as input and returns the node which matches the goal node /// </summary> /// <param name="startNode"></param> /// <param name="goalNode"></param> /// <returns></returns> public Node AStar(Node startNode, Node goalNode) { Node INIT = startNode; //Create a list to track the visited nodes List<Node> visitedNodes = new List<Node>(); //Create a Queue for the open nodes AlgoImpl.PriorityQueue queue = new AlgoImpl.PriorityQueue(); //Add the start node to the queue INIT.Priority = INIT.HCost(goalNode) + INIT.PathCost; queue.Enqueue(INIT); //Check the queue for nodes, if empty then return null while (queue.nodes.Count != 0) { //Get the best node with minimum f value from the queue Node current = queue.Dequeue(); //Add the node to visited nodes visitedNodes.Add(current); //Check if the node is goal node and return the node if true else expand its child nodes if (current.Equals(goalNode)) { return current; } else { //Increment the expanded nodes global variable ++noOfNodesExpanded; //Generated the successor nodes List<Node> childNodes = current.ChildrenNodes(); //Traverse the nodes foreach (Node successor in childNodes) { int distanceFromNewNode = successor.HCost(goalNode); //Compute the estimated g-cost of the successor int estGCost = current.GCost() + Math.Abs(distanceFromNewNode - current.HCost(goalNode)); int findVisited = FindNode(visitedNodes, successor); //check if the successor is already visited or not, if visted then discard it if (findVisited == -1) { //check if the successor is already in the queue int found = FindNode(queue.nodes, successor); //if true if (found != -1) { //if f cost of old node is greater than f cost of successor node, delete it from the queue //else discard the successor node if ((queue.nodes.ElementAt(found).PathCost > estGCost + distanceFromNewNode)) queue.Delete(found); else continue; } //Add the node to the queue and set its parent to curren successor.Parent = current; successor.PathCost = estGCost; successor.Priority = successor.PathCost + distanceFromNewNode; queue.Enqueue(successor); ++noOfNodesGenerated; } else { if (estGCost < successor.GCost()) { visitedNodes.RemoveAt(findVisited); continue; } } } } if (queue.nodes.Count == 100000) break; } return null; }