Implement a Generic Pathfinder in Unity using C#

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.

Click to Play the WebGL version on your browser (mobile devices may not support it)

Contact me

Read also

Pathfinding

Pathfinding is the plotting of the most optimal route between two points by a computer application. The most optimal path 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.

In computer games, pathfinding is primarily used for navigation by the non-playing characters (NPCs). A pathfinding solution lets us plan ahead of time the path to be taken by an NPC.

Algorithms

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

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

In this tutorial, I am not going to go through the detailed theory of all these pathfinding algorithms, but rather, I would 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.

For a rectangular grid-based map where an NPC can traverse all the neighbouring cells, including diagonal cells.
For a rectangular grid-based map, an NPC can traverse only left, right, up and down and not diagonally.
For a rectangular grid-based map where an NPC can traverse all the neighbouring cells, including diagonal cells, some are marked as a no-go (or non-walkable).

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

A graph-based map where an NPC can traverse from one vertex to another neighbour vertex but cannot do the same from the neighbour vertex to the original vertex. We call this type of graph a directed graph.
A graph-based map where an NPC can traverse from one vertex to another neighbour vertex and do the same from the neighbour vertex to the original vertex. For undirected edges, we show the line by undirected double arrows. The ones with a single arrow represent the directed edges.

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 AStarPathFinderDijkstraPathFinder 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

Leave a Reply

Your email address will not be published. Required fields are marked *