// function A*(start,goal) public Stack<IntVector2> AStar(IntVector2 start, IntVector2 goal) { Node[,] nodeArray = new Node[world.MapSize.X, world.MapSize.Y]; for (int x = 0; x < world.MapSize.X; x++) { for (int y = 0; y < world.MapSize.Y; y++) { nodeArray[x, y] = new Node(); nodeArray[x, y].Location = new IntVector2(x, y); } } // closedset := the empty set % The set of nodes already evaluated. List<Node> closedset = new List<Node>(); // openset := set containing the initial node % The set of tentative nodes to be evaluated. List<Node> openset = new List<Node>(); openset.Add(nodeArray[start.X, start.Y]); // g_score[start] := 0 % Distance from start along optimal path. nodeArray[start.X, start.Y].GScore = 0; // h_score[start] := heuristic_estimate_of_distance(start, goal) nodeArray[start.X, start.Y].HScore = huristicDistance(start, goal); // f_score[start] := h_score[start] % Estimated total distance from start to goal through y. //nodeArray[start.X, start.Y].FScore = nodeArray[start.X, start.Y].HScore; // while openset is not empty while (openset.Count > 0) { // x := the node in openset having the lowest f_score[] value Node x = openset.Where(n => n.FScore == openset.Min(m => m.FScore)).First(); // if x = goal if(x.Location == goal) { // return reconstruct_path(came_from,goal) return reconstructPath(goal, start, nodeArray); } // remove x from openset openset.Remove(x); // add x to closedset closedset.Add(x); // foreach y in neighbor_nodes(x) foreach(IntVector2 y in world.neighborHexes(x.Location)) { Node yNode = nodeArray[y.X, y.Y]; // if y in closedset // continue if(closedset.Where(n => n.Location == y).Count() > 0) continue; // tentative_g_score := g_score[x] + dist_between(x,y) float tentativeGScore = x.GScore + world.getMoveCost(x.Location, y); bool tentativeIsBetter = false; // if y not in openset // add y to openset // tentative_is_better := true // elseif tentative_g_score < g_score[y] // tentative_is_better := true // else // tentative_is_better := false if(openset.Where(n => n.Location == y).Count() == 0) { openset.Add(yNode); tentativeIsBetter = true; } else if(tentativeGScore < yNode.GScore) tentativeIsBetter = true; // if tentative_is_better = true // came_from[y] := x // g_score[y] := tentative_g_score // h_score[y] := heuristic_estimate_of_distance(y, goal) // f_score[y] := g_score[y] + h_score[y] if(tentativeIsBetter) { yNode.cameFrom = x.Location; yNode.GScore = tentativeGScore; yNode.HScore = huristicDistance(y, goal); } } } // return failure return null; }
//function reconstruct_path(came_from,current_node) Stack<IntVector2> reconstructPath(IntVector2 currentNode, IntVector2 start, Node[,] nodeArray) { // if came_from[current_node] is set // p = reconstruct_path(came_from,came_from[current_node]) // return (p + current_node) // else // return the empty path Stack<IntVector2> path = new Stack<IntVector2>(); IntVector2 current = currentNode; while (current != start) { path.Push(current); current = nodeArray[current.X, current.Y].cameFrom; } return path; }