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 BiDirectionalAStarSearch(Grid grid, Grid.Point startPos, Grid.Point endPos) { // True if opened from the start queue, false if opened by the end queue, not opened if not exists Dictionary <Grid.Point, bool> openedBy = new Dictionary <Grid.Point, bool>(); FakePriorityQueue <Grid.Point> startQueue = new FakePriorityQueue <Grid.Point>(); FakePriorityQueue <Grid.Point> endQueue = new FakePriorityQueue <Grid.Point>(); Dictionary <Grid.Point, float> startDistanceMap = new Dictionary <Grid.Point, float>(); Dictionary <Grid.Point, Grid.Point> startVisitedMap = new Dictionary <Grid.Point, Grid.Point>(); Dictionary <Grid.Point, float> endDistanceMap = new Dictionary <Grid.Point, float>(); Dictionary <Grid.Point, Grid.Point> endVisitedMap = new Dictionary <Grid.Point, Grid.Point>(); startQueue.Enqueue(startPos, 0); startDistanceMap[startPos] = 0; startVisitedMap[startPos] = null; openedBy[startPos] = true; endQueue.Enqueue(endPos, 0); endDistanceMap[endPos] = 0; endVisitedMap[endPos] = null; openedBy[endPos] = false; while (!startQueue.Empty && !endQueue.Empty) { // From start Grid.Point current = startQueue.Dequeue(); if (openedBy.ContainsKey(current) && openedBy[current] == false) { // Found goal or the frontier from the end queue // Return solution List <Grid.Point> startPath = GeneratePath(startVisitedMap, current); List <Grid.Point> endPath = GeneratePath(endVisitedMap, current); List <Grid.Point> allPath = new List <Grid.Point>(startPath); allPath.AddRange(endPath); List <Grid.Point> allVisited = new List <Grid.Point>(startVisitedMap.Keys); allVisited.AddRange(endVisitedMap.Keys); return(new SearchResult { Path = allPath, Visited = allVisited }); } foreach (Grid.Point neighbour in grid.GetAdjacentCells(current)) { float newCost = startDistanceMap[current] + grid.GetCostOfEnteringCell(neighbour); if (!startDistanceMap.ContainsKey(neighbour) || newCost < startDistanceMap[neighbour]) { startDistanceMap[neighbour] = newCost; openedBy[neighbour] = true; float priority = newCost + Heuristic(endPos, neighbour); startQueue.Enqueue(neighbour, priority); startVisitedMap[neighbour] = current; } } // From end current = endQueue.Dequeue(); if (openedBy.ContainsKey(current) && openedBy[current] == true) { // Found goal or the frontier from the start queue // Return solution List <Grid.Point> startPath = GeneratePath(startVisitedMap, current); List <Grid.Point> endPath = GeneratePath(endVisitedMap, current); List <Grid.Point> allPath = new List <Grid.Point>(startPath); allPath.AddRange(endPath); List <Grid.Point> allVisited = new List <Grid.Point>(startVisitedMap.Keys); allVisited.AddRange(endVisitedMap.Keys); return(new SearchResult { Path = allPath, Visited = allVisited }); } foreach (Grid.Point neighbour in grid.GetAdjacentCells(current)) { float newCost = endDistanceMap[current] + grid.GetCostOfEnteringCell(neighbour); if (!endDistanceMap.ContainsKey(neighbour) || newCost < endDistanceMap[neighbour]) { endDistanceMap[neighbour] = newCost; openedBy[neighbour] = false; float priority = newCost + Heuristic(startPos, neighbour); endQueue.Enqueue(neighbour, priority); endVisitedMap[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)); }