Edit and Warning (03-07-2009): As some have already noticed my old implementation of A* is allot faster than this one, and someone on another website pointed out that I totally forgot to add an heuristic to the scoring function, also there are some other notable improvements to be made by returning earlier and using a different datastructure, I’m very sorry that I rushed this one out so bad, it now seems to fast. I’ll get back to you all after the weekend with an updated version that will run allot faster (just adding the heuristics will cut allot of iterations).
So this all went a bit faster than I thought (or slower). Anyway I've just completed my new A* pathfinding class and I'd thought I'd share it with all of you.
This code is allot more readable than the first version mainly trough the use of a special small class "Breadcrumbs."
This really makes all those nasty points and ints go away. It now also works in 3D (as requested) but aswell in 2D (see the constructors on World below).
Below is the entire sourcecode. For the .cs file and a usage example please download the files from my SkyDrive here.
Good luck with this code, and don't hesitate to ask any questions or submit any bugs here. I'll always try to get to you asap.
The original (now deprecated) version and article can be found here here.
/// <summary>
/// Provides 3D PathFinding capabilites
/// </summary>
public static class PathFinder
{
/// <summary>
/// Method that switfly finds the best path from start to end.
/// </summary>
///
<param name="world"></param>
///
<param name="start"></param>
///
<param name="end">The starting breadcrumb. Use .parent to find the next breadcrumb</param>
public static BreadCrumb FindPath(World world, Vector3 start, Vector3 end)
{
//note we just flip start and end here so you don't have to.
return FindPathReversed(world, end, start);
}
/// <summary>
/// Method that uses an A* algorithm to find the best path from start to end.
/// </summary>
/// <returns>The end breadcrump where each parent is a step back (so its a reversed path!)</returns>
private static BreadCrumb FindPathReversed(World world, Vector3 start, Vector3 end)
{
//The openList will contains tiles we are going to examine
//The closedList contains tile that we've already examined and made part of a path
List<BreadCrumb> openList = new List<BreadCrumb>();
List<BreadCrumb> closedList = new List<BreadCrumb>();
//Add the start node to the openList
BreadCrumb current = new BreadCrumb(start);
BreadCrumb finish = new BreadCrumb(end);
openList.Add(current);
//Stop if there are no more options, or if we have found the finish
while (openList.Count > 0 && !closedList.Contains(finish))
{
current = openList[0];
//Find the lowest cost Tile that we can consider from the openList
for (int i = 1; i < openList.Count; i++)
{
if (openList[i].cost < current.cost)
{
current = openList[i];
}
}
//Switch that one over to the closed list
openList.Remove(current);
closedList.Add(current);
//Find neigbours add them to the openlist and check if those who where already
//on the openlist can be reached faster this way. Also calculate their best scores
Vector3 tmp;
for (int i = 0; i < surrounding.Length; i++)
{
tmp = current.position + surrounding[i];
if (tmp.X >= world.Left && tmp.X <= world.Right &&
tmp.Y >= world.Bottom && tmp.Y <= world.Top &&
tmp.Z >= world.Front && tmp.Z <= world.Back &&
world.PositionIsFree(tmp))
{
BreadCrumb node = new BreadCrumb(tmp, current);
if (!closedList.Contains(node))
{
if (!openList.Contains(node))
{
openList.Add(node);
}
Score(node, current, end);
}
}
}
}
//The last added node is the node containing the path!
if(closedList.Contains(finish)){
return closedList[closedList.Count -1];
}else{
return null; //no path found
}
}
/// <summary>
/// Scores the breadcrumbs
/// </summary>
private static void Score(BreadCrumb node, BreadCrumb parent, Vector3 end)
{
int cost;
//double diagonal moves cost 18 (all 3 different) normal diagonal moves cost 14 (2 different).
if (parent.position.X != node.position.X && parent.position.Y != node.position.Y && parent.position.Z != node.position.Z)
{
cost = 17 + parent.cost;
}
else if ((parent.position.X != node.position.X && (parent.position.Y != node.position.Y || parent.position.Z != node.position.Z)) ||
(parent.position.Y != node.position.Y && (parent.position.X != node.position.X || parent.position.Z != node.position.Z)))
{
cost = 14 + parent.cost;
}
else //orthogonal moves cost 10
{
cost = 10 + parent.cost;
}
if (cost < node.cost) //updates the cost only if the new cost is less
//this way new nodes and nodes already on the openlist both get correct scores
{
node.cost = cost;
node.next = parent;
}
}
//Array with all the possible offsets to get all straight and diagonal surrounding nodes
//See this as 3 3x3 slices of a 3x3x3 cube.
private static Vector3[] surrounding = new Vector3[]{
//Top slice (Y=1)
new Vector3(-1,1,1), new Vector3(0,1,1), new Vector3(1,1,1),
new Vector3(-1,1,0), new Vector3(0,1,0), new Vector3(1,1,0),
new Vector3(-1,1,-1), new Vector3(0,1,-1), new Vector3(1,1,-1),
//Middle slice (Y=0)
new Vector3(-1,0,1), new Vector3(0,0,1), new Vector3(1,0,1),
new Vector3(-1,0,0), new Vector3(1,0,0), //(0,0,0) is self
new Vector3(-1,0,-1), new Vector3(0,0,-1), new Vector3(1,0,-1),
//Bottom slice (Y=-1)
new Vector3(-1,-1,1), new Vector3(0,-1,1), new Vector3(1,-1,1),
new Vector3(-1,-1,0), new Vector3(0,-1,0), new Vector3(1,-1,0),
new Vector3(-1,-1,-1), new Vector3(0,-1,-1), new Vector3(1,-1,-1)
};
}
/// <summary>
/// These are the BreadCrumbs (linked list) that we lay so we can find back our path when we reach the finish
/// </summary>
public class BreadCrumb
{
public Vector3 position;
public BreadCrumb next;
public int cost = Int32.MaxValue; //High value so that real options will always be cheaper
public BreadCrumb(Vector3 position)
: this(position, null) { }
public BreadCrumb(Vector3 position, BreadCrumb parent)
{
this.position = position; this.next = parent;
}
//This way we can correctly tell if a BreadCrumb is in a list.
public override bool Equals(object obj)
{
if (obj is BreadCrumb)
{
return ((BreadCrumb)obj).position == this.position;
}
else
{
return false;
}
}
public override int GetHashCode()
{
return position.GetHashCode();
}
}
public class World
{
private bool[, ,] world; //extremely simple world where each node can be free or blocked: true=blocked
//Properties that allow us to see what size the world is
//Note that we use Y as height and Z as depth here!
public int Left { get { return 0; } }
public int Right { get { return world.GetLength(0); } }
public int Bottom { get { return 0; } }
public int Top { get { return world.GetLength(1); } }
public int Front { get { return 0; } }
public int Back { get { return world.GetLength(2); } }
/// <summary>
/// Creates a 2D world
/// </summary>
public World(int width, int height)
: this(width, height, 1)
{ }
/// <summary>
/// Creates a 3D world
/// </summary>
public World(int width, int height, int depth)
{
world = new Boolean[width, height, depth];
}
/// <summary>
/// Mark positions in the world als blocked (true) or unblocked (false)
/// </summary>
///
<param name="value">use true if you wan't to block the value</param>
public void MarkPosition(Vector3 position, bool value)
{
world[(int)position.X, (int)position.Y, (int)position.Z] = value;
}
/// <summary>
/// Checks if a position is free or marked
/// </summary>
/// <returns>true if the position is free</returns>
public bool PositionIsFree(Vector3 position)
{
return !world[(int)position.X, (int)position.Y, (int)position.Z];
}
}
. (Remember that they will send as fast as possible because they are asynchronous and don’t have to wait for the first send to complete so yeah some pause points are quite handy for now, in a real client you wouldn’t use while(true) but something more sophisticated like an update interval or when something changed.
