# Optimizing an A* algorithm

I am currently using an implementation of the A* algorithm described here.

Currently, when I run my algorithm, my IDE lags horribly and the .exe generated also lags terribly. Using the CPU analyzer, I narrowed down the culprit using around 95% of the available CPU to my PathFind method. I'm not entirely sure where to start with optimizing the algorithm or making it less taxing on the system.

    public struct Grid
{
public Rectangle Size;
public byte[,] Weight;
/// <summary>
/// Creates a grid for pathfinding, if weight is 0 is inaccessible, non-zero indicates accessible
/// </summary>
/// <param name="posX"></param>
/// <param name="posY"></param>
/// <param name="width"></param>
/// <param name="height"></param>
/// <param name="defaultValue"></param>
public Grid(int posX, int posY, int width, int height, byte defaultValue = 0)
{
Size = new Rectangle(posX, posY, width, height);
Weight = new byte[width, height];

for(int i = 0; i < width; i++)
{
for(int j = 0; j < height; j++)
{
Weight[i, j] = defaultValue;
}
}
}
public void SetWeight(int x, int y, byte weight)
{
Weight[x, y] = weight;
}

public List<Point> PathFind(Point start, Point end)
{
//Nodes that have already been analyzed and have a path from start to them
List<Point> closedSet = new List<Point>();

//Nodes that have been identified as a neighbor of an analyzed node, but not fully analyzed
List<Point> openSet = new List<Point> { start };

//A dictionary identifying the optimal origin point to each node.  This is used to back track from the end to find the optimal path
Dictionary<Point, Point> cameFrom = new Dictionary<Point, Point>();
//A dictionary indicating how far ceach analyzed node is from the start
Dictionary<Point, int> currentDistance = new Dictionary<Point, int>();
//A dictionary indicating how far it is expected to reach the end, if the path travels through the specified node
Dictionary<Point, float> predictedDistance = new Dictionary<Point, float>();

//Initialize the start node w/ dist of 0, and an est. distance of ydist + xdist, optimal path in a square grid that doesn't allow for diagonal movement
currentDistance.Add(start, 0);
predictedDistance.Add(start, 0 + Math.Abs(start.X - end.X) + Math.Abs(start.Y - end.Y));

//If there are unanalyzed nodes, process them
while(openSet.Count > 0)
{
//Get current node with the lowest est. cost to finish
Point current = (from p in openSet orderby predictedDistance[p] ascending select p).First();

//If it is finish, return path
if(current.X == end.X && current.Y == end.Y)
{
return ReconstructPath(cameFrom, end);
}

//Move current node from open to closed
openSet.Remove(current);
closedSet.Add(current);
IEnumerable<Point> e = GetNeighborNodes(current);
//Process each valid node around the current node
foreach(Point neighbor in e)
{
var tempCurrentDistance = currentDistance[current] + 1;

//If we already know theres a faster way to this neighbor, use that route and ignore this one
if(closedSet.Contains(neighbor) && tempCurrentDistance >= currentDistance[neighbor])
{ continue; }

//If we don't know the route to this neighbor, or if this is faster, store this route

if(!closedSet.Contains(neighbor) || tempCurrentDistance < currentDistance[neighbor])
{
if(cameFrom.Keys.Contains(neighbor))
{
cameFrom[neighbor] = current;
}
else
{
cameFrom.Add(neighbor, current);
}

currentDistance[neighbor] = tempCurrentDistance;
predictedDistance[neighbor] = currentDistance[neighbor] + Math.Abs(neighbor.X - end.X) + Math.Abs(neighbor.Y - end.Y);

//If this is a new node, add it to processing
if(!openSet.Contains(neighbor))
{ openSet.Add(neighbor); }
}
}
}
throw new Exception(string.Format("unable to find a path between {0}, {1}, and {2}, {3}", start.X, start.Y, end.X, end.Y));
}

private IEnumerable<Point> GetNeighborNodes(Point node)
{
List<Point> nodes = new List<Point>();

//Up
if(node.Y - 1 >= 0)
{
if (Weight[node.X, node.Y - 1] > 0)
{
nodes.Add(new Point(node.X, node.Y - 1));
}
}
//Right
if(node.X + 1 < Size.Width)
{
if (Weight[node.X + 1, node.Y] > 0)
{
nodes.Add(new Point(node.X + 1, node.Y));
}
}
//Down
if(node.Y + 1 < Size.Height)
{
if (Weight[node.X, node.Y + 1] > 0)
{
nodes.Add(new Point(node.X, node.Y + 1));
}
}
//Left
if(node.X - 1 > 0)
{
if (Weight[node.X - 1, node.Y] > 0)
{
nodes.Add(new Point(node.X - 1, node.Y));
}
}
return nodes;
}

private List<Point> ReconstructPath(Dictionary<Point, Point> cameFrom, Point current)
{
if(!cameFrom.Keys.Contains(current))
{
return new List<Point> { current };
}

List<Point> path = ReconstructPath(cameFrom, cameFrom[current]);
path.Add(current);
return path;
}
}


This is my implementation of the algorithm in practice:

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

using Microsoft.Xna.Framework;
using Wired.Code.GameObject;
namespace Wired.Code.AI
{
class AIChase : AIBase
{
private Entity _target;
private bool _requestPathUpdate;
private List<Point> _path;
private List<Point> _oldPath;
private int _pCount;
public AIChase(Entity parent, Entity target) : base(parent)
{
_target = target;
_requestPathUpdate = true;
_path = new List<Point>();
_oldPath = new List<Point>();
_pCount = 0;
}
public bool RequestPathUpdate
{
get { return _requestPathUpdate; }
}
public List<Point> Path
{
set { _path = value; }
}
public Vector2 TargetPos
{
get { return _target.GetOrigin; }
}
public override void Update(GameTime gt)
{
try
{
if (_oldPath.Count == _path.Count || _oldPath[_pCount] == _path[_pCount])
{
if (_pCount < _path.Count)
{
float dx = Math.Sign(_path[_pCount].X * 32 - _parent.Pos.X) * _parent.MoveSpeed;
float dy = Math.Sign(_path[_pCount].Y * 32 - _parent.Pos.Y) * _parent.MoveSpeed;
_parent.Move(new Vector2(dx, dy));
if (_parent.Pos == new Vector2(_path[_pCount].X * 32, _path[_pCount].Y * 32))
{
_pCount++;
}
}
}
else { _pCount = 0; }
}
catch (ArgumentOutOfRangeException e)
{
Console.WriteLine(e.Message);
}
_oldPath = _path;
}
}
}


Every update loop this line is called to set the AI path:

foreach (AIBase a in e.AIComponents)
{
if (a is AIChase)
{
if ((a as AIChase).RequestPathUpdate)
{
(a as AIChase).Path = GetPath(new Point((int)e.Pos.X / 32, (int)e.Pos.Y / 32), new Point((int)(a as AIChase).TargetPos.X / 32, (int)(a as AIChase).TargetPos.Y / 32));
}
}
}

• That seems to be a lot of (a as AIChase) it would probably be beneficial to just place that into a variable and use that value. Would look cleaner too. – Shelby115 Aug 8 '16 at 12:23

## 3 Answers

Without looking too closely at your actual algorithm, I can tell you're using the wrong data-structures.

• closedSet should to be a set. closedSet.Contains(neighbor) should be O(1) but right now it's O(n)
• openSet needs to be a priority queue. Dequeueing and adding items should be O(log n), but right now they're O(n log n) and O(n) (respectively).

(Shameless plug: I have a C# priority queue implementation specifically optimized for pathfinding on Github)

• Creating a custom Node data structure to represent the graph (instead of using a bunch of dictionaries/arrays) will give a slight speed boost, and more importantly make your code a lot cleaner.

Other stylistic notes:

• Keep your brace-style consistent. Inconsistent braces make the code harder to read.
• The line

if(!closedSet.Contains(neighbor) || tempCurrentDistance < currentDistance[neighbor])


is unnecessary, you check the negation of that a few lines above.

• It's debatable whether throwing an exception on "no path found" is the best solution. Exceptions should be for things that wouldn't occur during a normal execution of the program, and not having any path between two points is a common occurrence is many applications.

 Once you've done all the above, if it is still too slow for you there are various other algorithmic optimizations you can apply, such as altering tie-breaking behavior or using an incremental/suboptimal algorithm. These tend to be overkill for student projects, though.

You should also run your code through a profiler to find the hot-spots, at the very least for the experience. I recommend dotTrace, which is free for students.

openSet is sorted at the start of each while loop and the sorted list thrown away (if I read that selection syntax right). This is a cost of O(n log n) on each iteration. It is then followed by a remove which takes O(n) time.

You don't need the full sort, only which element has the smallest cost. And the priority heap will let you get that efficiently.

If you don't opt to go for a prio heap then keeping the sorted list around can speed up maintaining the list sorted. As sorting a partially sorted list can be faster than sorting a completely randomly ordered list. Especially when you take into account how nodes get out of order.

BlueRaja has some great tips on using the right data structure.

You may also wish to look at breaking the function down to allow running only ~100 iterations per frame, then picking up from there the next frame. Depending on the grid size, your A* can be running a very large number of steps, and having a multi-frame implementation is the only way to not "lock up" your interface to the user while it runs. You can tune this number as you run, based on how much of a frame's timing you are using (and how many pathfinds are happening simultaneously)

You can also look at 2- or more step hierarchical pathfinding, so a very fast first step could get your units moving in (almost always) the right direction, then a multi-frame full-pathfind could give them the full path to take once it completes.