Unity : A* Part 1

I recently wrote some grid graph based path finding code, and NavMesh for Unity in C#.  The first few posts will cover Node sorting, Grid Graph building and pathfinding, followed by building a NavMesh and Octree for more complicated pathfinding solutions.  Below are some of the great resources I used to learn the basics.  If you are new to path finding I’d recommend visiting the below sites:

I won’t cover the core concepts in this post.  They are covered far better than I could ever hope to in the above links.  To validate my understanding of the basic concepts I wrote a quick Manhattan based A* console app.  Shown on the right.  Randomly generated obstacles (#’s) and starting at the bottom found my way to the top.  Now to move onto something more complex: a GG in 3d.

GG based Path finding in 3D is the same as 2d.  There are just more possible node connections.  The configuration I used is displayed to the left.

For path finding we need a few key items.  An object to represent our nodes which holds their heuristic cost, path to cost and total cost.  This object must also record who it’s ‘parent’ is.  Next we need a container to handle an ‘Open list’ and a ‘Closed list’ of nodes and finally a method that processes nodes until a path is discovered, or it finds none exist.

To begin i’ll start with the open container (for the closed I used a hashset.)   Unfortunately the Mono Develop library does not contain a sorted set.  So I used a sorted dictionary and made a container that queues nodes based on their total cost.  This container needs to perform four essential functions.

1) it needs to handle the sorting of nodes by their total cost (heuristic + path to this node). To accomplish this I used a sorted dictionary with an Enqueu method.

1 public void Enqueue(Node toEnqueue)
2 {
3   //no key exists for this value we must create a key and queue
4   if (!theDic.TryGetValue(toEnqueue.totalCost, out aQueue))
5   { //came back null create a new Queue for this slot
6       aQueue = new Queue<Node>();
7       aQueue.Enqueue(toEnqueue);
8       theDic.Add(toEnqueue.totalCost, aQueue);
9   }
10   //queue already exists inside the dictionary append to exisiting key
11   else { aQueue.Enqueue(toEnqueue); }
12 }

2) It needs to return the cheapest node

1 /// <summary>
2 /// Dequeues the lowest costing node and returns it.
3 /// </summary>
4 /// <returns></returns>
5 public Node Dequeue()
6 {
7   var nodeQueue= theDic[theDic.Keys.Min()];
8   Node toReturn = nodeQueue.Dequeue();
9   //check if nothing is left if so destroy this key
10   if (nodeQueue.Count == 0) { theDic.Remove(theDic.Keys.Min()); }
11   return toReturn;
12 }

3) The OpenList has to have a way to check how many nodes it holds encase no valid path exists.

1 /// <summary>
2 /// How many Node values are Enqueue
3 /// </summary>
4 public int Count { get { return theDic.Count; } private set { } }

4) a check to see if our list already contains a node

1 /// <summary>
2 /// Does the queue contain this node?
3 /// </summary>
4 /// <param name="toCheck"></param>
5 /// <returns></returns>
6 public bool Contains(Node toCheck)
7 {
8    //See if there is a key containing this value
9    if (!theDic.TryGetValue(toCheck.totalCost, out aQueue)) { return false; }
10    //Key exists does its queue contain this node
11    if (aQueue.Contains(toCheck)) { return true; }
12    return false;
13 }

As one block of code:

1 /// <summary>
2 /// A Priority based sorted queue for nodes ordered by their 'totalValue' property
3 /// Would use a reg sortedset if this existed in Monodevelop
4 /// </summary>
5 class CheapestNodeQueue
6 {
7    Queue<Node> aQueue;
8    private SortedDictionary<int, Queue<Node>> theDic = new SortedDictionary<int, Queue<Node>>();
9    public void Enqueue(Node toEnqueue)
10    {
11 
12        //no key exists for this value we must create a key and queue
13        if (!theDic.TryGetValue(toEnqueue.totalCost, out aQueue))
14        { //came back null create a new Queue for this slot
15            aQueue = new Queue<Node>();
16            aQueue.Enqueue(toEnqueue);
17            theDic.Add(toEnqueue.totalCost, aQueue);
18        }
19        //queue already exists inside the dictionary append to exisiting key
20        else { aQueue.Enqueue(toEnqueue); }
21    }
22 
23    /// <summary>
24    /// Dequeues the lowest costing node and returns it.
25    /// </summary>
26    /// <returns></returns>
27    public Node Dequeue()
28    {
29        var nodeQueue= theDic[theDic.Keys.Min()];
30        Node toReturn = nodeQueue.Dequeue();
31        //check if nothing is left if so destroy this key
32        if (pair.Value.Count == 0) { theDic.Remove(theDic.Keys.Min()); }
33        return toReturn;
34    }
35 
36    /// <summary>
37    /// How many Node values are Enqueue
38    /// </summary>
39    public int Count { get { return theDic.Count; } private set { } }
40 
41    /// <summary>
42    /// Does the queue contain this node?
43    /// </summary>
44    /// <param name="toCheck"></param>
45    /// <returns></returns>
46    public bool Contains(Node toCheck)
47    {
48        //See if there is a key containing this value
49        if (!theDic.TryGetValue(toCheck.totalCost, out aQueue)) { return false; }
50        //Key exists does its queue contain this node
51        if (aQueue.Contains(toCheck)) { return true; }
52        return false;
53    }
54  }

In the next post I’ll cover my Node class and put everything together. Comments, suggestions or questions are always appreciated.  Hopefully these posts are of some help to others looking to learn and implement path finding.
-John