public static List <GridTile> GetPath(GridTile start, GridTile goal, bool isRawCount = false)
{
if (start == null)
{
return(new List <GridTile>());
}
//Evaluated nodes
var closedSet = new HashSet <GridTile>();
//Currently discovered nodes that have not been evaluated yet
var openSet = new HashSet <GridTile>();
openSet.Add(start);
//Priority queue that sorts by lowest fScore
var openSetbyFScore = new SimplePriorityQueue();
// For each node, which node it can most efficiently be reached from.
// If a node can be reached from many nodes, cameFrom will eventually contain the
// most efficient previous step.
var cameFrom = new Dictionary <GridTile, GridTile>();
// For each node, the cost of getting from the start node to that node with default value of Infinity
var gScore = new Dictionary <GridTile, int>();
gScore[start] = 0;
// For each node, the total cost of getting from the start node to the goal
// by passing by that node. That value is partly known, partly heuristic.
// with default value of Infinity
var fScore = new Dictionary <GridTile, float>();
fScore[start] = GetHeuristicEstimate(start, goal) - 1f;
openSetbyFScore.Add(start, fScore[start]);
while (openSet.Count > 0)
{
var current = openSetbyFScore.GetTop();
if (current == goal)
{
return(ReconstructPath(current, cameFrom, isRawCount));
}
openSet.Remove(current);
openSetbyFScore.Remove(current);
closedSet.Add(current);
foreach (Directions direction in System.Enum.GetValues(typeof(Directions)))
{
var neighbor = GridController.instance.GetNeighborAt(direction, current.WorldLocation);
if (neighbor == null || closedSet.Contains(neighbor) || neighbor.State == GridTile.MovementState.COLLIDER)
{
if (!goal.Equals(neighbor))
{
continue;
}
else
{
//We have gotten to the goal via the closest unoccupied neighbor
return(ReconstructPath(current, cameFrom, isRawCount));
}
}
var tentativeGScore = gScore[current] + GetDistanceBetween(current, neighbor);
if (!openSet.Contains(neighbor))
{
openSet.Add(neighbor);
}
else if (tentativeGScore >= gScore.GetValueOrDefault(neighbor))
{
continue;
}
cameFrom[neighbor] = current;
gScore[neighbor] = tentativeGScore;
fScore[neighbor] = gScore[neighbor] + GetHeuristicEstimate(neighbor, goal);
openSetbyFScore.Add(neighbor, fScore[neighbor]);
}
}
return(new List <GridTile>());
}