branch14.org

 1 class AStar
       2   
       3   def initialize(adjacency_func, cost_func, distance_func)
       4     @adjacency = adjacency_func
       5     @cost = cost_func
       6     @distance = distance_func
       7   end
       8   
       9   def find_path(start, goal)
      10     been_there = {}
      11     pqueue = PriorityQueue.new
      12     pqueue << [1, [start, [], 0]]
      13     while !pqueue.empty?
      14       spot, path_so_far, cost_so_far = pqueue.next
      15       next if been_there[spot]
      16       newpath = path_so_far + [spot]
      17       return newpath if (spot == goal)
      18       been_there[spot] = 1
      19       @adjacency.call(spot).each do |newspot|
      20         next if been_there[newspot]
      21         tcost = @cost.call(spot, newspot)
      22         next unless tcost
      23         newcost = cost_so_far + tcost
      24         pqueue << [newcost + @distance.call(goal, newspot),
      25                    [newspot, newpath, newcost]]
      26       end
      27     end
      28     return nil
      29   end   
      30   
      31   class PriorityQueue
      32     def initialize
      33       @list = []
      34     end
      35     def add(priority, item)
      36       @list << [priority, @list.length, item]
      37       @list.sort!
      38       self
      39     end
      40     def <<(pritem)
      41       add(*pritem)
      42     end
      43     def next
      44       @list.shift[2]
      45     end
      46     def empty?
      47       @list.empty?
      48     end
      49   end
      50   
      51 end