XNA A-star Pathfinding Algorithm
by
·
Join the DZone community and get the full member experience.
Join For Free
namespace PathFindingSample
{
public class Game1 : Microsoft.Xna.Framework.Game
{
GraphicsDeviceManager graphics;
SpriteBatch spriteBatch;
Texture2D tileImage;
A_Star a_star;
Agent agent;
MouseState mClick;
public Game1()
{
graphics = new GraphicsDeviceManager(this);
Content.RootDirectory = "Content";
graphics.PreferredBackBufferWidth = 500;
graphics.PreferredBackBufferHeight = 500;
this.IsMouseVisible = true;
}
public static TileInfo[,] tileInfo = new TileInfo[10, 10];
public Point startLoc = new Point(3, 4);
public Point endLoc = new Point(9, 9);
protected override void Initialize()
{
// Create the tile objects in the array
for (int x = 0; x < tileInfo.GetLength(0); x++)
{
for (int y = 0; y < tileInfo.GetLength(0); y++)
{
tileInfo[x, y] = new TileInfo();
}
}
// Change some tiles to walls
tileInfo[4, 0].tileType = TileInfo.TileType.Wall;
tileInfo[4, 1].tileType = TileInfo.TileType.Wall;
tileInfo[4, 2].tileType = TileInfo.TileType.Wall;
tileInfo[4, 3].tileType = TileInfo.TileType.Wall;
tileInfo[4, 4].tileType = TileInfo.TileType.Wall;
tileInfo[4, 5].tileType = TileInfo.TileType.Wall;
tileInfo[3, 5].tileType = TileInfo.TileType.Wall;
tileInfo[2, 5].tileType = TileInfo.TileType.Wall;
tileInfo[1, 5].tileType = TileInfo.TileType.Wall;
tileInfo[1, 4].tileType = TileInfo.TileType.Wall;
tileInfo[1, 3].tileType = TileInfo.TileType.Wall;
tileInfo[1, 2].tileType = TileInfo.TileType.Wall;
tileInfo[7, 6].tileType = TileInfo.TileType.Wall;
tileInfo[7, 7].tileType = TileInfo.TileType.Wall;
tileInfo[7, 8].tileType = TileInfo.TileType.Wall;
tileInfo[7, 9].tileType = TileInfo.TileType.Wall;
// Pass the tile information and a weight for the H.
// The lower the H weight value shorter the path
// the higher it is the less number of checks it take to determine
// a path. Less checks might be useful for a very large number of agents.
int hWeight = 2;
a_star = new A_Star(tileInfo, hWeight);
base.Initialize();
}
protected override void LoadContent()
{
// Create a new SpriteBatch, which can be used to draw textures.
spriteBatch = new SpriteBatch(GraphicsDevice);
tileImage = Content.Load("tile");
agent = new Agent(Content.Load("agent"), startLoc, a_star);
}
protected override void Update(GameTime gameTime)
{
// Allows the game to exit
if (GamePad.GetState(PlayerIndex.One).Buttons.Back == ButtonState.Pressed)
this.Exit();
mClick = Mouse.GetState();
if (mClick.LeftButton == ButtonState.Pressed)
{
startLoc.X = (int)mClick.X / 50;
startLoc.Y = (int)mClick.Y / 50;
}
if (mClick.RightButton == ButtonState.Pressed)
{
endLoc.X = (int)mClick.X / 50;
endLoc.Y = (int)mClick.Y / 50;
}
agent.setDestination(startLoc.X, startLoc.Y, endLoc.X, endLoc.Y);
base.Update(gameTime);
agent.Update(gameTime);
base.Update(gameTime);
}
protected override void Draw(GameTime gameTime)
{
GraphicsDevice.Clear(Color.CornflowerBlue);
spriteBatch.Begin();
for (int x = 0; x < tileInfo.GetLength(0); x++)
{
for (int y = 0; y < tileInfo.GetLength(0); y++)
{
if (tileInfo[x, y].tileType == TileInfo.TileType.Floor)
spriteBatch.Draw(tileImage, new Vector2(x * 50, y * 50), Color.White);
else if (tileInfo[x, y].tileType == TileInfo.TileType.Wall)
spriteBatch.Draw(tileImage, new Vector2(x * 50, y * 50), Color.DarkGray);
}
}
for (int i = 0; i < agent.a_star.Path.Count; i++)
{
spriteBatch.Draw(tileImage, new Vector2(agent.a_star.Path[i].X * 50, agent.a_star.Path[i].Y * 50), Color.Yellow);
}
spriteBatch.Draw(tileImage, new Vector2(startLoc.X * 50, startLoc.Y * 50), Color.Green);
spriteBatch.Draw(tileImage, new Vector2(endLoc.X * 50, endLoc.Y * 50), Color.Red);
agent.Draw(spriteBatch);
spriteBatch.End();
base.Draw(gameTime);
}
}
public class Agent
{
public bool isPlayer;
enum AgentState { Pathing, Stopped };
AgentState agentState = AgentState.Stopped;
Point tileLocation; // Which tile the agent is currently on
Vector2 drawLocation; // Where the agent is being drawn on screen
Vector2 velocity; // Speed X Direction of the agent
float speed = 3; // rate the agent moves
float distanceCheckValue; // distance to the nextPathLoc that is used to switch
public A_Star a_star; // Our pathfinding class instance
Vector2 drawOrigin; // The origin used to draw the agent
Texture2D texture; // The image of the agent
Vector2 nextPathLoc; // The next location in the path we are trying to reach
Point destination; // the final destination tile we are trying to reach
int setDestX;
int setDestY;
int setStartX;
int setStartY;
public void setDestination(int w , int x , int y , int z)
{
setStartX = w;
setStartY = x;
setDestX = y;
setDestY = z;
}
public Agent(Texture2D tex, Point loc, A_Star A)
{
drawLocation = getDrawLoc(loc);
UpdateTileLoc();
velocity = Vector2.Zero;
texture = tex;
drawOrigin = new Vector2(texture.Width, texture.Height) * 0.5f;
a_star = A;
distanceCheckValue = speed * 1.75f;
}
Vector2 getDrawLoc(Point loc)
{
// Convert tile location to onscreen location
return new Vector2(loc.X * 50 + 25, loc.Y * 50 + 25);
}
Vector2 getDrawLoc(Vector2 loc)
{
// Convert tile location from the path that is a vector to onscreen location
return new Vector2(loc.X * 50 + 25, loc.Y * 50 + 25);
}
void UpdateTileLoc()
{
// Update the tile location based on the onscreen location
Point tileLocationTmp = new Point((int)drawLocation.X / 50, (int)drawLocation.Y / 50);
// It the agent has moved from one location to tile to another remove the reference to the agent from
// the old tile, update the location and set a reference to the agent on the new tile
if (tileLocationTmp != tileLocation)
{
Game1.tileInfo[tileLocation.X, tileLocation.Y].unitOnTile = null;
tileLocation = tileLocationTmp;
Game1.tileInfo[tileLocation.X, tileLocation.Y].unitOnTile = this;
}
}
public Point GetDestintation()
{
// This is a function that would find the goal of the path
// For example this could be a function to find the closest player and set the enemy to move towards him.
if (tileLocation == new Point(setDestX, setDestY))
return new Point(setStartX, setStartY);
else
return new Point(setDestX, setDestY);
}
public void Update(GameTime gameTime)
{
float elapsed = (float)gameTime.ElapsedGameTime.TotalSeconds;
if (agentState == AgentState.Stopped)
{
destination = GetDestintation();
if (destination != tileLocation)
{
a_star.start(tileLocation.X, tileLocation.Y, destination.X, destination.Y);
if (a_star.pathAvailable)
{
// Start in the direction of the first part of the path
agentState = AgentState.Pathing;
nextPathLoc = getDrawLoc(a_star.Path[0]);
velocity = nextPathLoc - drawLocation;
velocity.Normalize();
velocity *= speed;
}
}
}
if (agentState == AgentState.Pathing)
{
if (a_star.pathAvailable && a_star.Path.Count > 1)
{
// Check if we need to start moving toward the next path part
if ((nextPathLoc - drawLocation).Length() < distanceCheckValue)
{
// remove the path part we are currently moving towards
a_star.Path.RemoveAt(0);
// set the location in screen co-ordinates for the new path part
nextPathLoc = getDrawLoc(a_star.Path[0]);
// Set the velocity pointing towards the next path part
velocity = nextPathLoc - drawLocation;
velocity.Normalize();
velocity *= speed;
}
}
// Update the draw location and tile location
drawLocation += velocity;
UpdateTileLoc();
// Check if we have reached the overall goal of the path
if (a_star.Path.Count == 1 && (nextPathLoc - drawLocation).Length() < distanceCheckValue)
{
// Stop the agent and make sure it is on the exact location of the goal
drawLocation = nextPathLoc;
velocity = Vector2.Zero;
agentState = AgentState.Stopped;
}
}
}
public void Draw(SpriteBatch spriteBatch)
{
spriteBatch.Draw(texture, drawLocation, null, Color.White, 0, drawOrigin, 1, SpriteEffects.None, 0);
}
}
}
// This is a new class Astar
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
namespace PathFindingSample
{
//public class Unit
//{
// public bool isPlayer;
//}
public class TileInfo
{
public enum TileType { Floor, Wall, Door, OpenDoor };
public TileType tileType = TileType.Floor;
public Agent unitOnTile;
}
public class A_Star
{
TileInfo[,] colMap;
Dictionary list = new Dictionary();
List openList = new List();
int hWeight;
int goalX;
int goalY;
int startX;
int startY;
int Width, Height;
public List Path;
public bool pathAvailable = false;
public struct listitem
{
public listitem(int X, int Y)
{
x = X;
y = Y;
opened = false;
closed = false;
F = 0;
G = 0;
H = 0;
parentX = 0;
parentY = 0;
}
public int x;
public int y;
public bool opened;
public bool closed;
public int F;
public int G;
public int H;
public int parentX;
public int parentY;
}
public A_Star(TileInfo[,] ColMap, int _hWeight)
{
hWeight = _hWeight;
colMap = ColMap;
Path = new List();
Width = colMap.GetLength(0);
Height = colMap.GetLength(1);
}
public bool start(int Sx, int Sy, int Gx, int Gy)
{
pathAvailable = false;
goalX = Gx;
goalY = Gy;
startX = Sx;
startY = Sy;
Path.Clear();
openList.Clear();
list.Clear();
bool noPath = false;
if ((Gx - 1 < 0 || colMap[Gx - 1, Gy].tileType == TileInfo.TileType.Wall) &&
(Gx + 1 > Width || colMap[Gx + 1, Gy].tileType == TileInfo.TileType.Wall) &&
(Gy - 1 < 0 || colMap[Gx, Gy - 1].tileType == TileInfo.TileType.Wall) &&
(Gy + 1 > Width || colMap[Gx, Gy + 1].tileType == TileInfo.TileType.Wall))
noPath = true;
if (!(Sx == Gx && Sy == Gy) && colMap[Gx, Gy].tileType != TileInfo.TileType.Wall && !noPath)
{
list.Add(new Point(Sx, Sy), new listitem(Sx, Sy));
list.Add(new Point(Gx, Gy), new listitem(Gx, Gy));
openList.Add(new listitem(Sx, Sy));
setOpenOnLowestF(Sx, Sy, Gx, Gy);
if (pathAvailable)
{
makePath(Gx, Gy, Sx, Sy);
}
else if (colMap[Gx, Gy].unitOnTile != null && colMap[Gx, Gy].unitOnTile.isPlayer
&& Math.Abs(goalX - startX) < 2 && Math.Abs(goalY - startY) < 2)
{
// If the player is on the goal tile then return true so we can attack him
Path.Add(new Vector2(Gx, Gy));
return true;
}
}
//Console.WriteLine(Path.ToString);
return pathAvailable;
}
void setOpenOnLowestF(int Sx, int Sy, int Gx, int Gy)
{
int test = 0;
int loops = 0;
int removeIndex = 0;
do
{
Point startLoc = new Point(Sx, Sy);
listitem ListItem = list[startLoc];
loops++;
ListItem.opened = false;
openList.RemoveAt(removeIndex);
ListItem.closed = true;
list[startLoc] = ListItem;
int YFin = Sy + 1;
int XFin = Sx + 1;
for (int y = Sy - 1; y <= YFin; ++y)
{
if (y < 0 || y > Height - 1)
continue;
for (int x = Sx - 1; x <= XFin; ++x)
{
Point Location = new Point(x, y);
if (!list.ContainsKey(Location))
list.Add(new Point(x, y), new listitem(x, y));
if ((x == Sx && y == Sy) || (x < 0 || x > Width - 1)
|| (!isWalkable(x, y)) || (list[Location].closed)) // || npcthere(x, y))
continue;
TileInfo ColNode = colMap[x, y];
listitem tempListItem = list[Location];
if (tempListItem.opened)
{
int tempG = 0;
if (x == Sx || y == Sy)
{
if (ColNode.unitOnTile == null)
tempG = 10 + list[Location].G;
else
{
tempListItem.G = 45 + tempListItem.G;
}
}
else
{
if (ColNode.unitOnTile == null)
tempG = 14 + tempListItem.G;
else
{
tempListItem.G = 62 + tempListItem.G;
}
}
if (tempG < list[Location].G)
{
tempListItem.G = tempG;
tempListItem.parentX = Sx;
tempListItem.parentY = Sy;
tempListItem.F = tempListItem.G + tempListItem.H;
}
list[Location] = tempListItem;
continue;
}
if (x == Sx || y == Sy)
{
if (ColNode.unitOnTile == null)
tempListItem.G = 10 + list[new Point(Sx, Sy)].G;
else
{
tempListItem.G = 45 + list[new Point(Sx, Sy)].G;
}
tempListItem.opened = true;
tempListItem.x = x;
tempListItem.y = y;
tempListItem.parentX = Sx;
tempListItem.parentY = Sy;
tempListItem.H = calculateH(goalX, x) + calculateH(goalY, y);
tempListItem.F = tempListItem.G + tempListItem.H;
openList.Add(tempListItem);
list[Location] = tempListItem;
}
else
{
if ((x + 1 < Width && !isWalkable(x + 1, y)) ||
(x - 1 >= 0 &&!isWalkable(x - 1, y)) ||
(y - 1 >= 0 && !isWalkable(x, y - 1)) ||
(y + 1 < Height && !isWalkable(x, y + 1)))
{
tempListItem.opened = false;
list[Location] = tempListItem;
continue;
}
if (ColNode.unitOnTile == null)
tempListItem.G = 14 + list[new Point(Sx, Sy)].G;
else
{
tempListItem.G = 62 + list[new Point(Sx, Sy)].G;
}
tempListItem.opened = true;
tempListItem.x = x;
tempListItem.y = y;
tempListItem.parentX = Sx;
tempListItem.parentY = Sy;
tempListItem.H = calculateH(goalX, x) + calculateH(goalY, y);
tempListItem.F = tempListItem.G + tempListItem.H;
openList.Add(tempListItem);
list[Location] = tempListItem;
}
}
}
if (!(list[new Point(Gx, Gy)].opened))
{
int lowestF = 10000;
test = 0;
int ITEMS = openList.Count;
for (int index = 0; index < ITEMS; ++index)
{
test++;
if (openList[index].F < lowestF)
{
lowestF = openList[index].F;
Sx = openList[index].x;
Sy = openList[index].y;
removeIndex = index;
}
}
}
//diplayCurrent(new Point(Sx, Sy));
} while (test != 0 && !list[new Point(Gx, Gy)].opened && loops < 300);
//diplayCurrent(new Point(Sx, Sy));
if (test == 0 || loops > 249)
pathAvailable = false;
else
pathAvailable = true;
}
bool isWalkable(int x, int y)
{
listitem tempItem = new listitem();
Point Loc = new Point(x, y);
if (colMap[x, y].tileType == TileInfo.TileType.Wall)
{
tempItem.closed = true;
if (!list.ContainsKey(Loc))
list.Add(Loc, tempItem);
list[new Point(x, y)] = tempItem;
return false;
}
else if (colMap[x, y].unitOnTile != null && colMap[x, y].unitOnTile.isPlayer && x != goalX && y != goalY)
{
tempItem.closed = true;
if (!list.ContainsKey(Loc))
list.Add(Loc, tempItem);
list[new Point(x, y)] = tempItem;
return false;
}
return true;
}
int calculateH(int G, int S)
{
if (S > G)
return (S - G) * hWeight;
else if (G > S)
return (G - S) * hWeight;
else
return 0;
}
void makePath(int Gx, int Gy, int Sx, int Sy)
{
Vector2 temp = new Vector2(Gx, Gy);
while (!(temp.X == Sx && temp.Y == Sy))
{
Path.Insert(0, temp);
int x = (int)temp.X;
int y = (int)temp.Y;
Point loc = new Point(x, y);
temp = new Vector2(list[loc].parentX, list[loc].parentY);
}
}
Vector2 getParentof(int x, int y)
{
Point loc = new Point(x, y);
return new Vector2(list[loc].parentX, list[loc].parentY);
}
public bool getOpened(int x, int y)
{
Point loc = new Point(x, y);
return list[loc].opened;
}
public bool getClosed(int x, int y)
{
Point loc = new Point(x, y);
return list[loc].closed;
}
public int getF(int x, int y)
{
Point loc = new Point(x, y);
return list[loc].F;
}
public int getG(int x, int y)
{
Point loc = new Point(x, y);
return list[loc].G;
}
public int getH(int x, int y)
{
Point loc = new Point(x, y);
return list[loc].H;
}
#if WINDOWS
public void diplayCurrent(Point p)
{
Console.Clear();
Console.WriteLine("Open or Closed");
for (int y = -1; y < colMap.GetLength(1); ++y)
{
for (int x = -1; x < colMap.GetLength(0); ++x)
{
if (x == -1 || y == -1)
{
if (y == -1 && x == -1)
Console.Write("\t");
if (y == -1 && x != -1)
{
if (x < 10)
Console.Write(" " + x);
else
Console.Write(" " + x);
}
if (y != -1 && x == -1)
Console.Write(y + "\t");
}
else
{
Point loc = new Point(x, y);
if (startX == x && startY == y)
{
Console.ForegroundColor = ConsoleColor.Cyan;
Console.Write(" S ");
}
else if (p.X == x && p.Y == y)
{
Console.ForegroundColor = ConsoleColor.Magenta;
Console.Write(" X ");
}
else if (goalX == x && goalY == y)
{
Console.ForegroundColor = ConsoleColor.Cyan;
Console.Write(" G ");
}
else if (list.ContainsKey(loc) && getOpened(x, y))
{
Console.ForegroundColor = ConsoleColor.Green;
Console.Write(" O ");
}
else if (list.ContainsKey(loc) && getClosed(x, y))
{
Console.ForegroundColor = ConsoleColor.Red;
Console.Write(" C ");
}
else if (colMap[x, y].tileType == TileInfo.TileType.Wall)
{
Console.ForegroundColor = ConsoleColor.Yellow;
Console.Write(" # ");
}
else
{
Console.ForegroundColor = ConsoleColor.White;
Console.Write(" x ");
}
}
}
Console.WriteLine();
Console.WriteLine();
Console.ForegroundColor = ConsoleColor.White;
}
//Console.ReadKey();
//Console.WriteLine("Values");
//for (int y = -1; y < colMap.GetLength(1); ++y)
//{
// for (int x = -1; x < colMap.GetLength(0); ++x)
// {
// if (x == -1 || y == -1)
// {
// if (y == -1 && x == -1)
// Console.Write("\t");
// if (y == -1 && x != -1)
// {
// if (x < 10)
// Console.Write(" " + x);
// else
// Console.Write(" " + x);
// }
// if (y != -1 && x == -1)
// Console.Write(y + "\t");
// }
// else
// {
// Point loc = new Point(x, y);
// if (list.ContainsKey(loc))
// {
// Console.ForegroundColor = ConsoleColor.Yellow;
// if (list[loc].F < 10)
// Console.Write(" 00" + list[loc].H + " ");
// else if (list[loc].F < 100)
// Console.Write(" 0" + list[loc].H + " ");
// else
// Console.Write(" " + list[loc].H + " ");
// }
// else
// Console.Write(" XXX");
// }
// }
// Console.WriteLine();
// Console.WriteLine();
// Console.ForegroundColor = ConsoleColor.White;
//}
//Console.WriteLine();
//Console.WriteLine("Values of G");
//for (int y = 0; y < colMap.GetLength(1); ++y)
//{
// for (int x = 0; x < colMap.GetLength(0); ++x)
// {
// Point loc = new Point(x, y);
// if (list.ContainsKey(loc))
// {
// if (list[loc].G > 9)
// Console.Write("" + list[loc].G + " ");
// else
// Console.Write(" 0" + list[loc].G + " ");
// }
// }
// Console.WriteLine();
// Console.WriteLine();
//}
//Console.WriteLine();
//Console.WriteLine("H Values");
// for (int y = 0; y < colMap.GetLength(1); ++y)
//{
// for (int x = 0; x < colMap.GetLength(0); ++x)
// {
// Point loc = new Point(x, y);
// if (list.ContainsKey(loc))
// {
// if (list[loc].H > 9)
// Console.Write(" " + list[loc].H + " ");
// else
// Console.Write(" 0" + list[loc].H + " ");
// }
// }
// Console.WriteLine();
// Console.WriteLine();
//}
//Console.ReadKey();
} // End of Function
#endif
}
}
Algorithm
Opinions expressed by DZone contributors are their own.
Comments