public static SearchResult BestFirstSearch(Grid grid, Grid.Point startPos, Grid.Point endPos)
{
FakePriorityQueue <Grid.Point> queue = new FakePriorityQueue <Grid.Point>();
Dictionary <Grid.Point, float> distanceMap = new Dictionary <Grid.Point, float>();
Dictionary <Grid.Point, Grid.Point> visitedMap = new Dictionary <Grid.Point, Grid.Point>();
queue.Enqueue(startPos, 0);
distanceMap.Add(startPos, 0);
visitedMap.Add(startPos, null);
while (!queue.Empty)
{
Grid.Point current = queue.Dequeue();
if (current.Equals(endPos))
{
return(new SearchResult
{
Path = GeneratePath(visitedMap, current),
Visited = new List <Grid.Point>(visitedMap.Keys)
});
}
foreach (Grid.Point neighbour in grid.GetAdjacentCells(current))
{
if (!visitedMap.ContainsKey(neighbour))
{
float priority = Heuristic(endPos, neighbour);
queue.Enqueue(neighbour, priority);
visitedMap.Add(neighbour, current);
distanceMap.Add(neighbour, distanceMap[current] + grid.GetCostOfEnteringCell(neighbour));
}
}
}
return(new SearchResult());
}
private static float Heuristic(Grid.Point endPos, Grid.Point point, bool useManhattan = true)
{
if (useManhattan)
{
return(Manhattan(endPos, point));
}
else
{
return(Euclidean(endPos, point));
}
}
public static List <Grid.Point> GeneratePath(Dictionary <Grid.Point, Grid.Point> parentMap, Grid.Point endState)
{
List <Grid.Point> path = new List <Grid.Point>();
Grid.Point parent = endState;
while (parent != null && parentMap.ContainsKey(parent))
{
path.Add(parent);
parent = parentMap[parent];
}
return(path);
}
private static float Euclidean(Grid.Point A, Grid.Point B)
{
return((float)Math.Sqrt(((A.X - B.X) * (A.X - B.X)) + ((A.Y - B.Y) * (A.Y - B.Y))));
}
private static float Manhattan(Grid.Point A, Grid.Point B)
{
return(Math.Abs(A.X - B.X) + Math.Abs(A.Y - B.Y));
}