Morten Nobel's Blog

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);
		frontierMap.Add(fromState, startNode);
		while (true){
			if (frontier.IsEmpty) return null;
			SearchNode<State,Action> node = frontier.Dequeue();
			if (node.state.Equals(toState)) return BuildSolution(node);
			exploredSet.Add(node.state);
			// 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);
					exploredSet.Add(child);
				} 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:

• Aron Granberg’s A* Pathfinding
• AngryAnt’s Behave