// A*
// http://en.wikipedia.org/wiki/A*_search_algorithm
// http://wiki.gamegardens.com/Path_Finding_Tutorial
/// <summary>
/// A* uses a best-first search and finds the least-cost path from a given initial node to one goal node (out of one or more possible goals).
/// </summary>
/// <param name="grid"></param>
/// <param name="start"></param>
/// <param name="goal"></param>
/// <param name="neighbors"></param>
/// <returns></returns>
public static IEnumerable <Coordinate> Astar(this long[,] grid, Coordinate start, Coordinate goal, Func <long[, ], Coordinate, IEnumerable <Coordinate> > neighbors)
{
var closedSet = new HashSet <Coordinate>();
var openSet = new HashSet <Coordinate> {
start
};
var cameFrom = new Dictionary <Coordinate, Coordinate>();
var gScore = new long[grid.GetLength(0), grid.GetLength(1)];
var hScore = new long[grid.GetLength(0), grid.GetLength(1)];
var fScore = new long[grid.GetLength(0), grid.GetLength(1)];
gScore[start.Row, start.Col] = 0;
hScore[start.Row, start.Col] = grid.DistanceEstimate(start, goal);
fScore[start.Row, start.Col] = hScore[start.Row, start.Col];
while (openSet.Any())
{
var min = openSet.Min(v => fScore[v.Row, v.Col]);
var u = openSet.First(v => fScore[v.Row, v.Col] == min);
if (u.Equals(goal))
{
foreach (var p in ReconstitutePath(cameFrom, cameFrom[u]))
{
yield return(p);
}
yield return(goal);
}
openSet.Remove(u);
closedSet.Add(u);
foreach (var neighbor in neighbors(grid, u))
{
if (closedSet.Contains(neighbor))
{
continue;
}
var tentativeGScore = gScore[u.Row, u.Col] + grid.DistanceBetween(u, neighbor);
var tentativeIsBetter = false;
if (!openSet.Contains(neighbor))
{
openSet.Add(neighbor);
tentativeIsBetter = true;
}
else if (tentativeGScore < gScore[neighbor.Row, neighbor.Col])
{
tentativeIsBetter = true;
}
if (tentativeIsBetter)
{
if (cameFrom.TryGetValue(neighbor, out var value))
{
value = u;
}
else
{
cameFrom.Add(neighbor, u);
}
gScore[neighbor.Row, neighbor.Col] = tentativeGScore;
hScore[neighbor.Row, neighbor.Col] = DistanceEstimate(grid, neighbor, goal);
fScore[neighbor.Row, neighbor.Col] = gScore[neighbor.Row, neighbor.Col] + hScore[neighbor.Row, neighbor.Col];
}
}
}
}