Posted by: Morten Nobel-Jørgensen | February 26, 2011

## A* pathfinding algorithm in Unity

A* pathfinding is most likely the most popular AI navigation used in games. It is often used for spatial pathfinding, but the algorithm itself is more general and can be used to other things.

## The general idea

Example of A* pathfinding problem

In pathfinding we want to find the shortest path between two states. Each state has a number of actions, and each action has a cost. In the example to the right, the states are: {S:Init, S:1, S:2, S:3, S:4, S:Goal}. The actions of S:Init is: {S:Init -> S:1, S:Init->S2, S:Init->S3}. With this information it is possible to find a path between two states, but not very efficiently, since we have no idea of what action is the best.

To make better choices and faster find the shortest path we need a guidance in form of a heuristic. The heuristic must estimate the cost from a given node to the goal node. For the algorithm to give the shortest path, the heuristic must be admissible (meaning it must never overestimate).

The pathfinding problem can be expressed in a C# interface using generics (to make it applicable to any kind of pathfinding problem):

```using UnityEngine;
using System.Collections;
using System.Collections.Generic;
/// <summary>
/// Interface for a shortest path problem
/// </summary>
public interface IShortestPath<State,Action> {
/// <summary>
/// Should return a estimate of shortest distance. The estimate must me admissible (never overestimate)
/// </summary>
float Heuristic(State fromLocation, State toLocation);

/// <summary>
/// Return the legal moves from a state
/// </summary>
List<Action> Expand(State position);

/// <summary>
/// Return the actual cost between two adjecent locations
/// </summary>
float ActualCost(State fromLocation, Action action);

/// <summary>
/// Returns the new state after an action has been applied
/// </summary>
State ApplyAction(State location, Action action);
}
```

## A* algorithm

The actual A* search algorithm is fairly simple. The algorithm maintains two sets:

• The explored sets which contains the number of states already explored. Initially empty.
• The frontier set which contains the number of states about to be explored. Initially contains the initial state.

The algorithm will always pick the state from the frontier with the lowest sum of the current cost and heuristic. This state will then be removed from the frontier and added to the explored set. It will then expand the state and add the new states to the frontier. If one of the new states is in the explored set, it will be ignored and if one of the new states is in the frontier with a lower sum of current cost and heuristic, it will replace the existing state in the frontier.

The search terminates when the goal state is taken from the frontier.

The C# implementation looks like this:

```using UnityEngine;
using System;
using System.Collections.Generic;
/// <summary>
/// Based on uniform-cost-search/A* from the book
/// Artificial Intelligence: A Modern Approach 3rd Ed by Russell/Norvig
/// </summary>
public class ShortestPathGraphSearch<State, Action> {
class SearchNode<State,Action> : IComparable<SearchNode<State,Action>> {
public SearchNode<State,Action> parent;
public State state;
public Action action;
public float g; // cost
public float f; // estimate
public SearchNode(SearchNode<State,Action> parent, float g, float f, State state, Action action){
this.parent = parent;
this.g = g;
this.f = f;
this.state = state;
this.action = action;
}
// Reverse sort order (smallest numbers first)
public int CompareTo(SearchNode<State,Action> other)
{
return other.f.CompareTo(f);
}
public override string ToString() {
return "SN {f:"+f+", state: "+state+" action: "+action+"}";
}
}
private IShortestPath<State,Action> info;
public ShortestPathGraphSearch(IShortestPath<State,Action> info){
this.info = info;
}
public List<Action> GetShortestPath(State fromState, State toState){
PriorityQueue<float,SearchNode<State,Action>> frontier = new PriorityQueue<float,SearchNode<State,Action>>();
HashSet<State> exploredSet = new HashSet<State>();
Dictionary<State, SearchNode<State,Action>> frontierMap = new Dictionary<State, SearchNode<State,Action>>();
SearchNode<State, Action> startNode = new SearchNode<State,Action>(null,0,0,fromState, default(Action));
frontier.Enqueue(startNode,0);
while (true){
if (frontier.IsEmpty) return null;
SearchNode<State,Action> node = frontier.Dequeue();
if (node.state.Equals(toState)) return BuildSolution(node);
// expand node and add to frontier
foreach (Action action in info.Expand(node.state)){
State child = info.ApplyAction(node.state, action);
SearchNode<State,Action> frontierNode = null;
bool isNodeInFrontier = frontierMap.TryGetValue(child, out frontierNode);
if (!exploredSet.Contains(child) && !isNodeInFrontier){
SearchNode<State,Action> searchNode = CreateSearchNode(node, action, child, toState);
frontier.Enqueue(searchNode,searchNode.f);
} else if (isNodeInFrontier) {
SearchNode<State,Action> searchNode = CreateSearchNode(node, action, child, toState);
if (frontierNode.f>searchNode.f){
frontier.Replace(frontierNode,frontierNode.f, searchNode.f);
}
}
}
}
}
private SearchNode<State,Action> CreateSearchNode(SearchNode<State,Action> node, Action action, State child, State toState){
float cost = info.ActualCost(node.state, action);
float heuristic = info.Heuristic(child, toState);
return new SearchNode<State,Action>(node, node.g+cost,node.g+cost+heuristic,child,action);
}
private List<Action> BuildSolution(SearchNode<State,Action> seachNode){
List<Action> list = new List<Action>();
while (seachNode != null){
if ((seachNode.action != null ) && (!seachNode.action.Equals(default(Action)))){
list.Insert(0,seachNode.action);
}
seachNode = seachNode.parent;
}
return list;
}
}
```

## A* in Unity

To get started with this simple approach you can download the full sourcecode from GitHub (it contains a PriorityQueue implementation and a iOS workaround):
https://github.com/mortennobel/UnityUtils

Note that my implementation is very general. There also exist multiple other pathfinding solutions for Unity, that let you setup navigation points directly in the editor:

## Responses

1. Hi, Morten.
I’ve made another path finding demo which used A* algorithm on the hexagon grid map.
This web demo and the source code are on my site.Check it.

2. Hello,
I’m trying to figure out how to apply this algorithm on a grid of 20×50 and I could sure use some guidance.

Is there some sort of Dictionary or Array input that I can send to the algorithm and get the shortest path. Read you blog entry 3 times now and still did not figure it out (did not sleep almost 2 days and I’m rather new to Unity and I hope that is the reason why I did not understand yet) :).

Looking forward to hearing from you.

• Try to take a look at this tutorial. This should give you a little more insight on how to use the A* algorithm:
http://www.policyalmanac.org/games/aStarTutorial.htm

• So what I have is a lets say List that has the StartNode and the a SubList with all the surrounding Nodes and so on recurrently for each Node ?

• Yes that’s basically it 🙂 Feel free to post an question with your code on http://answers.unity3d.com . I think that is a better forum if you have a specific problem. And post a link here, so I can find it 🙂

3. Ok. Just one more thing… (sorry to bother you and thank you for your time and prompt replies).

Let’s say I have a grid of 20×50 and the StartPoint is [1,1] and EndPoint is [20,50]. What function in your script do you call to get the shortest path return ?

• When you have setup your A* script you call GetShortestPath to get the list of actions (that will give you the shortest path).

Take a look at this example:

https://github.com/mortennobel/UnityUtils/blob/master/pathfinding/_test/ShortestPathTest.cs

• Thank you again for the time.

• Hello again,
I am writing to you since I did not get not one post on the Unity Answer page, and I am quite desperate. I spent more than 2 days now trying to understand how to implement your code and with no luck. I feel like I want to throw my laptop out the window.

I am not that good at C#, but I just don’t understand what function is called and with what input.

You said earlier that I should call GetShortestPath. I presume it is the one in the ShortestPathGraphSearch.cs file. Ok. It says : GetShortestPath(State fromState, State toState). I assumed that fromState is the StartPoint and toState is the EndPoint.

If so, and this is all the data that needs to be sent to the script, then how does the script know what square (node) is walkable and which one is not ? I assume that I am suppose to send some kind of a Dictionary of Nodes, but I just can’t find an input for a List or Dictionary anywhere.

I disected your code non stop and I still don’t get it. I undestand how the algorithm is suppose to work, and I guess I could write a code that would follow that algorithm , by since you provided a code (and a great one I might add), there is no reason to write my own one.

Please bare with me and help me out so I can finally have a good night sleep.

• Hi again.

First of all, A* programming is advanced stuff and I believe that there is a few ‘plug-and-play’ alternatives available for Unity (See links in the end of the article). My code solves A* in the general case, but you need to write some amount of code to integrate it into any game.

I hope you soon find a solution, that your laptop survives and that you end the end get your sleep cycle back to normal:)

• Hello and Thank you for your time (again).
I will post back a reply once I have figured it out 🙂 (almost there) 🙂

4. Hi, theres a slight error in ShortestPathTest.cs that may cause a confusion.

it implements float ActualCost(State fromLocation, Action action); as

public float ActualCost(Vector2i fromLocation, Vector2i toLocation)

which works in your example because of your coordinate system. Its not a big thing, but may save some head scratching if altered.

5. Hello fellow dev:
Im about to release a Unity Editor tool that uses your serializable dictionaries, and I want to know if it’ll be OK to include a copy of your library inside my package, so user don’t need to import it from a different file. (Proper reference to your site will be provided as well, of course)

Hope to hear from you soon
Cheers
Nico