Implement a Generic Pathfinder in Unity using C#

In this tutorial, we will implement a generic pathfinder in Unity using C#. We will approach the tutorial from a basic 2D grid-based pathfinding to a more robust generic pathfinder that we can use for a graph-based data structure. We will then apply our pathfinder to various pathfinding problems (8-Puzzle, rectangular grid-based map and graph-based map).

Find the GitHub repository of this tutorial at https://github.com/shamim-akhtar/tutorial-pathfinding.

Contact me

Read also

• Part 2 – 2D Grid-Based Pathfinding Using C# and Unity
• Part 3 – 8-Puzzle Problem Using A* in C# and Unity
• Part 4 – Graph-Based Pathfinding Using C# in Unity

Pathfinding

Pathfinding is plotting the most optimal route between two points by a computer application. The most optimal route is usually associated with identifying the path that best meets the problem’s objective. It could be finding the shortest, the cheapest, the fastest, or other criteria defined by the problem between two points in an extensive network of points.

Pathfinding determines a valid path for a character or object in a game to move from one location to another while navigating around obstacles and considering the game’s rules and constraints. It is a fundamental technique used in game development to create realistic and intelligent movements for characters within the game world.

Imagine you’re playing a game where you control a character and need to guide them from one point to another. Pathfinding algorithms help the game determine the most efficient and obstacle-free route for the character to follow. This is important because you want the character to move intelligently, avoiding obstacles like walls, trees, or other characters and reaching their destination smoothly.

Pathfinding algorithms use various techniques and data structures to search through the game environment and find the optimal path. These algorithms take into account factors like the layout of the game world, the presence of obstacles, the character’s movement capabilities, and any additional constraints or requirements.

A popular algorithm for pathfinding is called A* (pronounced “A-star”). A* combines breadth-first search and heuristic evaluation elements to find the best path efficiently. It uses a heuristic function to estimate the cost of reaching the destination from each node, allowing it to prioritize paths that appear to be more promising. By exploring the game world using A*, the algorithm can quickly find a path that is both efficient and avoids obstacles.

Pathfinding algorithms are essential in games to provide realistic and believable movement for characters. They are commonly used in various game genres, including role-playing (RPGs), real-time strategy (RTS), and puzzle games. Implementing pathfinding algorithms requires a good understanding of data structures and algorithms.

Understanding pathfinding in game navigation can provide valuable knowledge for game development or any field involving real-time navigation and movement, such as robotics, simulations, or virtual reality.

Algorithms

We can use a wide variety of algorithms for pathfinding problems. In this tutorial, we will implement the three most commonly used algorithms. These are Djikstra’s, Astar and Greedy best-first.

For a detailed understanding of pathfinding, I suggest the brilliant tutorial maintained by Amit on the Red Blob games site.

In this tutorial, I will not go through the detailed theory of all these pathfinding algorithms but implement all necessary functions for each of these pathfinding algorithms and apply them to:

1. Grid-based path finding problem
2. The 8 puzzle problem
3. Graph-based road network problem

Unity Project

Now, let’s dive right into our implementation. Our objective is to create a robust and generic pathfinder that can work in many applications.

Create a new Unity 2D project.

Rename the default scene to Demo_RectGridPathFinding. We will use this project as the base for creating our pathfinder and test out our pathfinder for a 2D grid-based problem.

Please create a new C# file and name it PathFinder.cs. Open the file in Visual Studio. We will place our pathfinding related codes with the namespace GameAI/PathFinding. You can choose your namespace as well.

namespace GameAI
{
  namespace PathFinding
  {
  }
}
Code language: C# (cs)

The Status

We start by defining an enumeration type to represent the different types of Status that a pathfinder can have at any given time.

namespace GameAI
{
  namespace PathFinding
  {
    // An enumeration type to represent the status of the 
    // pathfinder at any given time.
    public enum PathFinderStatus
    {
      NOT_INITIALIZED,
      SUCCESS,
      FAILURE,
      RUNNING,
    }
  }
}
Code language: C# (cs)

The Node

Next, we create the abstract Node class. The Node class is the base class for all other derived vertex of a pathfinding problem. We will see how we can implement a concrete version of this Node for our specific problem types in the following sections.

At this juncture, I want to point out that a pathfinder doesn’t need to know the entire map (or, in other words, doesn’t need to have the visibility of the whole map) at any given time.

Instead, it should only know which Nodes it can move from any given Node (and if the cost is not uniform, what is the cost of that move). To get this information from a Node, we define an abstract virtual function called GetNeightbours that returns a list of Nodes.

// The Noce class. 
// It is an abstract class that provides the base class
// for any type of vertex that you want to implement in
// your path finding problem.
abstract public class Node<T>
{
  // We store a reference to the T as Value.
  public T Value { get; private set; }

  // The constructor for the Node class.
  public Node(T value)
  {
    Value = value;
  }

  // Get the neighbours for this node. 
  // This is the most important function that 
  // your concrete vertex class should implement.
  abstract public List<Node<T>> GetNeighbours();
}
Code language: C# (cs)

The below diagrams illustrate the concept of neighbours for a rectangular grid-based pathfinding problem.

Similarly, the following diagrams illustrate the concept of neighbours for a graph-based problem.

The PathFinder

Our objective is to create a generic and robust pathfinder. Ideally, we do not want to hardcode any specific implementation, so we do not want to restrict the use of our pathfinder to only a particular domain.

• To do so, we, first of all, allow the user of the pathfinder to be able to create a specific concrete pathfinder such as AStarPathFinder, DijkstraPathFinder and GreedyPathFinder.
• Then the user should be able to initialize the pathfinder with a start and destination Node.
• Set the type of cost function that the user wants to apply
• Finally, the user should repeatedly call a step function until the pathfinder returns either a success or a failure status.

We want to allow the user to have the flexibility to create a specific concrete type of pathfinder. So, we will implement our pathfinder class as an abstract class and then create three concrete classes that will derive the base pathfinder class and implement the algorithm-specific implementation.

// The abstract generic PathFinder class that implements the core
// pathfinding related codes.
abstract public class PathFinder<T>
{

}
Code language: C# (cs)

We add a property called Status that holds the current Status of the pathfinder. By default, we set it to NOT_INITIALIZED. Also, note that we have made the set attribute for this property private to ensure that only this class can change and set the Status.

#region Properties

// Add a property that holds the current status of the
// pathfinder. By default it is set to NOT_INITIALIZED.
// Also note that we have made the set to private to 
// ensure that only this class can change and set
// the status.
public PathFinderStatus Status
{
  get;
  private set;
} = PathFinderStatus.NOT_INITIALIZED;

#endregion


Code language: C# (cs)

Next, we add the properties for the Start and the Goal Nodes.

// Add properties for the start and goal nodes.
public Node<T> Start { get; private set; }
public Node<T> Goal { get; private set; }
Code language: C# (cs)

We want our cost function to be just a signature and let the caller implement the actual cost function based on their problem statement.

Hence, next, we define a delegate and have two properties to hold the HeuristicCost cost function and the NodeTraversalCost cost function.

// Create a delegate that defines the signature
// for calculating the cost between two 
// Nodes (T which makes a Node)
public delegate float CostFunction(T a, T b);
public CostFunction HeuristicCost { get; set; }
public CostFunction NodeTraversalCost { get; set; }

Code language: C# (cs)

The HeuristicCost function property allows the user to define a heuristic cost function (Manhattan cost, Euclidean cost or any other cost function).

Similarly, the NodeTraversalCost function allows the user to define the cost of traversing from one Node to another.

We will see the actual implementation of these two functions later when we implement the concrete implementation.

The PathFinderNode

We will now implement the PathFinderNode class. This class is a node of the tree generated by the pathfinder when searching. This class encapsulates the Node and holds other attributes that allow us to traverse up via the Parent-child rule of tree traversal.

// The PathFinderNode class.
// This class equates to a node in a the tree generated
// by the pathfinder in its search for the most optimal
// path. Do not confuse this with the Node class on top.
// This class encapsulates a Node and hold other attributes
// needed for the search traversal.
// The pathfinder creates instances of this class at runtime
// while doing the search.
public class PathFinderNode
{
  // The parent of this node.
  public PathFinderNode Parent { get; set; }

  // The Node that this PathFinderNode is pointing to.
  public Node<T> Location { get; private set; }

  // The various costs.
  public float Fcost { get; private set; }
  public float GCost { get; private set; }
  public float Hcost { get; private set; }

  // The constructor.
  // It takes in the Node, the parent, the gvost and the hcost.
  public PathFinderNode(Node<T> location,
      PathFinderNode parent,
      float gCost,
      float hCost)
  {
    Location = location;
    Parent = parent;
    Hcost = hCost;
    SetGCost(gCost);
  }

  // Set the gcost. 
  public void SetGCost(float c)
  {
    GCost = c;
    Fcost = GCost + Hcost;
  }
}

Code language: C# (cs)

We will add a property that holds the current node that the pathfinder is at while searching.

// The property to access the CurrentNode that the
// pathfinder is now at.
public PathFinderNode CurrentNode { get; private set; }
Code language: C# (cs)

The Open and Closed List

Next, we add two lists to serve the open list and the closed list for the PathFinderNode.

#region Open and Closed lists and associated functions
// The open list for the path finder.
protected List<PathFinderNode> mOpenList = new List<PathFinderNode>();
// The closed list
protected List<PathFinderNode> mClosedList = new List<PathFinderNode>();

#endregion
Code language: C# (cs)

The open list contains the nodes we want to explore, whereas the closed list contains the nodes we have already explored. The open list needs to be sorted according to the cost because we want to explore the Node that is least in cost first.

There are several ways to implement this list. Ideally, we want to use a PriorityQueue that keeps a sorted list of nodes according to their costs.

For our tutorial, will we not concentrate on the performance issue right now and implement both the open and the closed list as norma lists. Whenever we request a node from the open list, we will search for the Node with the least cost and return that Node. So, let’s go ahead and implement a helper method called GetLeastCostNode.

// A helper method to find the least cost node from a list
protected PathFinderNode GetLeastCostNode(List<PathFinderNode> myList)
{
  int best_index = 0;
  float best_priority = myList[0].Fcost;
  for (int i = 1; i < myList.Count; i++)
  {
    if (best_priority > myList[i].Fcost)
    {
      best_priority = myList[i].Fcost;
      best_index = i;
    }
  }
  PathFinderNode n = myList[best_index];
  return n;
}Code language: C# (cs)

The input to the above method is a list of PathFinderNode. The function searches for the Node with the least cost and returns it.

Next, we implement another helper method that checks if a specific value of type T is in a list of PathFinderNode. If it does, then return the index of the item from the list. If it doesn’t, then return -1.

// A helper method to check if a value of T is in a list.
// If it is then return the index of the item where the
// value is. Otherwise return -1.
protected int IsInList(List<PathFinderNode> myList, T cell)
{
  for (int i = 0; i < myList.Count; ++i)
  {
    if (EqualityComparer<T>.Default.Equals(myList[i].Location.Value, cell))
      return i;
  }
  return -1;
}
Code language: C# (cs)

We have now created all the necessary functionality for us to implement the pathfinding search. Our next step is to implement pathfinding in two stages.

In our first stage, we initialize the PathFinder by setting all the necessary parameters. Then in the second stage, we implement the inner loop of the search as a Step method.

But before we do that, let’s add some delegates to allow the user the flexibility to add some action callbacks on specific stages of the code.

Add the following delegates to our PathFinder class.

#region Delegates for action callbacks.
// Some callbacks to handle on changes to the internal values.
// these callbacks can be used by the game to display visually the
// changes to the cells and lists.
public delegate void DelegatePathFinderNode(PathFinderNode node);
public DelegatePathFinderNode onChangeCurrentNode;
public DelegatePathFinderNode onAddToOpenList;
public DelegatePathFinderNode onAddToClosedList;
public DelegatePathFinderNode onDestinationFound;

public delegate void DelegateNoArgument();
public DelegateNoArgument onStarted;
public DelegateNoArgument onRunning;
public DelegateNoArgument onFailure;
public DelegateNoArgument onSuccess;
#endregion
Code language: C# (cs)

Initialize Method

In the initialize method, as explained earlier, we do the necessary setup for our pathfinding search. We start the initialize method by checking if the PathFinder is already running with an existing pathfinding search. If so, then we do not allow a new search and return false.

If the pathfinder status is not equal to RUNNING, we reset all the internal variables by calling the Reset method.

After that, we set the start and the goal nodes to Start and Goal variables, respectively, calculate the Heuristic cost from the Start to the Goal nodes and create a root node of the search tree with null as the parent.

// Stage 1. Initialize the serach.
// Initialize a new search.
// Note that a search can only be initialized if 
// the path finder is not already running.
public bool Initialize(Node<T> start, Node<T> goal)
{
  if (Status == PathFinderStatus.RUNNING)
  {
    // Path finding is already in progress.
    return false;
  }

  // Reset the variables.
  Reset();

  // Set the start and the goal nodes for this search.
  Start = start;
  Goal = goal;

  // Calculate the H cost for the start.
  float H = HeuristicCost(Start.Value, Goal.Value);

  // Create a root node with its parent as null.
  PathFinderNode root = new PathFinderNode(Start, null, 0f, H);

  // add this root node to our open list.
  mOpenList.Add(root);

  // set the current node to root node.
  CurrentNode = root;

  // Invoke the deletages to inform the caller if the delegates are not null.
  onChangeCurrentNode?.Invoke(CurrentNode);
  onStarted?.Invoke();

  // set the status of the pathfinder to RUNNING.
  Status = PathFinderStatus.RUNNING;
  return true;
}

Code language: C# (cs)

The Step Method

The Step method is where we will implement the inner loop for the pathfinding search. In other tutorials, you will often find the initialize and the step within a while loop together.

However, in this tutorial, I will make them separate and let the user decide how they want to arrange their code for a pathfinding search.

Typically, the user will call as below—sample code.

// Sample code 
// Create a specific pathfinder for example
mPathFinder = new AStarPathFinder<Vector2Int>();
mPathFinder.NodeTraversalCost = RectGridMap.GetCostBetweenTwoCells;
mPathFinder.HeuristicCost = RectGridMap.GetManhattanCost;

mPathFinder.Initialize(start, goal);
// blocking example. Use coroutines or threads for non blocking code.
while (mPathFinder.Status == PathFinderStatus.RUNNING)
{
  mPathFinder.Step();
}

if (mPathFinder.Status == PathFinderStatus.FAILURE)
{
  Debug.Log("Pathfinder could not find the path to the destination.");
}

if (mPathFinder.Status == PathFinderStatus.SUCCESS)
{
  Debug.Log("Destination found.");
}
Code language: C# (cs)

Separating the pathfinding calls to Initialize and Step allows us better flexibility and control over the code. The user can use coroutines or threads to manage the pathfinding.

The below is the code for the Step method. There are comments within the code that explains each line.

// Stage 2: Step until success or failure
// Take a search step. The user must continue to call this method 
// until the Status is either SUCCESS or FAILURE.
public PathFinderStatus Step()
{
  // Add the current node to the closed list.
  mClosedList.Add(CurrentNode);

  // Call the delegate to inform any subscribers.
  onAddToClosedList?.Invoke(CurrentNode);

  if (mOpenList.Count == 0)
  {
    // we have exhausted our search. No solution is found.
    Status = PathFinderStatus.FAILURE;
    onFailure?.Invoke();
    return Status;
  }

  // Get the least cost element from the open list. 
  // This becomes our new current node.
  CurrentNode = GetLeastCostNode(mOpenList);

  // Call the delegate to inform any subscribers.
  onChangeCurrentNode?.Invoke(CurrentNode);

  // Remove the node from the open list.
  mOpenList.Remove(CurrentNode);

  // Check if the node contains the Goal cell.
  if (EqualityComparer<T>.Default.Equals(
      CurrentNode.Location.Value, Goal.Value))
  {
    Status = PathFinderStatus.SUCCESS;
    onDestinationFound?.Invoke(CurrentNode);
    onSuccess?.Invoke();
    return Status;
  }

  // Find the neighbours.
  List<Node<T>> neighbours = CurrentNode.Location.GetNeighbours();

  // Traverse each of these neighbours for possible expansion.
  foreach (Node<T> cell in neighbours)
  {
    AlgorithmSpecificImplementation(cell);
  }

  Status = PathFinderStatus.RUNNING;
  onRunning?.Invoke();
  return Status;
}

abstract protected void AlgorithmSpecificImplementation(Node<T> cell);
Code language: C# (cs)

In the above code, line number 46 is highlighted. Using the function AlgorithmSpecificImplementation separates the implementation of algorithm-specific code.

If you notice, pathfinding code usually has the same flow for most of the algorithms. There are some differences between each of the three different algorithms. Viz; Astar, Djikstra and Greedy best-first. We will capture these differences in three concrete classes.

We will implement these three concrete classes after the next section.

The Reset Method

The Reset method is an internal protected method. It is where we clear our internal lists, set the CurrentNode to null and prepare the stage for a new pathfinding search.

// Reset the internal variables for a new search.
protected void Reset()
{
  if (Status == PathFinderStatus.RUNNING)
  {
    // Cannot reset path finder. Path finding in progress.
    return;
  }

  CurrentNode = null;

  mOpenList.Clear();
  mClosedList.Clear();

  Status = PathFinderStatus.NOT_INITIALIZED;
}
Code language: C# (cs)

Algorithm Specific Classes

We will now implement the algorithm-specific codes in three separate classes that derive from the PathFinder base class.

Instead of using inheritance, you could also have used an enumeration type within the PathFinder class and hardcode three implementations. 
However, I prefer to arrange the code using inheritance. That way we can make reusability better.

AStarPathFinder

For Astar implementation, we need to use both the H (Heuristic) cost and the G (the accumulated cost until the current node) cost.

public class AStarPathFinder<T> : PathFinder<T>
{
  protected override void AlgorithmSpecificImplementation(Node<T> cell)
  {
    // first of all check if the node is already in the closedlist.
    // if so then we do not need to continue search for this node.
    if (IsInList(mClosedList, cell.Value) == -1)
    {
      // The cell does not exist in the closed list.

      // Calculate the cost of the node from its parent.
      // Remember G is the cost from the start till now.
      // So to get G we will get the G cost of the currentNode
      // and add the cost from currentNode to this cell.
      // We can actually implement a function to calculate the cost 
      // between two adjacent cells. 

      float G = CurrentNode.GCost + NodeTraversalCost(
          CurrentNode.Location.Value, cell.Value);

      float H = HeuristicCost(cell.Value, Goal.Value);

      // Check if the cell is already there in the open list.
      int idOList = IsInList(mOpenList, cell.Value);
      if (idOList == -1)
      {
        // The cell does not exist in the open list.
        // We will add the cell to the open list.

        PathFinderNode n = new PathFinderNode(cell, CurrentNode, G, H);
        mOpenList.Add(n);
        onAddToOpenList?.Invoke(n);
      }
      else
      {
        // if the cell exists in the openlist then check if the G cost 
        // is less than the one already in the list.
        float oldG = mOpenList[idOList].GCost;
        if (G < oldG)
        {
          // change the parent and update the cost to the new G
          mOpenList[idOList].Parent = CurrentNode;
          mOpenList[idOList].SetGCost(G);
          onAddToOpenList?.Invoke(mOpenList[idOList]);
        }
      }
    }
  }
}

Code language: C# (cs)

DijkstraPathFinder

For Djikstra implementation, we only use the G (the accumulated cost until the current node) cost. We do not use the heuristic cost for Djikstra’s implementation.

public class DijkstraPathFinder<T> : PathFinder<T>
{
  protected override void AlgorithmSpecificImplementation(Node<T> cell)
  {
    if (IsInList(mClosedList, cell.Value) == -1)
    {
      float G = CurrentNode.GCost + NodeTraversalCost(
          CurrentNode.Location.Value, cell.Value);

      //Dijkstra doesn't include the Heuristic cost
      float H = 0.0f;

      // Check if the cell is already there in the open list.
      int idOList = IsInList(mOpenList, cell.Value);
      if (idOList == -1)
      {
        // The cell does not exist in the open list.
        // We will add the cell to the open list.

        PathFinderNode n = new PathFinderNode(cell, CurrentNode, G, H);
        mOpenList.Add(n);
        onAddToOpenList?.Invoke(n);
      }
      else
      {
        // if the cell exists in the openlist then check if the G cost is less than the 
        // one already in the list.
        float oldG = mOpenList[idOList].GCost;
        if (G < oldG)
        {
          // change the parent and update the cost to the new G
          mOpenList[idOList].Parent = CurrentNode;
          mOpenList[idOList].SetGCost(G);
          onAddToOpenList?.Invoke(mOpenList[idOList]);
        }
      }
    }
  }
}

Code language: C# (cs)

Greedy best-first

For Greedy best-first implementation, we need to use only the H (Heuristic) cost. We do not use the G cost for Greedy best-first implementation.

public class GreedyPathFinder<T> : PathFinder<T>
{
  protected override void AlgorithmSpecificImplementation(Node<T> cell)
  {
    if (IsInList(mClosedList, cell.Value) == -1)
    {
      //Greedy best-first does doesn't include the G cost
      float G = 0.0f;
      float H = HeuristicCost(cell.Value, Goal.Value);

      // Check if the cell is already there in the open list.
      int idOList = IsInList(mOpenList, cell.Value);
      if (idOList == -1)
      {
        // The cell does not exist in the open list.
        // We will add the cell to the open list.

        PathFinderNode n = new PathFinderNode(cell, CurrentNode, G, H);
        mOpenList.Add(n);
        onAddToOpenList?.Invoke(n);
      }
    }
  }
}
Code language: C# (cs)

In our next tutorial, we will use this PathFinder and apply it to solve the 8-Puzzle problem – 8-Puzzle Problem Using A* in C# and Unity.

Complete PathFinder Code

using System.Collections.Generic;

namespace GameAI
{
  namespace PathFinding
  {
    // An enumeration type to represent the status of the 
    // pathfinder at any given time.
    public enum PathFinderStatus
    {
      NOT_INITIALIZED,
      SUCCESS,
      FAILURE,
      RUNNING,
    }

    // The Noce class. 
    // It is an abstract class that provides the base class
    // for any type of vertex that you want to implement in
    // your path finding problem.
    abstract public class Node<T>
    {
      // We store a reference to the T as Value.
      public T Value { get; private set; }

      // The constructor for the Node class.
      public Node(T value)
      {
        Value = value;
      }

      // Get the neighbours for this node. 
      // This is the most important function that 
      // your concrete vertex class should implement.
      abstract public List<Node<T>> GetNeighbours();
    }

    // The abstract PathFinder class that implements the core
    // pathfinding related codes.
    abstract public class PathFinder<T>
    {

      #region Delegates for cost calculation
      // Create a delegate that defines the signature
      // for calculating the cost between two 
      // Nodes (T which makes a Node)
      public delegate float CostFunction(T a, T b);
      public CostFunction HeuristicCost { get; set; }
      public CostFunction NodeTraversalCost { get; set; }
      #endregion

      #region PathFinderNode
      // The PathFinderNode class.
      // This class equates to a node in a the tree generated
      // by the pathfinder in its search for the most optimal
      // path. Do not confuse this with the Node class on top.
      // This class encapsulates a Node and hold other attributes
      // needed for the search traversal.
      // The pathfinder creates instances of this class at runtime
      // while doing the search.
      public class PathFinderNode
      {
        // The parent of this node.
        public PathFinderNode Parent { get; set; }

        // The Node that this PathFinderNode is pointing to.
        public Node<T> Location { get; private set; }

        // The various costs.
        public float Fcost { get; private set; }
        public float GCost { get; private set; }
        public float Hcost { get; private set; }

        // The constructor.
        // It takes in the Node, the parent, the gvost and the hcost.
        public PathFinderNode(Node<T> location,
            PathFinderNode parent,
            float gCost,
            float hCost)
        {
          Location = location;
          Parent = parent;
          Hcost = hCost;
          SetGCost(gCost);
        }

        // Set the gcost. 
        public void SetGCost(float c)
        {
          GCost = c;
          Fcost = GCost + Hcost;
        }
      }
      #endregion

      #region Properties

      // Add a property that holds the current status of the
      // pathfinder. By default it is set to NOT_INITIALIZED.
      // Also note that we have made the set to private to 
      // ensure that only this class can change and set
      // the status.
      public PathFinderStatus Status
      {
        get;
        private set;
      } = PathFinderStatus.NOT_INITIALIZED;

      // Add properties for the start and goal nodes.
      public Node<T> Start { get; private set; }
      public Node<T> Goal { get; private set; }

      // The property to access the CurrentNode that the
      // pathfinder is now at.
      public PathFinderNode CurrentNode { get; private set; }

      #endregion

      #region Open and Closed lists and associated functions
      // The open list for the path finder.
      protected List<PathFinderNode> mOpenList = new List<PathFinderNode>();

      // The closed list
      protected List<PathFinderNode> mClosedList = new List<PathFinderNode>();

      // A helper method to find the least cost node from a list
      protected PathFinderNode GetLeastCostNode(List<PathFinderNode> myList)
      {
        int best_index = 0;
        float best_priority = myList[0].Fcost;
        for (int i = 1; i < myList.Count; i++)
        {
          if (best_priority > myList[i].Fcost)
          {
            best_priority = myList[i].Fcost;
            best_index = i;
          }
        }

        PathFinderNode n = myList[best_index];
        return n;
      }

      // A helper method to check if a value of T is in a list.
      // If it is then return the index of the item where the
      // value is. Otherwise return -1.
      protected int IsInList(List<PathFinderNode> myList, T cell)
      {
        for (int i = 0; i < myList.Count; ++i)
        {
          if (EqualityComparer<T>.Default.Equals(myList[i].Location.Value, cell))
            return i;
        }
        return -1;
      }

      #endregion

      #region Delegates for action callbacks.
      // Some callbacks to handle on changes to the internal values.
      // these callbacks can be used by the game to display visually the
      // changes to the cells and lists.
      public delegate void DelegatePathFinderNode(PathFinderNode node);
      public DelegatePathFinderNode onChangeCurrentNode;
      public DelegatePathFinderNode onAddToOpenList;
      public DelegatePathFinderNode onAddToClosedList;
      public DelegatePathFinderNode onDestinationFound;

      public delegate void DelegateNoArgument();
      public DelegateNoArgument onStarted;
      public DelegateNoArgument onRunning;
      public DelegateNoArgument onFailure;
      public DelegateNoArgument onSuccess;
      #endregion

      #region Actual path finding search functions
      // Stage 1. Initialize the serach.
      // Initialize a new search.
      // Note that a search can only be initialized if 
      // the path finder is not already running.
      public bool Initialize(Node<T> start, Node<T> goal)
      {
        if (Status == PathFinderStatus.RUNNING)
        {
          // Path finding is already in progress.
          return false;
        }

        // Reset the variables.
        Reset();

        // Set the start and the goal nodes for this search.
        Start = start;
        Goal = goal;

        // Calculate the H cost for the start.
        float H = HeuristicCost(Start.Value, Goal.Value);

        // Create a root node with its parent as null.
        PathFinderNode root = new PathFinderNode(Start, null, 0f, H);

        // add this root node to our open list.
        mOpenList.Add(root);

        // set the current node to root node.
        CurrentNode = root;

        // Invoke the deletages to inform the caller if the delegates are not null.
        onChangeCurrentNode?.Invoke(CurrentNode);
        onStarted?.Invoke();

        // set the status of the pathfinder to RUNNING.
        Status = PathFinderStatus.RUNNING;

        return true;
      }

      // Stage 2: Step until success or failure
      // Take a search step. The user must continue to call this method 
      // until the Status is either SUCCESS or FAILURE.
      public PathFinderStatus Step()
      {
        // Add the current node to the closed list.
        mClosedList.Add(CurrentNode);

        // Call the delegate to inform any subscribers.
        onAddToClosedList?.Invoke(CurrentNode);

        if (mOpenList.Count == 0)
        {
          // we have exhausted our search. No solution is found.
          Status = PathFinderStatus.FAILURE;
          onFailure?.Invoke();
          return Status;
        }

        // Get the least cost element from the open list. 
        // This becomes our new current node.
        CurrentNode = GetLeastCostNode(mOpenList);

        // Call the delegate to inform any subscribers.
        onChangeCurrentNode?.Invoke(CurrentNode);

        // Remove the node from the open list.
        mOpenList.Remove(CurrentNode);

        // Check if the node contains the Goal cell.
        if (EqualityComparer<T>.Default.Equals(
            CurrentNode.Location.Value, Goal.Value))
        {
          Status = PathFinderStatus.SUCCESS;
          onDestinationFound?.Invoke(CurrentNode);
          onSuccess?.Invoke();
          return Status;
        }

        // Find the neighbours.
        List<Node<T>> neighbours = CurrentNode.Location.GetNeighbours();

        // Traverse each of these neighbours for possible expansion.
        foreach (Node<T> cell in neighbours)
        {
          AlgorithmSpecificImplementation(cell);
        }

        Status = PathFinderStatus.RUNNING;
        onRunning?.Invoke();
        return Status;
      }

      abstract protected void AlgorithmSpecificImplementation(Node<T> cell);

      // Reset the internal variables for a new search.
      protected void Reset()
      {
        if (Status == PathFinderStatus.RUNNING)
        {
          // Cannot reset path finder. Path finding in progress.
          return;
        }

        CurrentNode = null;

        mOpenList.Clear();
        mClosedList.Clear();

        Status = PathFinderStatus.NOT_INITIALIZED;
      }

      #endregion
    }

    #region AstarPathFinder
    // The AstarPathFinder.
    public class AStarPathFinder<T> : PathFinder<T>
    {
      protected override void AlgorithmSpecificImplementation(Node<T> cell)
      {
        // first of all check if the node is already in the closedlist.
        // if so then we do not need to continue search for this node.
        if (IsInList(mClosedList, cell.Value) == -1)
        {
          // The cell does not exist in the closed list.

          // Calculate the cost of the node from its parent.
          // Remember G is the cost from the start till now.
          // So to get G we will get the G cost of the currentNode
          // and add the cost from currentNode to this cell.
          // We can actually implement a function to calculate the cost 
          // between two adjacent cells. 

          float G = CurrentNode.GCost + NodeTraversalCost(
              CurrentNode.Location.Value, cell.Value);

          float H = HeuristicCost(cell.Value, Goal.Value);

          // Check if the cell is already there in the open list.
          int idOList = IsInList(mOpenList, cell.Value);
          if (idOList == -1)
          {
            // The cell does not exist in the open list.
            // We will add the cell to the open list.

            PathFinderNode n = new PathFinderNode(cell, CurrentNode, G, H);
            mOpenList.Add(n);
            onAddToOpenList?.Invoke(n);
          }
          else
          {
            // if the cell exists in the openlist then check if the G cost 
            // is less than the one already in the list.
            float oldG = mOpenList[idOList].GCost;
            if (G < oldG)
            {
              // change the parent and update the cost to the new G
              mOpenList[idOList].Parent = CurrentNode;
              mOpenList[idOList].SetGCost(G);
              onAddToOpenList?.Invoke(mOpenList[idOList]);
            }
          }
        }
      }
    }
    #endregion

    #region DijkstraPathFinder
    public class DijkstraPathFinder<T> : PathFinder<T>
    {
      protected override void AlgorithmSpecificImplementation(Node<T> cell)
      {
        if (IsInList(mClosedList, cell.Value) == -1)
        {
          float G = CurrentNode.GCost + NodeTraversalCost(
              CurrentNode.Location.Value, cell.Value);

          //Dijkstra doesn't include the Heuristic cost
          float H = 0.0f;

          // Check if the cell is already there in the open list.
          int idOList = IsInList(mOpenList, cell.Value);
          if (idOList == -1)
          {
            // The cell does not exist in the open list.
            // We will add the cell to the open list.

            PathFinderNode n = new PathFinderNode(cell, CurrentNode, G, H);
            mOpenList.Add(n);
            onAddToOpenList?.Invoke(n);
          }
          else
          {
            // if the cell exists in the openlist then check if the G cost is less than the 
            // one already in the list.
            float oldG = mOpenList[idOList].GCost;
            if (G < oldG)
            {
              // change the parent and update the cost to the new G
              mOpenList[idOList].Parent = CurrentNode;
              mOpenList[idOList].SetGCost(G);
              onAddToOpenList?.Invoke(mOpenList[idOList]);
            }
          }
        }
      }
    }
    #endregion

    #region GreedyPathFinder
    public class GreedyPathFinder<T> : PathFinder<T>
    {
      protected override void AlgorithmSpecificImplementation(Node<T> cell)
      {
        if (IsInList(mClosedList, cell.Value) == -1)
        {
          //Greedy best-first does doesn't include the G cost
          float G = 0.0f;
          float H = HeuristicCost(cell.Value, Goal.Value);

          // Check if the cell is already there in the open list.
          int idOList = IsInList(mOpenList, cell.Value);
          if (idOList == -1)
          {
            // The cell does not exist in the open list.
            // We will add the cell to the open list.

            PathFinderNode n = new PathFinderNode(cell, CurrentNode, G, H);
            mOpenList.Add(n);
            onAddToOpenList?.Invoke(n);
          }
          else
          {
            // if the cell exists in the openlist then check if the G cost is less than the 
            // one already in the list.
            float oldG = mOpenList[idOList].GCost;
            if (G < oldG)
            {
              // change the parent and update the cost to the new G
              mOpenList[idOList].Parent = CurrentNode;
              mOpenList[idOList].SetGCost(G);
              onAddToOpenList?.Invoke(mOpenList[idOList]);
            }
          }
        }
      }
    }
    #endregion
  }
}
Code language: C# (cs)

Read My Other Tutorials

1. Generic Finite State Machine Using C#
2. Implement Bezier Curve using C# in Unity
3. Create a Jigsaw Tile from an Existing Image
4. Create a Jigsaw Board from an Existing Image
5. Solving 8 puzzle problem using A* star search
6. A Configurable Third-Person Camera in Unity
7. Player Controls With Finite State Machine Using C# in Unity
8. Finite State Machine Using C# Delegates in Unity
9. Enemy Behaviour With Finite State Machine Using C# Delegates in Unity
10. Augmented Reality – Fire Effect using Vuforia and Unity
11. Implementing a Finite State Machine Using C# in Unity
12. Solving 8 puzzle problem using A* star search in C++
13. What Are C# Delegates And How To Use Them
14. How to Generate Mazes Using Depth-First Algorithm

References

1. https://en.wikipedia.org/wiki/A*_search_algorithm
2. https://mat.uab.cat/~alseda/MasterOpt/AStar-Algorithm.pdf
3. https://www.redblobgames.com/pathfinding/a-star/introduction.html
4. https://en.wikipedia.org/wiki/Graph_traversal
5. https://en.wikipedia.org/wiki/Pathfinding

Shamim Akhtar

A  committed and optimistic professional who brings passion and enthusiasm to help motivate, guide and mentor young students into their transition to the Industry and reshape their careers for a fulfilling future. The past is something that you cannot undo. The future is something that you can build.

I enjoy coding, developing games and writing tutorials. Visit my GitHub to see the projects I am working on right now.
Educator | Developer | Mentor