Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
Ruby A* implementation
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)
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.