AI: Path Finding & Following

See all my AI projects HERE

Path Finding

I used A* algorithm for path finding.

A typical A* algorithm runs in a node map or a graph system. It runs from the start node untill a path is found to the goal or when there is no more node to run on. It selects the path that minimizes the combination of two things:

  • Cost: The current cost of the current node being calculated to the start node
  • Heuristic: The estimated cost of the cheapest path from current node to the goal

For more description, visit here:

The path find function:

public List PathFind (GridNode StartNode, GridNode GoalNode)
    StartNode.CostSoFar = 0;
    StartNode.Heu = heuristic (StartNode, GoalNode);
    Open.Clear ();
    Closed.Clear ();
    Open.Enqueue (0, StartNode);
    List thePath = new List ();
    while (true) {
        GridNode currentNode = Open.DequeueValue ();

    if (currentNode == GoalNode) {
        Closed.Clear ();
        GridNode fromnode = GoalNode;
        while (fromnode != StartNode) {
            thePath.Add (fromnode);
        fromnode = fromnode.prevNode;
        thePath.Add (StartNode);
        return new List (thePath);

    Closed.Add (currentNode);
    foreach (GridNode nextNode in currentNode.Neighbors) {
        if (nextNode._NodeStatus == GridNode.NodeStatus.blocked) {

        int cost_so_far = heuristic (nextNode, currentNode) * nextNode.Cost + currentNode.CostSoFar;
        int heu = heuristic (nextNode, GoalNode);
        if (nextNode.CostSoFar == 0 && nextNode != StartNode) {
        nextNode.CostSoFar = cost_so_far;
        if (nextNode.Heu == 0 && nextNode != StartNode) {
        nextNode.Heu = heu;

        if (inOpen (nextNode).Value != null && cost_so_far + heu < inOpen (nextNode).Key) {
        Open.Remove (inOpen (nextNode));
        Open.Enqueue (cost_so_far + heu, nextNode);
        nextNode.CostSoFar = cost_so_far;
        nextNode.Heu = heu;
        nextNode.prevNode = currentNode; 

         if (inOpen (nextNode).Value == null && !Closed.Contains (nextNode)) {
        Open.Enqueue (cost_so_far + heu, nextNode);
        nextNode.prevNode = currentNode;

        nextNode.SetText ((nextNode.CostSoFar + nextNode.Heu).ToString ());
        nextNode._text.gameObject.SetActive (true);
    if (Open.IsEmpty && currentNode!=GoalNode) {
        List emptyList = new List ();
        emptyList.Add( StartNode);
        return new List(emptyList);

For my project I created a grid of nodes. User could place walls(fully blocks path), a start point and a goal point on the grid. Everytime there is a change on the grid, it recalculates the shortest path from the start point to the goal point.

I also use color to show which nodes on the grid have been searched, and then I used GUI text to show their combined cost and heuristic.

It looks like this in the end:


Path Following

For this path following project, I added a cost system in the path finding algorithm. Different nodes may have different cost to path through, to simulate terrains in games.

In the same grid, player can now draw different elements that have different cost: grass(cost: 2), rock(cost: 3) and wall(fully blocks path).


After drawing the terrain, instantiate an actor that find path to a randomly decided destination. Once he reaches, he change his destination randomly again.

It looks like this in the end:


Leave a Reply

Fill in your details below or click an icon to log in: Logo

You are commenting using your account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s