# C# A* Pathfinding implementation, focusing on reusability and efficiency

I wrote a C# A* pathfinding (Though, it's generic enough that it could be used for other search purposes) implementation. I started this when I found that existing implementations often didn't support 3D environments, even though that only requires changing the neighbours function. Hence I tried to make this implementation very generic and reusable, such that it could be used for any number of dimensions. Many implementations also required you to extend their base classes with your tile class, which I'd rather avoid. Lastly I put a lot of effort into efficiency, iterating and performance testing several designs before settling on one that seems to perform well (though I've only compared it to other versions of itself so far).

It uses BlueRaja's High-Speed-Priority-Queue-for-C-Sharp. There are two classes, Node that extends BlueRaja's FastPriorityQueueNode and PathFinder which requires the heuristic, neighbours and indexMap parameterised functions. The tile or position is a totally generic type T which these functions can act on arbitrarily. The other two are hopefully self-explanatory, but indexMap is a function that returns a unique integer ID for T. For example, this simple 3D implementation: tile => (tile.x * SizeX * SizeY) + (tile.y * SizeY) + tile.z;. Lastly, PathFinder also requires a size variable which is the largest value that indexMap can possibly produce.

Any feedback is appreciated. I'm particularly curious about efficiency, and how many use of generic types may have affected efficiency. The code is all in a repo including some tests that also work as examples, but I'll include the two relevant classes and some sample implementations of the generic functions below:

Node.cs

using Priority_Queue;

namespace Jansk.Pathfinding
{
public class Node<T> : FastPriorityQueueNode
{
public T Position;
public int Previous;
public int Cost;
public int Heuristic;
public int Index;

public Node(T position)
{
Position = position;
}
}
}


Pathfinder.cs

using System;
using System.Collections.Generic;
using Priority_Queue;

namespace Jansk.Pathfinding
{
public class PathFinder<T>
{
private FastPriorityQueue<Node<T>> frontier;
public Node<T>[] Graph;
private Func<T, T, int> heuristic;
private Func<T, IEnumerable<T>> neighbours;
private Func<T, int> indexMap;
private int size;

public const int MaximumPathLength = 650;

public PathFinder(Func<T, T, int> heuristic, int size)
{
this.heuristic = heuristic;
this.size = size;
}

public T[] Path(T startPosition, T goalPosition, Func<T, int> indexMap, Func<T, IEnumerable<T>> neighbours)
{
Node<T> goalNode = null;
this.indexMap = indexMap;
this.neighbours = neighbours;

BuildGraph(startPosition, delegate(Node<T> current)
{
if (current.Position.Equals(goalPosition))
{
goalNode = current;
return true;
}
return false;
}, position => heuristic(position, goalPosition));

return GeneratePathFromGraph(startPosition, goalNode);
}

public void BuildGraph(T startPosition, Func<Node<T>, bool> goalTest, Func<T, int> heuristic,
Func<T, IEnumerable<T>> neighbours, Func<T, int> indexMap)
{
this.neighbours = neighbours;
this.indexMap = indexMap;
BuildGraph(startPosition, goalTest, heuristic);
}

public void BuildGraph(T startPosition, Func<Node<T>, bool> goalTest, Func<T, int> heuristic)
{
frontier = new FastPriorityQueue<Node<T>>(150);
Graph = new Node<T>[size];

var initial = new Node<T>(startPosition) {Index = indexMap(startPosition) };
frontier.Enqueue(initial, 0);
Graph[initial.Index] = initial;

while (frontier.Count > 0 && frontier.Count < MaximumPathLength)
{
var current = frontier.Dequeue();

if (goalTest(current))
{
break;
}

}
}

private T[] GeneratePathFromGraph(T startPosition, Node<T> goalNode)
{
var path = new List<T>();

if (goalNode != null)
{
var node = goalNode;
while (true)
{
if (node.Position.Equals(startPosition)) break;
node = Graph[node.Previous];
}
}

path.Reverse();
return path.ToArray();
}

private void AddNeighbours(Node<T> node, Func<T,int> heuristic)
{
foreach (var neighbour in neighbours(node.Position))
{
var newCost = node.Cost + 1;
var index = indexMap(neighbour);
if (index >= 0 && index < size)
{
var existingNeighbour = Graph[index];

if (existingNeighbour == null || newCost < existingNeighbour.Cost)
{
var next = new Node<T>(neighbour) {Cost = newCost, Index = index};
Graph[next.Index] = next;
if (next.Heuristic < 0 && heuristic != null) next.Heuristic = heuristic(neighbour);
frontier.Enqueue(next, next.Cost + next.Heuristic);

next.Previous = node.Index;
}
}
}
}
}
}


A sample 3D neighbours function:

delegate(Tile tile)
{
var neighbours = new List<Tile>();
if (tile.x - 1 >= 0 && !Tiles[tile.x - 1, tile.y, tile.z].IsBlocking)
if (tile.x + 1 < SizeX && !Tiles[tile.x + 1, tile.y, tile.z].IsBlocking)
if (tile.y + 1 < SizeY && !Tiles[tile.x, tile.y + 1, tile.z].IsBlocking)
if (tile.y - 1 >= 0 && !Tiles[tile.x, tile.y - 1, tile.z].IsBlocking)
if (tile.z - 1 >= 0 && Tiles[tile.x, tile.y, tile.z - 1].IsStairs)
if (tile.z + 1 < SizeZ && Tiles[tile.x, tile.y, tile.z + 1].IsStairs)

return neighbours.ToArray();
};


A sample 3D heuristic function:

delegate (Tile from, Tile to)
{
return Math.Abs(from.x - to.x) + Math.Abs(from.y + to.y) + Math.Abs(from.z + to.z);
};


I was writing a long answer to this but I kept tripping over the fact that you depend on the consumer providing an index mapping, so I'll keep this short and leave a big answer to someone else.

This code would be much easier to use if you didn't pass size to the constructor: it should be provided with the index map.

Failing to provide a 'correct' size results in AddNeighbours silently omitting any node whose index is too high, which would be near enough impossible to debug without code access: this needs to be documented. I'd also consider throwing an exception if an index is provided that is too high so that a consumer knows this has happened: is there a use-case where this silenty rejection is valuable?

public const int MaximumPathLength = 650;


What is this for? Why is it constant? This severly limits the reusability of your code. If this limit is hit (or there is no path), Path just returns an empty array, which is not terribly helpful (I'd must prefer null, or indeed an explicity bool return for "success" with an out param containing the path). If there must be a cutoff, then I'd like it to be configurable.

I also don't like that GeneratePathFromGraph(T, Node<T>) is responsible for handling a null goalNode: I would remove the goalNode != null check. This check should be done in Path. (If GeneratePathFromGraph was public, I would instead suggest you do the check, but throw a helpful exception instead of making up a meaningless result).

BuildGraph is a bit odd. I'd suggest renaming it to FindPath and returning the goalNode, instead of requiring the horrid assignment in the lambda provided in Path.

Node<T> could (and I think should) be immutable (i.e. only readonly fields/properties), with 2 constructors: one for the 'start' Node, and one for 'real' Nodes. Related is this line from AddNeighbours...

if (next.Heuristic < 0 && heuristic != null) next.Heuristic = heuristic(neighbour);


...which I'm pretty sure doesn't work (next.Heuristic will always be 0). I don't understand the point of this check... and this bug wouldn't have crept in if you'd been forced to compute the heuristic before assembling the node.

• Thanks so much for this response, it's really helpful. Good catch on the next.Heuristic < 0 in particular, that's probably left in from a previous design iteration that I missed. Mar 11 '18 at 11:28
• Re BuildGraph, I'd actually forgotten about this until now but there was a reason I wrote it that way: I found myself using this code to build Djikstra maps for other purposes, particularly for things like advertising the utility of an object in an environment, a la The Sims' Smart Objects. This required building the graph to be separate from generating the path from that graph. It also required goalTest be generic, because the goal may be "Tile that advertises greater utility than I". Mar 11 '18 at 11:38
• I can see why the lambda passed in in Path is fugly though. I'll make it a private field named DefaultGoalTest, that should make it clearer. Could also split the class into two, one for pathing and one for graph-building, to better represent a single responsibility. Mar 11 '18 at 11:40
• @Jansky I don't like the assignment in the lambda, primarily because - for it to work - it depends on the implementation of BuildGraph: goalTest clearly says "tell me if this is a goal state", but there is no guarantee of when it will be called (granted this is an example where you can be very confident, but it still puts an 'unwritten' dependency in there). The API for (public) BuildGraph is also unclear, because it depends on this undocumented lambda usage (to retrieve the nearest goal). I wouldn't make the lambda a field, certainly not if you want to keep BuildGraph public! Mar 11 '18 at 11:50

At a quick glance, it looks like you never call UpdatePriority(). This means your pathfinder won't work with non-consistent heuristics. That's fine (it's a common assumption in A* because it allows you to skip that check), but it should be documented.

I'd recommend returning a IList<T> or IEnumerable<T> instead of T[]. It's much more flexible, and allows you to skip the call to ToArray().

PriorityQueue uses float priorities. For int, GenericPriorityQueue<int> may be faster, you'd have to profile.

• Hey BlueRaja, thanks for replying and thanks so much for High-Speed-Priority-Queue. Good points, I'll make these changes. Mar 11 '18 at 11:52