public List <GridCell> Solve()
{
AStarGridCell starGridCell = grid.Get(StartPos);
starGridCell.GScore = 0;
starGridCell.FScore = CalcHScore(StartPos);
SortedSet <AStarGridCell> openSet = new SortedSet <AStarGridCell>();
openSet.Add(starGridCell);
while (openSet.Count > 0)
{
AStarGridCell current = openSet.Min();
openSet.Remove(current);
// reached end
if (current.Position == EndPos)
{
return(CreatePathFrom(current));
}
foreach (AStarGridCell neighbor in GetNeighbors(current))
{
if (!current.CameFrom.Equals(neighbor))
{
// calculate new scores
double newGScore = current.GScore + neighbor.Position.Distance(current.Position);
double newFScore = newGScore + CalcHScore(neighbor.Position);
// check if this path is better
if (newFScore < neighbor.FScore)
{
neighbor.GScore = newGScore;
neighbor.FScore = newFScore;
neighbor.CameFrom = current;
openSet.Add(neighbor);
}
/* dijkstra
* double newDistanceFromStart = neighbor.Position.Distance(current.Position) + current.DistanceFromStart;
* if (newDistanceFromStart < neighbor.DistanceFromStart)
* {
* neighbor.DistanceFromStart = newDistanceFromStart;
* neighbor.CameFrom = current;
* openSet.Add(neighbor);
* }
*/
}
}
}
throw new PathNotFoundException();
}
private List <AStarGridCell> GetNeighbors(AStarGridCell cell)
{
List <Vector2Int> positions = new List <Vector2Int>();
if (WalkDiagonal)
{
foreach (Vector2Int offset in DiagonalDirectionOffsets)
{
positions.Add(cell.Position + offset);
}
}
foreach (Vector2Int offset in DirectionOffsets)
{
positions.Add(cell.Position + offset);
}
return(positions.Where(pos => pos.X > -1 && pos.Y > -1 && pos.X < grid.Width && pos.Y < grid.Height)
.Select(pos => grid.Get(pos))
.Where(gridCell => gridCell.Walkable)
.ToList());
}
