public static SearchResult Dijkstra(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 adj in grid.GetAdjacentCells(current))
{
float newDist = distanceMap[current] + grid.GetCostOfEnteringCell(adj);
if (!distanceMap.ContainsKey(adj) || newDist < distanceMap[adj])
{
distanceMap[adj] = newDist;
visitedMap[adj] = current;
queue.Enqueue(adj, newDist);
}
}
}
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);
}
public static SearchResult AStarSearch(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[startPos] = 0;
visitedMap[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))
{
float newCost = distanceMap[current] + grid.GetCostOfEnteringCell(neighbour);
if (!distanceMap.ContainsKey(neighbour) || newCost < distanceMap[neighbour])
{
distanceMap[neighbour] = newCost;
float priority = newCost + Heuristic(endPos, neighbour);
queue.Enqueue(neighbour, priority);
visitedMap[neighbour] = current;
}
}
}
return(new SearchResult());
}
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));
}
