astar.rb

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)