Introduction to Graph Theory: Finding The Shortest Path (Part 2) (Posted on February 23rd, 2013)

A couple of weeks ago I did an introduction to graph theory using Dijkstra's algorithm. I received some awesome feedback so this week I want to take it a step further and build on that using the A* algorithm. The big difference between A* and Dijkstra's algorithm is the addition of a heuristic function. This heuristic function allows us to give a best guess of which path to take rather than always taking the path that is the shortest distance from the source node. If you're a gamer or someone looking to get into game development this algorithm is the basis of how path finding is done in a lot of games.

Dijkstra's Algorithm Refresher

Given a source node in a graph we examine all connected edges for the shortest distance from that node. We then mark the source node as visited so we don't have to check the edges again. Based off our edge examinations we then go to the next closest node that hasn't been visited. We repeat this process until we reach our destination node. Then we check the path we took to get there and print that out.

If you'd like a more detailed explanation definitely check out my blog post on the algorithm.

The Heuristic Function

The goal of this function is to give us a dynamic way to pick a path to our target node. An example of this would be planning a route home from work. You may normally take the highway but as you get closer to the highway you realize that there is an accident blocking the road. So instead of taking the highway home you use back roads which get you home faster. You took a guess that the highway would be the quickest way home but after checking the path it turned out you need take an alternate route. The heuristic function is used to give a best guess of the path to take.

There are several different ways to come up with a heuristic function but it is important to make sure that the function is admissible (never overestimates the cost of reaching the goal). A common heuristic function is the Chebyshev Distance which allows us to move diagonally along a grid rather than just up, down, and side to side like the Manhattan Distance. If you're making something where diagonal movement isn't allowed then definitely use the Manhattan Distance. The Chebyshev Distance states that the distance between two nodes is the greatest of their differences along any coordinate dimension. The code for this calculation is easy to implement.

chebyshev_distance = [(current.x - target.x).abs, (current.y - target.y).abs].max

The Setup

Just need one gem for this one. PriorityQueue will allow us to have a heap where we can change the priority and have the tree automatically rebalance which is nice.

gem install PriorityQueue

Creating the Graph

Let's start by creating a grid of a specified size. This grid will be our graph and will be a square for simplicity. For A* each node in the graph will need to store some extra information about it so let's just create a Node class that will store this information.

class Node
    def initialize(x, y)
        @x = x
        @y = y
        @obstacle = false
        @g_score = Float::INFINITY
    end
    
    def x()
        return @x
    end
    
    def y()
        return @y
    end
    
    def set_obstacle()
        @obstacle = true
    end
    
    def obstacle()
        return @obstacle
    end
    
    def set_g_score(score)
        @g_score = score
    end
    
    def g_score()
        return @g_score
    end
    
    def to_s
        return "(" + @x.to_s + ", " + @y.to_s + ", " + @obstacle.to_s + ")"
    end
end

The x and y variables will give us our coordinate position, the obstacle variable will tell us if we can pass through this position or not, and the g_score variable will be the best distance from the source node to this node. The g_score for each node starts at infinity since no paths have been calculated yet.

Next we need to create a grid out of these Nodes. So let's create a graph class and insert a grid of a size we choose.

class Graph
    def initialize(size)
        @size = size-1 # Last index in 0 based array
        @grid = []
        for y in 0..@size
          row = []
          for x in 0..@size
            row.push(Node.new(x, y))
          end
          @grid.push(row)
        end
    end
    
    def to_s
        return @grid.inspect
    end
end

Our graph takes a variable called size and creates a square of nodes of that size. Here's basically how our graph looks in memory if we make a size 8 graph.

0 0 0 0 0 0 0 0 
0 0 0 0 0 0 0 0 
0 0 0 0 0 0 0 0 
0 0 0 0 0 0 0 0 
0 0 0 0 0 0 0 0 
0 0 0 0 0 0 0 0 
0 0 0 0 0 0 0 0 
0 0 0 0 0 0 0 0     

Something to keep in mind is that the ordered pair (0,0) is at the top left. So as Y increases you actually go down instead of up. This just makes array access easier. Otherwise you'll need to convert the x and y variables every time to map to the bottom of the graph which is definitely doable. So just remember (0,0) is at the top left and our positive y axis is pointed down.

Implementing A*

Before we start here is a great graphic from Wikipedia that shows how the algorithm works.

The algorithm will take in the starting x and y position as well as the finishing x and y position. We can then convert those coordinates into the actual nodes they are referencing. We can also use this time to initialize our variables that will be keeping track of our nodes.

def shortest_path(start_x, start_y, finish_x, finish_y)
        
    start = @grid[start_y][start_x]
    finish = @grid[finish_y][finish_x]
    
    visited = Set.new # The set of nodes already evaluated
    previous = {} # Previous node in optimal path from source
    previous[start] = 0
    f_score = PriorityQueue.new
        
end

Since we'll never visit the same node twice we want to make sure we don't waste time evaluating the same node more than once so we create a set of visited nodes for easy O(1) checking. We'll also want to keep track of how we got to a node. We keep track of nodes the same way as we do in Dijkstra's algorithm. A* improves on Dijkstra's by having an f_score. The f_score variable will be a min-heap (priority queue) of a node's distance from the source + the heuristic calculation. Since Set and PriorityQueue are not implemented by default in Ruby let's go ahead and require them at the top of our file.

require 'priority_queue'
require 'set'

A node at most can have 8 other touching nodes (up-left, up, up-right, right, down-right, down, down-left, and left). In order to evaluate these 8 nodes that could be possibly touching the node we're looking at I'm going to create an array of all possible combinations.

# All possible ways to go in a node
dx = [1, 1, 0, -1, -1, -1, 0, 1]
dy = [0, 1, 1, 1, 0, -1, -1, -1]

Using this array we'll always check the node to the right first. So if our starting node is (0,0) then the next node we evaluate is (0 + dx[0], 0 + dy[0]) = (1,0). This will be easier to see when we actually use it for the code. Right now we're just initializing it.

The final initialization step will be to set our start node's g_score to 0 and calculate the f_score. To calculate the f_score we'll need to implement the heuristic function (Chebyshev Distance) we talked about earlier. The code so far looks like this:

def shortest_path(start_x, start_y, finish_x, finish_y)
        
    def heuristic(current, target)
        return [(current.x - target.x).abs, (current.y - target.y).abs].max
    end
    
    start = @grid[start_y][start_x]
    finish = @grid[finish_y][finish_x]
    
    visited = Set.new # The set of nodes already evaluated
    previous = {} # Previous node in optimal path from source
    previous[start] = 0
    f_score = PriorityQueue.new
    
    # All possible ways to go in a node
    dx = [1, 1, 0, -1, -1, -1, 0, 1]
    dy = [0, 1, 1, 1, 0, -1, -1, -1]
    
    start.set_g_score(0) # Cost from start along best known path
    f_score[start] = start.g_score + heuristic(start, finish) # Estimated total cost from start to finish
end

Now that everything is setup let's start churning through nodes that need to be evaluated.

while !f_score.empty?
    current = f_score.delete_min_return_key # Node with smallest f_score
    visited.add(current)
end

This just grabs the lowest f_score from our priority queue and saves the node off to the current variable and adds it to the visited set so we know not to check it again.

Next we want to evaluate all the nodes that touch the current node. This is where our dx and dy arrays come in to play.

# Examine all directions for the next path to take
for direction in 0..7
    new_x = current.x + dx[direction]
    new_y = current.y + dy[direction]
    
    if new_x < 0 or new_x > @size or new_y < 0 or new_y > @size #Check for out of bounds
        next # Try next configuration
    end
end

The direction variable will be our index in the dx and dy array. We check to see if a node exists in each of the 8 possible directions. If we get a negative number or the value is greater than the size of the grid than we know we're on the edge of the grid. Since we can't check nodes outside of the grid we just say next which takes us to the next iteration of the for loop.

Once we get past the out of bounds check we know that the node exists in our grid. We'll call this node neighbor since it's adjacent to our current node. We then check if we've visited this node before, if it's set to be evaluated later, or if it's an obstacle. In any of the cases we don't want to evaluate any node more than once, unless it's an obstacle which we don't want to evaluate at all, so we skip that node and go to the next node.

# Examine all directions for the next path to take
for direction in 0..7
    new_x = current.x + dx[direction]
    new_y = current.y + dy[direction]
    
    if new_x < 0 or new_x > @size or new_y < 0 or new_y > @size #Check for out of bounds
        next # Try next configuration
    end
    
    neighbor = @grid[new_y][new_x]
    
    # Check if we've been to a node or if it is an obstacle
    if visited.include? neighbor or f_score.has_key? neighbor or neighbor.obstacle
        next
    end
end

When navigating the grid it's important to remember that not all paths are created equal. Think about a square with a side length of 10. The edge distance on any side of the square is going to be 10. However, if we want go diagonally across the square we have to bust out some Pythagoras. 10² + 10² = 200. When we take the square root of 200 we get about 14.1. One optimization that a lot of games make is rounding this number to 14 so you don't have to worry about floating point calculations. Let's go ahead and model that for our A* implementation. All diagonals will have a distance of 14 and all non-diagonals will have a distance of 10.

#dx = [1, 1, 0, -1, -1, -1, 0, 1]
#dy = [0, 1, 1, 1, 0, -1, -1, -1]
# Examine all directions for the next path to take
for direction in 0..7
    new_x = current.x + dx[direction]
    new_y = current.y + dy[direction]
    
    if new_x < 0 or new_x > @size or new_y < 0 or new_y > @size #Check for out of bounds
        next # Try next configuration
    end
    
    neighbor = @grid[new_y][new_x]
    
    # Check if we've been to a node or if it is an obstacle
    if visited.include? neighbor or f_score.has_key? neighbor or neighbor.obstacle
        next
    end
    
    if direction % 2 == 1
        tentative_g_score = current.g_score + 14 # traveled so far + distance to next node diagonal
    else
        tentative_g_score = current.g_score + 10 # traveled so far + distance to next node vertical or horizontal
    end
end

Our direction variable is essentially the index for our dx and dy arrays. All odd indices are diagonal paths. You can tell this by the fact that there is a 1 or -1 in both the dx and dy array at the same position. So when we're on an odd index we set our tentative_g_score to the current node's g_score + 14 which is our diagonal distance.

We're almost done! We just need to relax. Our relax function will essential be the same as our Dijkstra's implementation.

# Examine all directions for the next path to take
for direction in 0..7
    new_x = current.x + dx[direction]
    new_y = current.y + dy[direction]
    
    if new_x < 0 or new_x > @size or new_y < 0 or new_y > @size #Check for out of bounds
        next # Try next configuration
    end
    
    neighbor = @grid[new_y][new_x]
    
    # Check if we've been to a node or if it is an obstacle
    if visited.include? neighbor or f_score.has_key? neighbor or neighbor.obstacle
        next
    end
    
    if direction % 2 == 1
        tentative_g_score = current.g_score + 14 # traveled so far + distance to next node diagonal
    else
        tentative_g_score = current.g_score + 10 # traveled so far + distance to next node vertical or horizontal
    end
    
    # If there is a new shortest path update our priority queue (relax)
    if tentative_g_score < neighbor.g_score
        previous[neighbor] =  current
        neighbor.set_g_score(tentative_g_score)
        f_score[neighbor] = neighbor.g_score + heuristic(neighbor, finish)
    end
end

We're basically checking to see if our new path is going to be shorter than the previous path we found. If it is let's update how we get there, the distance to get there, and let's add the node to our priority queue to be evaluated in the future.

After this all that's left to do is simply print out the path. We can use the same procedure as we used for Dijkstra's. That is when get the current node we then check to see if the current variable is the same node as our finish variable. If it is print out the path and return. I also created a print_path function so that I could see the path graphically. Here's the necessary code to traverse the grid from the finish node back to the start node:

def print_path(path)
    for y in 0..@size
        for x in 0..@size
            if @grid[y][x].obstacle
                print "X "
            elsif path.include? @grid[y][x]
                print "- "
            else
                print "0 "
            end
        end
        print "\n"
    end
end

if current == finish
    path = Set.new
    while previous[current]
        path.add(current)
        current = previous[current]
    end
    
    print_path(path)
    return "Path found"
end

At the end of the shortest_path function we can also return "Failed to find path" if no path can be found. Now we have all we need to test our code. Let's try some configurations.

g = Graph.new(8)
g.set_obstacle(1,0)
g.set_obstacle(1,1)
g.set_obstacle(1,2)
g.set_obstacle(1,3)
g.set_obstacle(1,4)
g.set_obstacle(1,5)
g.set_obstacle(1,6)
puts g.shortest_path(0,0,4,2)

- X 0 0 0 0 0 0 
- X 0 0 0 0 0 0 
- X 0 0 - 0 0 0 
- X 0 - 0 0 0 0 
- X - 0 0 0 0 0 
- X - 0 0 0 0 0 
- X - 0 0 0 0 0 
0 - 0 0 0 0 0 0 
Path found


g = Graph.new(8)
g.set_obstacle(1,1)
g.set_obstacle(2,2)
g.set_obstacle(3,3)
g.set_obstacle(4,4)
g.set_obstacle(5,5)
g.set_obstacle(6,6)
g.set_obstacle(2,1)
g.set_obstacle(3,2)
g.set_obstacle(4,3)
g.set_obstacle(5,4)
g.set_obstacle(6,5)
puts g.shortest_path(0,2,6,3)

0 - - - 0 0 0 0 
- X X 0 - 0 0 0 
- 0 X X 0 - 0 0 
0 0 0 X X 0 - 0 
0 0 0 0 X X 0 0 
0 0 0 0 0 X X 0 
0 0 0 0 0 0 X 0 
0 0 0 0 0 0 0 0 
Path found

Pretty cool right? I've posted the final source code below for reference.

Final Source Code

require 'priority_queue'
require 'set'

class Node
    def initialize(x, y)
        @x = x
        @y = y
        @obstacle = false
        @g_score = Float::INFINITY
    end
    
    def x()
        return @x
    end
    
    def y()
        return @y
    end
    
    def set_obstacle()
        @obstacle = true
    end
    
    def obstacle()
        return @obstacle
    end
    
    def set_g_score(score)
        @g_score = score
    end
    
    def g_score()
        return @g_score
    end
    
    def to_s
        return "(" + @x.to_s + ", " + @y.to_s + ", " + @obstacle.to_s + ")"
    end
end

class Graph
    def initialize(size)
        @size = size-1 # Last index in 0 based array
        @grid = []
        for y in 0..@size
          row = []
          for x in 0..@size
            row.push(Node.new(x, y))
          end
          @grid.push(row)
        end
    end
  
    def set_obstacle(x, y)
        @grid[y][x].set_obstacle()
    end
    
    def shortest_path(start_x, start_y, finish_x, finish_y)
        
        def heuristic(current, target)
            return [(current.x - target.x).abs, (current.y - target.y).abs].max
        end
        
        start = @grid[start_y][start_x]
        finish = @grid[finish_y][finish_x]
        
        visited = Set.new # The set of nodes already evaluated
        previous = {} # Previous node in optimal path from source
        previous[start] = 0
        f_score = PriorityQueue.new
        
        # All possible ways to go in a node
        dx = [1, 1, 0, -1, -1, -1, 0, 1]
        dy = [0, 1, 1, 1, 0, -1, -1, -1]
        
        start.set_g_score(0) # Cost from start along best known path
        f_score[start] = start.g_score + heuristic(start, finish) # Estimated total cost from start to finish
        
        while !f_score.empty?
            current = f_score.delete_min_return_key # Node with smallest f_score
            visited.add(current)
            
            if current == finish
                path = Set.new
                while previous[current]
                    path.add(current)
                    current = previous[current]
                end
                
                print_path(path)
                return "Path found"
            end
            
            # Examine all directions for the next path to take
            for direction in 0..7
                new_x = current.x + dx[direction]
                new_y = current.y + dy[direction]
                
                if new_x < 0 or new_x > @size or new_y < 0 or new_y > @size #Check for out of bounds
                    next # Try next configuration
                end
                
                neighbor = @grid[new_y][new_x]
                
                # Check if we've been to a node or if it is an obstacle
                if visited.include? neighbor or f_score.has_key? neighbor or neighbor.obstacle
                    next
                end
                
                if direction % 2 == 1
                    tentative_g_score = current.g_score + 14 # traveled so far + distance to next node diagonal
                else
                    tentative_g_score = current.g_score + 10 # traveled so far + distance to next node vertical or horizontal
                end
                
                # If there is a new shortest path update our priority queue (relax)
                if tentative_g_score < neighbor.g_score
                    previous[neighbor] =  current
                    neighbor.set_g_score(tentative_g_score)
                    f_score[neighbor] = neighbor.g_score + heuristic(neighbor, finish)
                end
            end
        end
        
        return "Failed to find path"
    end
    
    def print_path(path)
        for y in 0..@size
            for x in 0..@size
                if @grid[y][x].obstacle
                    print "X "
                elsif path.include? @grid[y][x]
                    print "- "
                else
                    print "0 "
                end
            end
            print "\n"
        end
    end
    
    def to_s
        return @grid.inspect
    end
end

g = Graph.new(8)
g.set_obstacle(1,1)
g.set_obstacle(2,2)
g.set_obstacle(3,3)
g.set_obstacle(4,4)
g.set_obstacle(5,5)
g.set_obstacle(6,6)
g.set_obstacle(2,1)
g.set_obstacle(3,2)
g.set_obstacle(4,3)
g.set_obstacle(5,4)
g.set_obstacle(6,5)
puts g.shortest_path(0,2,6,3)

I also created a gist of this implementation so that you can view and play around with the code on Github. Definitely try implementing this in another language or if Ruby is your language of choice try and modify/optimize my implementation. A simple modification would be adjusting the graph and A* function to allow for a rectangular shape. Let me know in the comments what you come up with.

As always if you have any feedback or questions feel free to drop them in the comments below or contact me privately on my contact page. Thanks for reading!

P.S. If your company is looking to hire an awesome soon-to-be college graduate (May 2013) let me know!

Tags: Ruby