Hi Everyone,

Background:
I have recently implemented the A* pathfinding algorithm in C# based on some pseudo code that I found.
I need the algorithm to run as fast as possible and at present my code isn't quite cutting it. I understand that the heuristics used can have a big impact on the performance of the algorithm, however, I have gotten to the point where I feel that the only way that I can make further gains is to optimise the algorithm itself.

Reason:
The reason why I need the algorithm to be so fast is so that I can 'instantly' determine a decent estimate for the time it would take to travel between two points.

Further Info:
So far I have tried using a hierarchical graph structure to allow at least a partial path to be returned immediately -- The problem with this approach is that I don't actually need path data immediately, I simply need the path to be computed in order to get the time estimate.

I have also considered making the pathfind function unsafe in order to improve performance but I'm not really sure how this would work -- I can't create node pointers because they are "non-unmanaged" types.

This is essentially the first time where I've had to write performance critical code in C# (usually I'm working in C++) so I'm not sure what I could do to really squeeze those gains. If anyone has any ideas then I would greatly appreciate it.

2
Contributors
7
Replies
9
Views
5 Years
Discussion Span
Last Post by spuriousgeek

Without seeing the code and how you implemented the priority queue there really isn't anything we can say, it would all be guesses.

A* algorithm:

using System.Collections.Generic;

namespace Route
{
public delegate float EdgeCostDelegate(GraphEdge edge);

public class RouteFinder
{
ref List<NavigationNode> outPath, HeuristicDelegate heuristic, EdgeCostDelegate costFunc)

{
// Create a priority queue to store the open list of nodes
var openList = new PriorityQueue<float, NavigationNode>();

// Put the start node in the open list
startNode.Cost = 0.0f;
startNode.EstimatedCostToGoal = heuristic(startNode, goalNode);
openList.Enqueue(startNode.EstimatedCostToGoal, startNode);

// While the open list isn't empty
while (!openList.IsEmpty)
{
// Get the smallest element in the open list
currentNode = openList.Dequeue();

// If the current node is the goal node then terminate
if(currentNode.Equals(goalNode))
{
break;
}

// Get all potential successor nodes via edges
var edges = currentNode.Edges;
foreach(var edge in edges)
{

// Set the cost of the successor to be the cost of the current node
// plus the cost to get to the successor node from the current node
var fEdgeCost = costFunc(edge);
var fNeighbourCost = currentNode.Cost + fEdgeCost;

switch (neighbour.State)
{
// If the successor is in the closed list but the current node is as good
// as, or better, then discard the successor and continue
case NodeState.closed:
if (neighbour.Cost <= fNeighbourCost)   // Skip if no shorter route is found
{
continue;
}
// Else, remove the node from the closed list
neighbour.State = NodeState.other;
break;
case NodeState.open:
if (neighbour.Cost <= fNeighbourCost)   // Skip if no shorter route is found
{
//continue;
}
break;
case NodeState.unvisited:    // An unvisited node
// Use heuristic function to calculate H value of neighbour
neighbour.Cost = fNeighbourCost;
neighbour.Predecessor = currentNode;
neighbour.EstimatedCostToGoal = (fNeighbourCost + heuristic(neighbour, goalNode));

// Add the node to the open list
neighbour.State = NodeState.open;
openList.Enqueue(neighbour.EstimatedCostToGoal, neighbour);
break;
default:
break;
}
}

// The current node has already been removed from the open list
// but still needs a status change
currentNode.State = NodeState.closed;
}

// We are at this point either when the goal node has been found or there's no solution
// Determine which:
if(!currentNode.Equals(goalNode))
{
return false;
}

// Work along the path, accumulating nodes
while(!currentNode.Equals(startNode))
{
currentNode = currentNode.Predecessor;
}

// Include the start node

// The path is in reverse order at this point (start is last)
// Use the reverse function:
outPath.Reverse();

return true;
}
}
}

Priority Queue:

using System.Collections.Generic;
using System.Linq;

namespace Route
{
public class PriorityQueue<P, V>
{
private SortedDictionary<P, Queue<V>> _list = new SortedDictionary<P, Queue<V>>();

/// <summary>
/// Enqueue adds elements to the priority queue. First it determines whether or not
/// _list already contains a matching priority entry --> if it does then the sub-queue
/// will have the value added to it.
/// --> Otherwise a new sub-queue will be created to store the value
/// </summary>
/// <param name="priority"></param>
/// <param name="value"></param>
public void Enqueue(P priority, V value)
{
Queue<V> q;
if (!_list.TryGetValue(priority, out q))
{
q = new Queue<V>();
}

q.Enqueue(value);
}

/// <summary>
/// Dequeue removes elements from _list by grabbing the first element and calling
/// the sub-queue's Dequeue function. If at that point there are no more elements
/// in the sub-queue, then the sub-queue is completely removed, otherwise
/// it will be left at the top so the the other elements (with matching priority)
/// are dequeued next time the function is called.
/// </summary>
/// <returns>Returns the highest priority value</returns>
public V Dequeue()
{
// Will throw if there isn't a first element!
var pair = _list.First();
var v = pair.Value.Dequeue();
if (pair.Value.Count == 0) // Nothing left of the top priority (the queue on top)
{
_list.Remove(pair.Key);
}

return v;
}

/// <summary>
/// Determines whether list contains any elements. True if empty, false otherwise
/// </summary>
public bool IsEmpty
{
get { return !_list.Any(); }
}
}
}

I don't know why I didn't provide any code before o_O

A bit different than how I would do this as it only works on graphs (not everything is stored as a graph :)) and I wouldn't have used a SortedDictionary storing Queues. Let me play with this a bit to see what I can come up with.

I should probably mention that I don't mind suggestions on data structure alterations etc. if it improves performance. For example, if you have a more efficient implementation for a PriorityQueue then I would like to see it :) -- Calls to dequeue in my implementation are slightly more costly than I would like

So I wrote a PriorityQueue (which still might be improved) and ran some timing tests against the one you gave above. The three tests are enqueue x items, dequeue x items, enqueue then dequeue x items (which is probably closer to what an A* would be doing)

Timings:
Momerath enqueue = 852 milliseconds for 1,000,000 items
Momerath dequeue = 6357 milliseconds for 1,000,000 items
Momerath en/dequeue = 292 milliseconds for 1,000,000 items
spuriousgeek enqueue = 8535 milliseconds for 1,000,000 items
spuriousgeek dequeue = 5098 milliseconds for 1,000,000 items
spuriousgeek en/dequeue = 966 milliseconds for 1,000,000 items

using System;

namespace rwhittle.Collections.Generic {
public class PriorityQueue<P, D> where P : IComparable {
Node[] nodes = new Node[1];

int position = 1;
public Boolean IsEmpty {
get {
return position <= 1;
}
}

public void Enqueue(P priority, D data) {
Node node = new Node(priority, data);
if (position >= nodes.Length) {
Array.Resize(ref nodes, nodes.Length * 2);
}
nodes[position] = node;
Swim(position);
position++;
}

public D Dequeue() {
if (position <= 1) {
throw new InvalidOperationException("Priority Queue is empty");
}

D value = nodes[1].data;
position--;
nodes[1] = nodes[position];
Sink(1);

return value;
}

private void Sink(int n) {
int t = n << 1;
t = MinNode(t, t + 1);
while (t < position && nodes[n] > nodes[t]) {
Node temp = nodes[n];
nodes[n] = nodes[t];
nodes[t] = temp;
n = t;
t = n << 1;
t = MinNode(t, t + 1);
}
}

private void Swim(int n) {
int t = n >> 1;
while (n > 1 && nodes[n] < nodes[t]) {
Node temp = nodes[n];
nodes[n] = nodes[t];
nodes[t] = temp;
n = t;
t = n >> 1;
}
}

private int MinNode(int a, int b) {
return b < position ? nodes[a] < nodes[b] ? a : b : a < position ? a : position;
}

internal struct Node {
internal P priority;
internal D data;

public Node(P p, D d) {
priority = p;
data = d;
}

public static Boolean operator <(Node a, Node b) {
return a.priority.CompareTo(b.priority) < 0;
}

public static Boolean operator >(Node a, Node b) {
return a.priority.CompareTo(b.priority) >= 0;
}
}
}
}

Well the performance timings definitely seem promising. I will look into implementing this and get back to you when I've had chance to get it done.

I've just done some testing within my application and I have to say the performance has been improved significantly :)
For now I will mark this thread as solved, however if you think of any further optimisations then please feel free to pm me.