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;
}
AddNeighbours(current, heuristic);
}
}
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;
path.Add(node.Position);
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)
neighbours.Add(Tiles[tile.x - 1, tile.y, tile.z]);
if (tile.x + 1 < SizeX && !Tiles[tile.x + 1, tile.y, tile.z].IsBlocking)
neighbours.Add(Tiles[tile.x + 1, tile.y, tile.z]);
if (tile.y + 1 < SizeY && !Tiles[tile.x, tile.y + 1, tile.z].IsBlocking)
neighbours.Add(Tiles[tile.x, tile.y + 1, tile.z]);
if (tile.y - 1 >= 0 && !Tiles[tile.x, tile.y - 1, tile.z].IsBlocking)
neighbours.Add(Tiles[tile.x, tile.y - 1, tile.z]);
if (tile.z - 1 >= 0 && Tiles[tile.x, tile.y, tile.z - 1].IsStairs)
neighbours.Add(Tiles[tile.x, tile.y, tile.z - 1]);
if (tile.z + 1 < SizeZ && Tiles[tile.x, tile.y, tile.z + 1].IsStairs)
neighbours.Add(Tiles[tile.x, tile.y, tile.z + 1]);
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);
};
