Skip to content

Instantly share code, notes, and snippets.

# mburst/astar.rb Created Feb 24, 2013

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)
to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.