A* Pathfinding Algorithm with Pseudocode

I'm currently working on a game and I want to use A* (star) pathfinding algorithm to in my game. So let's imagine a grid like in the below:

a star pathfinding algorithm

Each box on the grid is a node.

Blue Box: Start node

Red Box: End node

Eclipse color Boxes: Block nodes

The most important thing is a formula about this pathfinding. Each node should have calculated according to the this formula:

f = g + h

g : distance value between current node and the start node.

h : this is a heuristic calculation between current node and the end node and block nodes are ignored . This can be calculated with different ways. Some of these ways are:

  • Euclidian Distance
  • Manhattan Distance
  • Chebyshev Distance
f : sum of costs g and h

Let's calculate the f, g and h costs of a node we have chosen according to the information above:

Heuristic Value in A* pathfinding

In the above image. I chose a node as current node. We need to calculate h and g costs of this node. Remember each node has its own costs g and h. 

I used euclidian distance to calculate heuristic value. We don't need to calculate square root of number after that. Because one way or another, whichever is the smallest value will be used. Of course, it also depends g cost value. 

h  =  (1 - 5)^2 + (0 - 4)^2 = 32 => sqrt(32)

In addition, we have to calculate g cost:

g = (5 - 6)^2 + (4 - 5)^2 = 2 => sqrt(2)

And the f cost of the current node is:

f = sqrt(32) + sqrt(2)

In this way, we can calculate the cost f of the other nodes. 

First, the cost f of the nodes around the starting node is calculated. Whichever has the smallest cost value among these calculated costs is continued on that node. The same operations are applied to that node and the lowest cost node in its around is tried to be calculated.

To calculate of the shortest way on the grid, I'm going type a pseudo code This below image will be my reference to make easier of somethings:

Numbered Grid for A* Pathfinding

  • Create Node structure. This structure should contain these attributes:
    • Same type parent as a node.
    • Position (x, y). I used Vector type for it.
    • Three integer variables called f, g, and h
    • Create a Pathfinder class. The fields of Pathfinder:
      • notVisited is a list or array
      • visited is a list or array
      • allNodes is a dictionary. <Vector Pos, Node> It contains all nodes from the map. I use it otherwise it will cause stack overflow.
      • neighbours is a list or array.
      • Three nodes are created that called startNode, endNode, currentNode. This is not necessary at the start of the class.
      • map is an array. This array can be one or two dimensional. It depends how do you render your map.
      • path is a list or array for final result.
    • Also I created an array with 8 elements for defining neighbours of the current node. [vec(-1,-1), vec(0,-1), vec(1,-1), vec(-1,0), vec(1, 0), vec(-1, 1), vec(0, 1), vec(1, 1)] However, It's not necessary you can define the neighbours with nested loops. The method is optional.
    • At the start, I created all nodes according to the size of map and stored in the dictionary. Key is node position and value is node itself.


    Let's implement a* (star) algorithm to the pathfinder class:
    1. Passed startNode and endNode.
    2. The g cost value of the startNode set to 0.
    3. created notVisited(opened) and visited(closed) list.
    4. startNode added in the notVisited list.
    5. Start a while-loop and run it:
      1. set currentNode to notVisited[0]
      2. start a for-loop with notVisited list
        1. if node.f is less than currentNode.f or node.f equals currentNode.f and node.h is less than currentNode.h then set currentNode to node itself.
      3. remove currentNode from notVisited list.
      4. add currentNode to visited list.
      5. if the position of currentNode equals the position of endNode then set endNode to currentNode and return the result path. The end of the loop. If not then keep looping the nodes.
      6. Start a for-loop with the neigbours of currentNode:
        1. Ignore the nodes if they are unwalkable.
        2. Calculate the new g cost of the each neighbour. (currentNode.g + getDistance(currentNode, neighbourNode)
        3. If the new g cost value is less than the g cost of  neighbourNode or notVisited list not contain the neighbourNode then define the g, h, and f cost of the neighbourNode and set the parent of neighbourNode to currentNode. After that;
          1. If notVisited not contains this neighbourNode then add this neighbourNode to the notVisited list.
    6. Go to step 5 and do same things until length of notVisited is zero.
    Well, that's look terrible and difficult. I tried to explain it in the simplest way I can for now. I think I will update this post from time to time.

    No comments:

    Post a Comment