/**
* 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);
}
