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