# How can I improve the performance of my A* pathfinding code?

I'm currently working on implementing A* pathfinding into a little project of mine. My ideal goal is that my game should be able to handle about 1000+ objects without any noticable impact on performance.

Right now I'm using threading to handle my pathfinding as best as I can and it's working pretty nicely but once I start to reach upwards 1000 objects it'll start eating away at my performance regardless.

How could I improve my performance further? Any tips or tricks? I'll answer questions if you need more information, and I appreciate any help you can give me :)

Here's how I handle my pathfinding:

public class Pathfinding
{
// Returns a Vector3 list, representing a path on the AStarGrid.
public static List<Vector3> CalculatePath(Vector3 fromPos, Vector3 toPos)
{
// Get a reference to the pathfinding grid.
AStarGrid grid = AStarGrid.instance;

// Get the start- and endnode.
GridNode startNode = AStarGrid.instance.GetNodeAtPosition(fromPos);
GridNode endNode = AStarGrid.instance.GetNodeAtPosition(toPos);

if(startNode == null || endNode == null || startNode == endNode)
{
// No need to calculate a path if the startNode or endNode is null,
// or if the endNode is already on the startNode.
return null;
}

// The dictionary used to pair a node together with some data so that we dont have to modify a node directly since,
// we want this function to be able to be run several times simultaneously with threads.
Dictionary<GridNode, NodeData> openNodes = new Dictionary<GridNode, NodeData>();

// A separate dictionary which couples a node with another in order to be able to retrace a path by following a nodes parents.
Dictionary<GridNode, GridNode> nodeParents = new Dictionary<GridNode, GridNode>();

// The closedNodes only need to be a HashSet since we'll only be checking if a node exists in it, nothing else.
HashSet<GridNode> closedNodes = new HashSet<GridNode>();

// Begin the search and keep searching for as long as there are open nodes or the currentNode is the endNode.
GridNode currentNode;
while(openNodes.Count > 0)
{
currentNode = GetLowestFCostNode(openNodes);

// Check if the current node is the endNode, meaning we found our path.
if(currentNode == endNode)
{
// Path was found, retrace the path and return it.
return RetracePath(endNode, startNode, nodeParents);
}

// Loop through the current node's neighbours.
List<GridNode> neighbours = GetNeighbours(currentNode, grid);
for(int i = 0; i < neighbours.Count; i++)
{
GridNode neighbour = neighbours[i];

// Ignore closed nodes.
if(closedNodes.Contains(neighbour))
continue;

// Calculations used to determine gCost and hCost
int hDistanceFromCurrentToNeighbour = openNodes[currentNode].gCost + GetHeuristicDistance(currentNode, neighbour);
int hDistanceFromNeighbourToPathEnd = GetHeuristicDistance(neighbour, endNode);

// Check if the neighbour is already in the open set.
if(openNodes.ContainsKey(neighbour))
{
// Check if the gCost is lower using this path, recalculate costs and parent if it is.
if(hDistanceFromCurrentToNeighbour < openNodes[neighbour].gCost)
{
openNodes[neighbour].gCost = hDistanceFromCurrentToNeighbour;
openNodes[neighbour].hCost = hDistanceFromNeighbourToPathEnd;
nodeParents[neighbour] = currentNode;
}
}
else
{
// Add it to the open set along with its data, store its parent and calculate costs.
NodeData neighbourData = new NodeData();
neighbourData.gCost = hDistanceFromCurrentToNeighbour;
neighbourData.hCost = hDistanceFromNeighbourToPathEnd;
}
}

// Remove the current node from the open set and add it to the closed set instead.
openNodes.Remove(currentNode);
}

// No path was found, return null.
return null;
}

// Retraces the path from the specified node by following its data parents which are stored in the openNodes dictionary.
static List<Vector3> RetracePath(GridNode fromNode, GridNode toNode, Dictionary<GridNode, GridNode> nodeParents)
{
List<Vector3> path = new List<Vector3>();
GridNode currentlyTracing = fromNode;
while(currentlyTracing != toNode && currentlyTracing != null)
{
currentlyTracing = nodeParents[currentlyTracing];
}
return path;
}

// Gets the heuristic distance from one node to the other.
static int GetHeuristicDistance(GridNode fromNode, GridNode toNode)
{
// Casting is a little faster than Mathf.RoundToInt and the nodes shouldn't be in a floated position anyway.
int dx = Mathf.Abs((int)fromNode.position.x - (int)toNode.position.x);
int dy = Mathf.Abs((int)fromNode.position.y - (int)toNode.position.y);

// The longest axis tells us how many times a node needs to move vertically.
// The difference between the longest axis and the shortest axis tells us the diagonal moves required.
// Moving vertically has a cost of 14 and moving diagonally has a cost of 10.
if(dx > dy)
return 14 * dy + 10 * (dx - dy);

return 14 * dx + 10 * (dy - dx);
}

// Gets the neighbouring nodes of a target node within the specified grid.
static List<GridNode> GetNeighbours(GridNode targetNode, AStarGrid grid)
{
List<GridNode> neighbours = new List<GridNode>();
for(int x = -1; x <= 1; x++)
{
for(int y = -1; y <= 1; y++)
{
// Skip self.
if(x == 0 && y == 0)
continue;

// Fetches the neighbour, will be null if it's out of bounds etc.
GridNode neighbour = grid.GetNodeAtPosition(new Vector3(x + targetNode.position.x, y + targetNode.position.y, 0));
if(neighbour == null || !neighbour.isWalkable)
continue;

}
}
return neighbours;
}

// Get the node with the lowest fCost.
// Pretty slow. Optimize using a min heap somehow maybe? Got to look into this.
static GridNode GetLowestFCostNode(Dictionary<GridNode, NodeData> openNodes)
{
GridNode lowestNode = null;
NodeData lowestNodeData = null;
foreach(KeyValuePair<GridNode, NodeData> pair in openNodes)
{
if(lowestNode == null)
{
lowestNode = pair.Key;
lowestNodeData = pair.Value;
}
else
{
if(pair.Value.fCost < lowestNodeData.fCost)
{
lowestNode = pair.Key;
lowestNodeData = pair.Value;
}
}
}
return lowestNode;
}

// NodeData class, in order to pair a node with data.
class NodeData
{
public int gCost;
public int hCost;
public int fCost
{
get
{
return gCost + hCost;
}
}
}
}


Well firstly, your openSet needs to be a priority queue. Here's one I wrote specifically for pathfinding.

Secondly, unless you are a pathfinding and optimization expert, you're not going to beat an off-the-shelf implementation. I highly recommend you find one rather than implementing it yourself.

Thirdly, and most importantly, pathfinding optimization in games usually involves finding tricks to pathfind as infrequently as possible, rather than optimizing the pathfinding itself.

You said you're trying to pathfind for 1000 units.

• Do they all have the same destination? If so, you can do a single reverse pathfind instead of 1000 normal ones, or a single normal one + a flocking algorithm.
• Is it possible to pre-compute the paths to save time at runtime, using something like a vector field?
• Will approximate best paths work, like HPA*?

To improve performance you need to know what to optimize. To do this tough, profile your code enough to know where the hotspots are.

To find hotspots i recomend using the System.Diagnostics.Stopwatch; You can then encapsule some code between the start and stop. this will tell you the amount of MS thats beeing used on those lines.

From there you sort of need to figure out whats going wrong, and i don't think anyone can just tell you whats wrong. you unfortunately need to do the profiling and optimisation yourself. If you however find that some piece of code is slow, posting that particular code might be easier for someone to dissect and say whats not really optimzed and what is.