public List<GridCell> AStar(GridCell startGridCell, GridCell targetGridCell, bool ignoresCovers = false)
{
//Evaluated nodes
HashSet<Vector2Int> closedSet = new HashSet<Vector2Int>();
//Unevaluated discovered nodes, default only start is known
HashSet<Vector2Int> openSet = new HashSet<Vector2Int>{startGridCell.Position};
//Where each node came from
Dictionary<Vector2Int, Vector2Int> cameFrom = new Dictionary<Vector2Int, Vector2Int>();
//G score map, cost of getting to node from start, default value infinity
Dictionary<Vector2Int, float> gScore = _grid.Cast<GridCell>().ToDictionary(cell => cell.Position, cell => float.MaxValue);
//F score map, gCost + heuristic cost, default value infinity
Dictionary<Vector2Int, float> fScore = new Dictionary<Vector2Int, float>(gScore);
//Start has g score 0
gScore[startGridCell.Position] = 0.0f;
//Start has f score as pure heuristic
fScore[startGridCell.Position] = Vector2Int.Distance(startGridCell.Position, targetGridCell.Position);
while (openSet.Count > 0)
{
//Current node is the node with the lowest f score present in the open set
float lowest = openSet.Min(c => fScore[c]);
Vector2Int current = openSet.First(node => Mathf.Approximately(fScore[node], lowest));
//If we are at the target position, reconstruct the path
if (current == targetGridCell.Position)
return GetPath(current);
//Current node is set as evaluated
closedSet.Add(current);
openSet.Remove(current);
//Iterate over all neighbouring cells
List<Vector2Int> neighbours = GenerateNeighbours(current);
foreach (Vector2Int neighbour in neighbours)
{
//If node is already evaluated, skip it
if (closedSet.Contains(neighbour))
continue;
//Cost between nodes is hard set as 1.0f for now
float tentativeGScore = gScore[current] + 1.0f;
if (!openSet.Contains(neighbour)) //Newly discovered node
openSet.Add(neighbour);
else if (tentativeGScore >= gScore[neighbour]) //Lower cost path exists
continue;
cameFrom[neighbour] = current;
gScore[neighbour] = tentativeGScore;
fScore[neighbour] = tentativeGScore + Vector2Int.Distance(neighbour, targetGridCell.Position);
}
}
return null;
List<Vector2Int> GenerateNeighbours(Vector2Int node)
{
Vector2Int right = node + Vector2Int.right;
Vector2Int left = node + Vector2Int.left;
Vector2Int up = node + Vector2Int.up;
Vector2Int down = node + Vector2Int.down;
Vector2Int[] neighbourPositions = { right, left, up, down };
return neighbourPositions.Where(position => position.x >= 0 && position.y >= 0 && position.x < GridSize && position.y < GridSize &&
(GetCell(position).Type == 0 && !GetCell(position).Occupied || ignoresCovers) ).ToList();
}
List<GridCell> GetPath(Vector2Int node)
{
List<GridCell> path = new List<GridCell>();
Vector2Int current = targetGridCell.Position;
while(cameFrom.ContainsKey(current))
{
path.Add(GetCell(current));
current = cameFrom[current];
}
path.Reverse();
path.Add(GetCell(node));
return path;
}
}