public static GamePath mp_grid_path(Vector2 start, Vector2 goal, bool allowDiag = true)
{
GamePath p = new GamePath();
// 1) Convert positions to cell indexes
int relativeX = (int)start.X - pathfindingGrid.X;
int relativeY = (int)start.Y - pathfindingGrid.Y;
int startX = relativeX / pathfindingGrid.CellSizeX;
int startY = relativeY / pathfindingGrid.CellSizeY;
relativeX = (int)goal.X - pathfindingGrid.X;
relativeY = (int)goal.Y - pathfindingGrid.Y;
int goalX = relativeX / pathfindingGrid.CellSizeX;
int goalY = relativeY / pathfindingGrid.CellSizeY;
// Prepare heaps for a*
openList = new Heap <AStarCell>(heapSize);
closedList.Clear();
// Prepare goal + start cells
AStarCell startCell = pathfindingGrid.Grid[startX, startY];
AStarCell goalCell = pathfindingGrid.Grid[goalX, goalY];
openList.Add(startCell);
int iter = 0;
while (openList.Count > 0)
{
AStarCell currentNode = openList.RemoveFirst();
closedList.Add(currentNode);
if (currentNode.X == goalX && currentNode.Y == goalY)
{
RetracePath(startCell, goalCell);
foreach (AStarCell a in tempList)
{
p.points.Add(new Vector2(a.X * pathfindingGrid.CellSizeX + pathfindingGrid.X + 16, a.Y * pathfindingGrid.CellSizeY + pathfindingGrid.Y + 16));
}
break;
}
foreach (AStarCell cell in MpGetNeighbors(currentNode, allowDiag))
{
if (cell == null || !cell.Empty || closedList.Contains(cell))
{
continue;
}
int d = currentNode.G + MpGetDistance(currentNode, cell);
if (d < cell.G || !openList.Contains(cell))
{
cell.G = d;
cell.H = MpGetDistance(cell, goalCell);
cell.F = cell.G + cell.H;
cell.ParentNode = currentNode;
if (!openList.Contains(cell))
{
openList.Add(cell);
}
}
}
}
return(p);
}