A* Pathfinding in C#

[spuriousgeek]

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.

[Momerath]

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

[spuriousgeek]

A* algorithm:

using System.Collections.Generic;

namespace Route
{
    public delegate float HeuristicDelegate(NavigationNode node, NavigationNode goalNode);
    public delegate float EdgeCostDelegate(GraphEdge edge);

    public class RouteFinder
    {
        public bool ComputePath(NavigationNode startNode, NavigationNode goalNode,
                                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);

            NavigationNode currentNode = 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)
                {
                    var neighbour = (NavigationNode)edge.Head;

                    // 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))
            {
                outPath.Add(currentNode);
                currentNode = currentNode.Predecessor;
            }

            // Include the start node
            outPath.Add(startNode);

            // 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>();
                _list.Add(priority, q);
            }

            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

[Momerath]

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.

[spuriousgeek]

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

[Momerath]

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;
            }
        }
    }
}

[spuriousgeek]

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.

[spuriousgeek]

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.