// get the near neighbour cells
private List<Cell> getNeighbors(Cell currentCell)
{
int cellX = currentCell.X;
int cellY = currentCell.Y;
int cellSizeX = grid.GetLength(0) - 1;
int cellSizeY = grid.GetLength(1) - 1;
List<Cell> neighborCells = new List<Cell>();
//For x
if (cellX > 0 && cellX < cellSizeX)
{
neighborCells.Add(grid[cellX - 1, cellY]);
neighborCells.Add(grid[cellX + 1, cellY]);
}
else if (cellX == 0)
{
neighborCells.Add(grid[cellX + 1, cellY]);
}
else if (cellX == cellSizeX)
{
neighborCells.Add(grid[cellX - 1, cellY]);
}
//For y
if (cellY > 0 && cellY < cellSizeY)
{
neighborCells.Add(grid[cellX, cellY + 1]);
neighborCells.Add(grid[cellX, cellY - 1]);
}
else if (cellY == 0)
{
neighborCells.Add(grid[cellX, cellY + 1]);
}
else if (cellY == cellSizeY)
{
neighborCells.Add(grid[cellX, cellY - 1]);
}
//remove blockers
for (int i = 0; i < neighborCells.Count; i++)
{
if (neighborCells[i].IsBlockedMove)
neighborCells.Remove(neighborCells[i--]);
}
return neighborCells;
}
// A* path finding algorithm
private List<Cell> findPath(Cell startCell, Cell endCell)
{
openSet = new List<Cell>();
closeSet = new List<Cell>();
navigatedCells = new Dictionary<Cell, Cell>();
openSet.Add(startCell);
startCell.FScore = startCell.GScore + getDistance(startCell, endCell);
Cell currentCell;
while (openSet.Count > 0)
{
currentCell = getLowFscoreCell();
if (currentCell.X == endCell.X && currentCell.Y == endCell.Y)
{
Debug.WriteLine("Goal reached");
return constructPath(navigatedCells, endCell);
}
openSet.Remove(currentCell);
closeSet.Add(currentCell);
List<Cell> neighborCells = getNeighbors(currentCell);
foreach (Cell neighbor in neighborCells)
{
if (closeSet.Contains(neighbor))
continue;
int gScore = currentCell.GScore + 1;
if (!openSet.Contains(neighbor) || gScore < neighbor.GScore)
{
neighbor.GScore = gScore;
neighbor.FScore = neighbor.GScore + getDistance(neighbor, endCell);
navigatedCells.Add(neighbor, currentCell);
if (!openSet.Contains(neighbor))
openSet.Add(neighbor);
}
}
}
return null;
}
// return the distance bettwenn 2 cells
private int getDistance(Cell neighbor, Cell endCell)
{
int XDistance = Math.Abs(endCell.X - neighbor.X);
int YDistance = Math.Abs(endCell.Y - neighbor.Y);
return XDistance + YDistance;
}
// construct the optimum path
private List<Cell> constructPath(Dictionary<Cell, Cell> navigatedCells, Cell endCell)
{
List<Cell> optimumPath = new List<Cell>();
Cell current = endCell;
while (navigatedCells.ContainsKey(current))
{
optimumPath.Add(current);
current = navigatedCells[current];
}
return optimumPath;
}