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); }