// 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;
}