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
About Me
My name is Max Burstein and I am a graduate of the University of Central Florida and creator of Problem of the Day and Live Dota. I enjoy developing large, scalable web applications and I seek to change the world.