private static int CalculateHeuristic(AStarNode current, AStarNode target)
{
return((int)(Math.Abs(current.Position.X - target.Position.X) + Math.Abs(current.Position.Y - target.Position.Y)));
}
// Source : http://www.policyalmanac.org/games/aStarTutorial.htm
public static List <Point> DoAStar(Point to, Point from)
{
int tileAcross = TileMap.Width;
int tileDown = TileMap.Height;
int[] neighbourX = new int[] { -1, 0, 1, 0 };
int[] neighbourY = new int[] { 0, -1, 0, 1 };
AStarNode[,] squares = new AStarNode[tileAcross, tileDown];
for (int y = 0; y < tileDown; y++)
{
for (int x = 0; x < tileAcross; x++)
{
squares[x, y] = new AStarNode(x, y);
}
}
//TODO: There is a bug that I can't find that requires to switch the positions
AStarNode origin = squares[to.X, to.Y];
AStarNode destination = squares[from.X, from.Y];
List <AStarNode> openList = new List <AStarNode>();
List <AStarNode> closedList = new List <AStarNode>();
openList.Add(origin);
AStarNode current = null;
while (openList.Count > 0)
{
current = openList[0];
for (int i = 0; i < openList.Count; i++)
{
if (openList[i].G + CalculateHeuristic(openList[i], destination) < current.G + CalculateHeuristic(current, destination))
{
current = openList[i];
}
}
if (current == destination) // If destination has been reached.
{
int pathX = destination.Position.X,
pathY = destination.Position.Y;
List <AStarNode> path = new List <AStarNode>();
do
{
path.Add(current);
pathX = current.Position.X;
pathY = current.Position.Y;
current = current.ParentNode;
}while (pathX != origin.Position.X || pathY != origin.Position.Y);
List <Point> pathCoord = new List <Point>();
foreach (AStarNode n in path)
{
pathCoord.Add(n.Position);
}
return(pathCoord);
}
// Add current to closed list
closedList.Add(current);
// Remove current from openlist
openList.Remove(current);
//Grab surrounding squares
for (int neighOffset = 0; neighOffset < 4; neighOffset++)
{
// Grab the coordinates of a neighbour tile.
int NX = current.Position.X + neighbourX[neighOffset];
int NY = current.Position.Y + neighbourY[neighOffset];
if (NX < 0 || NX >= tileAcross || NY < 0 || NY >= tileDown)
{
continue;
}
AStarNode neighbour = squares[NX, NY];
if (!IsWalkable(neighbour.Position))
{
continue;
}
if (!closedList.Contains(neighbour))
{
if (!openList.Contains(neighbour))
{
neighbour.H = CalculateHeuristic(neighbour, destination); // Calculate heuristic value.
neighbour.ParentNode = current;
neighbour.G = 10 + current.G; // Add 10 (movement cost) to the current tiles cost.
openList.Add(neighbour);
}
else
{
if (current.G + CalculateHeuristic(current, destination) < neighbour.G + CalculateHeuristic(neighbour, destination))
{
neighbour.ParentNode = current;
neighbour.G = 10 + current.G; // Add 10 (movement cost) to the current tiles cost.
}
}
}
}
}
return(null);
}
