/** * Find the shortest path between a start and goal node. * * @param grid The grid on which the search is performed. * * @param start The start node of the path to be found * * @param goal The goal node of the path to be found * * @param heuristicCostEstimate A function that returns * an heuristic cost of reaching one node from another. * The heuristic cost estimate must always be equal or * less than the actual cost. This is used to make the algorithm * perform well when the search space is big and the actual cost * calculation is expensive. In other cases, you can use the same * function as the cost parameter. * * @param cost The actual cost of reaching one node from * another. In many instances, the distance between the cells * is the cost. But cost can also consider other factors such * as danger, reward, travel time, and so on. This parameter * allows you to specify a cost function that takes this into * account. The cost function is only computed for adjacent * nodes. * * A common cost metric is the Euclidean metric, which can * be defined as shown below, with PointyHexPoint replaced * with the appropriate point: * * @code * public float EuclideanDistance(PointyHexPoint p1 PointyHexPoint p2) * { * return (map[p1] - map[p2]).magnitude; * } * @endcode * * @param isAccessible Whether a cell is accessible. * * @tparam TCell the type of cell of the grid that this algorithm takes. * @tparam TPoint the type of point of the grid that this algorithm takes. * * @returns The list of nodes on the path in order. If no * path is possible, null is returned. * * @version1_7 */ public static IEnumerable <TPoint> AStar <TCell, TPoint>( IGrid <TCell, TPoint> grid, TPoint start, TPoint goal, Func <TPoint, TPoint, float> heuristicCostEstimate, Func <TCell, bool> isAccessible, Func <TPoint, TPoint, float> cost) where TPoint : IGridPoint <TPoint> { var closedSet = new PointList <TPoint>(); // The set of tentative nodes to be evaluated var openSet = new PointList <TPoint> { start }; // The map of navigated nodes. var cameFrom = new Dictionary <TPoint, TPoint>(new PointComparer <TPoint>()); // Cost from start along best known path. var gScore = new Dictionary <TPoint, float>(new PointComparer <TPoint>()); gScore[start] = 0; // Estimated total cost from start to goal through y. var fScore = new Dictionary <TPoint, float>(new PointComparer <TPoint>()); fScore[start] = gScore[start] + heuristicCostEstimate(start, goal); while (!openSet.IsEmpty()) { var current = FindNodeWithLowestScore(openSet, fScore); if (current.Equals(goal)) { return(ReconstructPath(cameFrom, goal)); } openSet.Remove(current); closedSet.Add(current); var currentNodeNeighbors = grid.GetNeighbors(current); var accessibleNeighbors = from neighbor in currentNodeNeighbors where isAccessible(grid[neighbor]) select neighbor; foreach (var neighbor in accessibleNeighbors) { var tentativeGScore = gScore[current] + cost(current, neighbor); if (closedSet.Contains(neighbor)) { if (tentativeGScore >= gScore[neighbor]) { continue; } } if (!openSet.Contains(neighbor) || tentativeGScore < gScore[neighbor]) { cameFrom[neighbor] = current; gScore[neighbor] = tentativeGScore; fScore[neighbor] = gScore[neighbor] + heuristicCostEstimate(neighbor, goal); if (!openSet.Contains(neighbor)) { openSet.Add(neighbor); } } } } return(null); }
public static Dictionary <TPoint, int> GetPointsInRangeCost <TCell, TPoint>( IGrid <TCell, TPoint> grid, TPoint start, Func <TCell, bool> isAcessible, Func <TPoint, TPoint, int> getCellMoveCost, int moveRange) where TPoint : IGridPoint <TPoint> { // Nodes in range var closedSet = new HashSet <TPoint>(new PointComparer <TPoint>()); var costToReach = new Dictionary <TPoint, int>(new PointComparer <TPoint>()); var openSet = new PointList <TPoint> { start }; // Cost from start along best known path up to current point var costToReachContains = grid.CloneStructure(false); costToReach[start] = 0; while (!openSet.IsEmpty()) { // Process current node var current = FindNodeWithLowestScore(openSet, costToReach); openSet.Remove(current); closedSet.Add(current); //foreach (var neighbor in grid.GetNeighbors(current)) var neighbors = grid.GetNeighbors(current).ToPointList(); for (int i = 0; i < neighbors.Count; i++) { var neighbor = neighbors[i]; if (!closedSet.Contains(neighbor) && !openSet.Contains(neighbor) && isAcessible(grid[neighbor]) && (costToReach[current] + getCellMoveCost(current, neighbor) <= moveRange) ) { // Cost of current node + neighbor's move cost int newCost = costToReach[current] + getCellMoveCost(current, neighbor); if (costToReachContains[neighbor]) { if (costToReach[neighbor] > newCost) { costToReach[neighbor] = newCost; } } else { costToReach[neighbor] = newCost; costToReachContains[neighbor] = true; } openSet.Add(neighbor); } } } return(costToReach); }