# A* Algorithm in C# for pathfinding 2D tile grid

I am very new programming and am trying to develop a small tycoon game as a hobby project in Unity. This algorithm executes beautifully but if 100+ agents are getting new paths simultaneously there is a significant drop in frame rate. Where should I start with optimizing?

I use 4 classes for my pathfinding. The PathFinder creates a PathGrid when Initialized, and has the actual algorithm. The PathGrid contains a multidimensional array of PathNode, and all methods needed to modify the grid. The PathNode is just a location, corresponding to a tile, and a List of PathEdge. PathEdge is a simple struct containing a ref to a PathNode (the edge's end) and a move cost.

PathController:

public class PathController : MonoBehaviour {

//main control class for game
GameController gameController;
PathGrid pathGrid;

public void Initialize()
{
pathGrid = new PathGrid ();
}

public Queue<Location> GetPath (Location start, Location end)
{
//GameController keeps record of map size
if (gameController.IsInBounds (start) && gameController.IsInBounds (end)) {
int xSize = gameController.xSize;
int zSize = gameController.zSize;
Queue<Location> path = new Queue<Location> ();
List<PathNode> frontier = new List<PathNode> ();
bool[,] visited = new bool[xSize,zSize];
Dictionary<PathNode, PathNode> leadingMap = new Dictionary<PathNode, PathNode> ();
float[,] scores = new float[xSize,zSize];
Location currentPosition = start;
int attempts = 0;

while (true) {
if (gameController.AreLocationsIdentical (currentPosition, end)) {
break;
} else {
attempts++;

if (attempts >= 10000) {
Debug.LogError ("Frontier while loop timed out.");
return null;
}
PathNode currentNode = pathGrid.GetPathNode (currentPosition);
foreach (PathEdge edge in currentNode.edges) {
int x = edge.end.location.x;
int z = edge.end.location.z;
if (!visited [x,z] && !frontier.Contains (edge.end)) {
float f = Mathf.Abs (end.x - currentPosition.x) + Mathf.Abs (end.z - currentPosition.z);
float g = attempts + edge.moveCost;
scores [x, z] = f + g;
}
}
frontier.Remove (currentNode);
visited [currentPosition.x, currentPosition.z] = true;
PathNode best = frontier [0];
foreach (PathNode node in frontier) {
if (scores [node.location.x, node.location.z] < scores [best.location.x, best.location.z]) {
best = node;
}
}
currentPosition = new Location (best.location.x, best.location.z);
}
}

attempts = 0;
Stack<Location> rev = new Stack<Location> ();
currentPosition = end;
while (true) {
attempts++;
if (attempts >= 10000) {
Debug.LogError ("While loop for stack population timed out!");
return null;
}
if (gameController.AreLocationsIdentical (currentPosition, start)) {
break;
}
rev.Push (currentPosition);
currentPosition = nextNode.location;
}
int pathCount = rev.Count;
for (int i = 0; i < pathCount; i++) {
path.Enqueue (rev.Pop ());
}
return path;
} else {
Debug.LogError ("Tried to get a path with start or end out of bounds: S " + start.x + "," + start.z + " E " + end.x + "," + end.z);
return null;
}
}

public void UpdateNodeEdges (Location location)
{
pathGrid.UpdateNodeEdges (location);
}
}


PathGrid:

public class PathGrid  {

public PathNode[,] pathNodes { get; protected set; }

public PathGrid ()
{
int x = GameController.Instance.xSize;
int z = GameController.Instance.zSize;

pathNodes = new PathNode[x,z];
//Create all nodes
for (int i = 0; i < x; i++) {
for (int j = 0; j < z; j++) {
PathNode node = new PathNode (new Location (i, j));
pathNodes [i, j] = node;
}
}
//Create all edges
for (int i = 0; i < x; i++) {
for (int j = 0; j < z; j++) {
UpdateNodeEdges (new Location (i, j));
}
}
}

public PathNode GetPathNode (Location location)
{
if (GameController.Instance.IsInBounds (location)) {
return pathNodes [location.x, location.z];
} else {
Debug.LogError ("Tried to get PathNode out of range: " + location.x + "," + location.z);
return null;
}
}

public void UpdateNodeEdges(Location location)
{
List<PathEdge> edges = new List<PathEdge> ();
foreach (PathNode neighbor in GetNodeNeighbors(location)) {
}
GetPathNode (location).edges = edges;
}

List<PathNode> GetNodeNeighbors (Location location)
{
List<PathNode> neighbors = new List<PathNode> ();
foreach (Location neighborLoc in GameController.Instance.DirectionalLocations(location)) {
}
return neighbors;
}

PathEdge CreatePathEdge (Location start, Location end)
{
int moveCost = (GetMovementCostForTile (start) + GetMovementCostForTile (end)) / 2;
PathEdge edge = new PathEdge (moveCost, GetPathNode (end));
return edge;
}

int GetMovementCostForTile (Location location)
{
TileData data = GameController.Instance.dataController.GetTileData (location);
if (data.installedObject == null) {
return 20;
} else {
switch (data.installedObject.installedObjectType) {
case InstalledObjectType.None:
return 20;
case InstalledObjectType.Structure:
if (data.installedObject.subType == 1) {
return 2;
} else {
return 1000;
}
case InstalledObjectType.Nature:
return 1000;
case InstalledObjectType.Resource:
return 400;
default:
Debug.LogError ("Tried to get move cost for unsupported tile at " + location.x + "," + location.z);
return 100;
}
}
}
}


PathNode:

public class PathNode  {
public Location location;
public List<PathEdge> edges;
public PathNode (Location location)
{
this.location = location;
}
}


PathEdge:

public struct PathEdge  {
public int moveCost;
public PathNode end;
public PathEdge (int moveCost, PathNode end)
{
this.moveCost = moveCost;
this.end = end;
}
}


In case it's needed, here is the script for Location, as it's a custom struct I made and is used extensively throughout my classes.

Location:

public struct Location  {
public int x { get; set; }
public int z { get; set; }
public Location(int x, int z) : this()
{
this.x = x;
this.z = z;
}
}


This is my first time asking for help on a project. ANY critique is greatly appreciated as I feel as though self teaching is sometimes like reaching in to the dark and hoping to come up with a working solution. I have not run in to a problem with optimization yet, and could not find a good example to fit my needs with my underdeveloped google-fu skills.

• I havn't the energy to write a proper answer now - hopefully you'll get one - but to get you started, you need to use a smarter data-structure for the frontier (i.e. a priority queue). Usingfrontier.Contains (edge.end) to not add to the frontier looks like a bug (i.e. you might have just found a shorter route to the node) – VisualMelon Jun 24 '18 at 17:56
• Speaking of a C# property queue for pathfinding! github.com/BlueRaja/High-Speed-Priority-Queue-for-C-Sharp – BlueRaja - Danny Pflughoeft Jun 24 '18 at 18:30
• I'll definitely look in to a priority queue for the frontier, thank you guys. – JCoffey Jun 24 '18 at 19:44
• You need to @VisualMelon me if you want me to notice something :) Yes, the way you've done things, it would be a serious problem going over the same ground twice (as opposed to just an efficiency concern), but the issue I see is that you reject a route to a node (i.e. don't add it to the frontier) when you already have a route, only that original route could be longer (if your edge costs are different). How you solve this will depend on the data-structure you use for the frontier. – VisualMelon Jun 24 '18 at 21:11
• @BlueRaja That's awesome thanks man! I'll have some time in the next couple days to look in to it more, you probably just saved me hours! – JCoffey Jun 25 '18 at 0:22

There is a lot of code here, so there is a lot to say; no way I'll covered everything! I'll do the easy things first, and then assume lots of them before we look at the algorithm.

## Style

• Usually C#ers put starting braces for everything on a new line.

• You've been inconsistent with putting spaces after method identifier names (e.g. GetPathNode (Location location) vs UpdateNodeEdges(Location location). Personally I don't put a space in, but you should be consistent with yourself. The same with method calls (pathGrid.GetPathNode(currentPosition) vs. rev.Pop ()).

## Naming

This is really your choice, but the Microsoft naming conventions are extremely prevalent. As far as I'm concerned, anything public (or indeed internal) should be ProperCamelCase (type names, public members, etc.). I like anything local to a method to be lowerCamelCase (variable names, arguments, etc.). I use a variety of forms for private member depending on the situation, but it doesn't matter: the public interface is what really matters.

I would, then, prefer that the (public) members of your types be ProperCamelCase (e.g. PathEdge.MoveCost).

More importantly, a few of your variable names are less than ideal. What is f? What is g? You mostly use x and z as coordinates, but then as dimensions in PathGrid.ctor(): change the dimensions to width and height. rev is half-way to a good name, because it's being used to reverse something, but really it's an accumulator that you happen to want to reverse.

## Encapsulation and Mutable Objects

The three classes/structs you show all very mutable. You're using a combination of public fields and public properties. Location and PathEdge, in particular, look like they should be immutable (and as a general rule, mutable structs are a source of misery). To this end, I'd be inclined remove the setters in Location, and add readonly getters in PathEdge. This makes it impossible to invalidate the state, which makes understanding and maintaining the types much simpler. Mutable structs are misery inducing because you can end up modifying a copy of the struct, and get no-where.

public struct Location
{
public int X { get; }
public int Z { get; }

public Location(int x, int z) : this()
{
X = x;
Z = z;
}
}


It seems you need Edges to be mutable in PathNode, but if possible, I would make this immutable as well. Obviously you can't create cycles if you can't change the edges, but initialising it with a List<T> which you add to cannot do much harm.

## PathNode

Actaully, while I'm talking about it, I'd add methods to PathNode to wire up the graph neatly, and expose the edges as an IReadOnlyList<PathEdge>.

public class PathNode
{
public Location Location { get; }
private List<PathEdge> _edges { get; }

public PathNode(Location location)
{
this.Location = location;
_edges = new List<PathEdge>();
}

public void Join(PathNode other, int cost)
{
}
}


This should just tidy up calling code, making it more readable, and harder to break.

PathNode a = new PathNode(aloc);
PathNode b = new PathNode(bloc);
a.Join(b, 1);


You don't have to worry about setting b.Edges, and you don't have to think about how to add an edge, you hide the details behind a clean interface which expressions your intentions.

It's not applicable in your case (from the way you've coded it), but suppose you actually want two-way joins between nodes. You can encode this by adding detail to PathNode.Join (and maybe giving it a better name), instead of having to remember to add both edges. You can even add diagnostic checks in Join, to make sure it makes sense. I'm just making these up, but the point is that you can pour domain knowledge into this class, and thereby make it harder to misuse.

public void Join(PathNode other, int cost)
{
if (other == null)
throw new ArgumentException($"{other} must not be null", nameof(other)); if (other == this) throw new ArgumentException($"Cannot join a node to itself; {nameof(other)} must not be this", nameof(other));
if (cost < 0)
throw new ArgumentException(\$"{cost} must non-negative", nameof(cost));

}


You could argue you want this class to be general-purpose pathfinding node, but then I'd expect Location to be a generic type, and these particular examples I've given are generally applicable to route finding.

Another advantage of using abstract interfaces like IReadOnlyList<T> is that they give you freedom to change the implementation in the future, without breaking the external interface. As a crazy example, you might decide to start storing edges in some other medium, other than a List<PathEdge>, but still want to expose the edges as PathEdges. IReadOnlyList<T> here is ideal here, and 2 options spring to mind:

• store the edges as a type that extends PathEdge, in which case you can convert a List<ExtendedPathEdge> to an IReadOnlyList<PathEdge> without any overhead

• write a small type which translates whatever your internal representation is into PathNodes on the fly

Options! By exposing a nice interface, you've freed yourself from making hard implementation decisions early on, forced yourself to contemplate how the type will be used (as opposed to how the type will work or just what state the type represents) and giving yourself wiggle room to modify the implementation in the future.

## Location

This is a nice simple struct. It might be better to use a class (I'd use one 'by default'), but that's something you can profile if performance continue to be a problem.

You've not provided the method AreLocationsIdentical, but I'm guessing it's really boring and looks like this:

public static bool AreLocationsIdentical(Location l, Location r)
{
return l.X == r.X && l.Z == r.Z;
}


I would put this logic inside Location. Because it's such a simple type, I'd be happy to define == and !=, and I would expect this to have explicit overloads for Equals and GetHashCode.

You have the code float f = Mathf.Abs (end.x - currentPosition.x) + Mathf.Abs (end.z - currentPosition.z); in GetPath. I'd pull that out into a nice descriptive method:

public int ManhattenDistanceTo(Location other)
{
return Math.Abs(X - other.X) + Math.Abs(Z - other.Z);
}


## PathGrid

### public PathNode[,] pathNodes { get; protected set; }

Good to see this somewhat encapsulated, but why protected? Unless it is part of the design that you want this to modifiable by an extending type, then don't be afraid to hide it away, or even make this readonly.

Also, you probably don't want 'outsiders' being able to modify this. You have GetPathNode(Location location) which hides away this implementation detail behind a nice interface, only to expose it to the world anyway! Hide it!

### GetPathNode (Location location)

Just for fun, you could make GetPathNode(Location location) an indexer, since it is the entire public interface, it doesn't really warrant a name, but it's great that you are looking nodes up based on Location and no individual x/z coordinates. People are use to indexers throwing IndexOutOfRange exceptions, so I think it would if yours did to.

More importantly,

### public void UpdateNodeEdges(Location location)

It's good that you've separated out this logic, but it should only ever be called by the constructor. Duly, I'd make it a local function in the constructor (along with GetNodeNeighbors). It should at least be private.

### PathEdge CreatePathEdge (Location start, Location end)

I've already suggested add PathNode.Join, so I'll have to suggest just renaming this ComputePathEdgeCost and returning only the cost. Alternatively, you could have this method wire up the nodes itself. Or, reject my advice about adding PathNode.Join, and keep this as it is.

### GetMovementCostForTile

The switch is kind of hard to read (style point). Also, it is full of magic numbers!

I'd have expected this information to be stored in some type ObjectType class. Currently you the information regarding the object jammed in the routing code, rather than living with other information thereabout. This would save you fro whatever (I suspect nightmarish) situation led to the creation of the subType property. It would also save you from this:

default:
Debug.LogError ("Tried to get move cost for unsupported tile at " + location.x + "," + location.z);
return 100;


How can return 100 possibly make sense?! The last thing you ever want to do is cover-over an unexpected situation by making information up!

Don't be afraid to crash violently. Crashing violently is great: you can't ignore it, and you get a stack-trace. System.Debug.Assert() is great for this sort of thing, because you can ignore the problem is you want to, and then log it politely. I commend you, however, for clearly documenting this (and other) potential problems.

It might be OK, but if (data.installedObject == null) return 20; looks wrong as well. It feels like you are expecting something to go wrong, and are covering over it by returning a default value. I'd at least expect some justification for returning 20, or rather InsertAppropriateConstantNameHere.

## Path-finding

The algorithm has two big problems:

• it will potentially reject PathNodes which are shorter than paths to the same node already on the Frontier

• using attempts as a cost makes no sense whatsoever! You must keep track of the true cost of nodes! Replace scores[x, z] = f + g

As already suggested, you should probably be using a Priority Queue of some description to store the Frontier. There are lots of different ways to implement an A*, and what you go with depends heavily on the data-structure you use for the frontier. You are in the nice situation of having a known-size search space, and can efficiently index into it, which could influence your decision.

The main job of the priority queue is to avoid the search for the 'best' node to expand next: this is a linear-time in your code: a priority queue will give you either a constant or logarithmic time lookup. This is a trade-off against the cost of adding to the frontier: your current code it is O(1) amortised, but with a priority queue it will be logarithmic in the number of elements, but overall the algorithm is only then logarithmic in the size of the frontier, not linear (and the frontier size is exponential in the branching factor).

If you have a nice Priority Queue, it will also help to solve the problem I mentioned, by allowing you to lookup the priority of already queued elements efficiently*, and to remove existing elements. You can otherwise simulate this by using your own map to keep track of the best route, and only (re-)queuing if the new route you have found is shorter (again: you can only have found a shorter route if the previous route was only on the frontier**). This is less efficient, because it leaves stuff on the frontier and requires you to check you haven't visited a node already when adding it to the set of seen nodes (that last bit is usually essentially free through), but from experience it's mostly fine: I've rarely used anything more than a simple binary heap (very efficient, but limited functionality).

### Queue<Location> GetPath (Location start, Location end)

First off, I would pull out Queue<Location> GetPath(PathNode start, PathNode end) method, and call this after doing all the checks and lookups and such.

Personally, I would make PathEdge a class, and store a reference to the previous edge in that class along with the true cost (i.e. scores) for simplicity, but I'll keep to what you're doing in this example code. It could be more efficient, I don't know (I almost never care about performance, and usually when I do, all my intuitions about the cost of creating objects are wrong: profile profile profile!). Actually... PathEdge is useless, so we can ditch it altogether.

Here is an example of using a PriorityQueue, which hopefully doesn't have the two deficiencies mentioned above (it's completely untested). Everything which I would have put in PathEdge (thought I'd have called it PathNode, and called PathNode something else...) is instead stored in dictionaries (these could be combined for greater efficiency).

public Queue<Location> GetPath(PathNode start, PathNode end)
{
SimplePriorityQueue<PathNode, float> frontier = new SimplePriorityQueue<PathNode, float>();

// made these hash-sets/dictionaries, the arrays may well be marginally faster, but if you aren't searching far, this will save potentially large allocations (feel free to convert back)
HashSet<PathNode> visited = new HashSet<PathNode>();
Dictionary<PathNode, float> costs = new Dictionary<PathNode, float>(); // like 'scores', only these are true costs left

// each node points to the points to the node that came before it
Dictionary<PathNode, PathNode> leadingMap = new Dictionary<PathNode, PathNode>();

frontier.Enqueue(start, 0f);

while (frontier.Count > 0)
{
PathNode cur = frontier.Dequeue();

if (cur == end)
{
// found the end: do something...
return null; // (not this)
}

float cost = costs[cur];

foreach (var edge in cur.Edges)
{
if (visited.Contains(edge.End))

var trueCost = cost + edge.MoveCost; // the actual cost
var heuristicCost = cur.Location.ManhattenDistanceTo(edge.End.Location); // the estimated cost left (this is almost certainly inlined)
var combinedCost = trueCost + heuristicCost; // the estimated total cost

// I've written two paths because it's probably more efficient, but I'd be inclined to pull this out into a local method                if (frontier.TryGetPriority(edge.End, out var existingCost))
{
if (combinedCost < existingCost)
{
frontier.Enqueue(edge.End, combinedCost);
costs[edge.End] = trueCost;
}
}
else
{
frontier.UpdatePriority(edge.End, combinedCost);
}
}
}

// no route found!
System.Diagnostics.Debug.Fail("Unable to find path"); // a more useful message would be more useful
return null;
}


This code demonstrates the use of a priority queue, using SimplePriorityQueue from OptimisedPriorityQueue as an example

Hopefully the code above is enough explanation for how to use the priority queue (if it needed any). It addresses problem 1 by checking no only that something is on the frontier, but that we are also better than it. We address deficiency 2 by storing the true cost to reach a node in costs.

I'd also pull out the reversing code, or just use LINQ. This bit is not a performance concern, so make it readable.

other issues with GetPath:

• if (gameController.IsInBounds (start) && gameController.IsInBounds (end)) {: you perform this check at the top of the method, and deal with the problem at the other end! I'd squash these together into an early return, or invert them.

• attempts: everything involving attempts makes no sense: it's not a cost, and it's not timing out! And what is 10000?! Remove it! All of it!

* I don't like referring to hashsets as O(1) lookups; I note that the documentation OptimisedPriorityQueue has some reassuring comments to this effect! Thanks BlueRaja!

** This assumes a monotonically increasing cost and admissible heuristic, which basically means that you never over-estimate the cost of getting anywhere: this means when you sort by the estimated cost, you know that any node that is not the 'best' node can cost no-less than it's estimated cost to get to the end. If it went via the current node, then it would necessarily have an estimated cost (from said node) that is no less than its current estimate, and since it would then have the same heuristic term as your 'best' node, you know it must then have a higher true cost of reaching the 'best' node.

• Great review. My lingering gripe with OP's Location is that I would be expecting properties X and Y, but Y has been awkwardly replaced with Z. – Rick Davin Jun 25 '18 at 11:56
• @RickDavin that's probably a hold-over from it using Unity. I guess Y is up in unity world. – VisualMelon Jun 25 '18 at 11:58
• @VisualMelon Holy cow dude, you're awesome haha! Unfortunately I didn't have much time for my project today, but I've probably read over your review 5 times. Thank you so much for your help. My next step will be implementing a priority queue based on BlueRajas, and the whole deal with the admissible heuristic was just me rushing this script. I looked at my past versions which did actually save the cost per tile on each step. I don't know why I used my attempts var there, that was just a quick hack to catch a time out when writing the script in case I missed something. Thank you again! – JCoffey Jun 26 '18 at 3:30
• @JCoffey glad you like it! Your heuristic is admissible - as far as I can tell - because it assumes a cost of 1 for each tile (and the cheapest tile is 2, so you could assume a cost of 2` and it would still be); I just put that comment in as a justification of something and in case someone less familiar with the algorithm reads this. – VisualMelon Jun 26 '18 at 7:42