Dijkstra.rb

class Node
  attr_accessor :id, :edges
  def initialize(node_id)
    @id = node_id
    @edges = []
  end

  def edge(other_node)
    @edges.each do |edge|
      return edge if edge.has_node?(other_node)
    end
    return nil
  end

end

class Edge
  attr_accessor :cost, :node_id, :nodes

  def initialize(cost, node_id0, node_id1)
    @cost = cost
    @node_id = [node_id0, node_id1]
    @nodes = [nil, nil]
  end

  def has_node?(node)
    return @nodes[0] == node || @nodes[1] == node
  end

  def other(node)
    return node == @nodes[0] ? @nodes[1] : @nodes[0]
  end

end

class Route
  attr_accessor :found, :parents

  def initialize(start_sym = :START, goal_sym = :GOAL)
    @parents = Hash.new # child -> parent
    @way_costs = Hash.new # child -> way cost
    @found = false
    @start_sym = start_sym
    @goal_sym = goal_sym
  end

  def record(child, parent, cost)
    @parents[child] = parent
    @way_costs[child] = cost
  end

  def cost_to(node)
    if @way_costs.include?(node)
      return @way_costs[node]
    else
      return Float::MAX
    end
  end

  def get_path
    return [] if not @found

    goal = @parents.find {|c, p| c.id == @goal_sym}.first
    path = [goal]
    parent = @parents[goal]

    while parent != nil
      path << parent
      parent = @parents[parent]
    end

    return path.reverse!
  end

end


if __FILE__ == $0

  # S-1-2
  # | | |
  # 3-4-5
  # | | |
  # 6-7-G
  $road_map = [
    Edge.new(1.0, :START, :N1),
    Edge.new(1.0, :START, :N3),
    # Edge.new(5.0, :START, :GOAL),
    Edge.new(1.0, :N1, :N2),
    Edge.new(1.0, :N1, :N4),
    Edge.new(1.0, :N2, :N5),
    Edge.new(1.0, :N3, :N4),
    Edge.new(1.0, :N3, :N6),
    Edge.new(1.0, :N4, :N5),
    Edge.new(1.0, :N4, :N7),
    Edge.new(1.0, :N5, :GOAL),
    Edge.new(1.0, :N6, :N7),
    Edge.new(1.0, :N7, :GOAL),
  ]

  $nodes = Hash.new
  $route = Route.new(:START, :GOAL)

  # Initialize
  $road_map.each do |edge|
    edge.node_id.each_with_index do |id, index|
      unless $nodes.has_key? id
        $nodes[id] = Node.new(id)
      end
      edge.nodes[index] = $nodes[id]
    end
  end

  $road_map.each do |edge|
    $nodes[edge.node_id[0]].edges << edge
    $nodes[edge.node_id[1]].edges << edge
  end

  $route.record($nodes[:START], nil, 0.0)

  # Dijkstra Search
  open_list  = [$nodes[:START]]
  close_list = []

  until open_list.empty?
    minimum_cost_nodes = []
    open_list.each do |node|
      minimum_cost_nodes << [$route.cost_to(node), node] # pair : [cost, node]
    end
    minimum_cost_nodes.sort_by! {|pair| pair[0]}

    n =  minimum_cost_nodes.first[1]
    if n.id == :GOAL
      $route.found = true
      break
    end

    n.edges.each do |edge|
      m = edge.other(n)
      cost_current = $route.cost_to(n) + edge.cost
      if open_list.include?(m)
        if cost_current < $route.cost_to(m)
          $route.record(m, n, cost_current)
        end
      elsif close_list.include?(m)
        if cost_current < $route.cost_to(m)
          $route.record(m, n, cost_current)
          close_list.delete(m)
          open_list.push(m)
        end
      else
        $route.record(m, n, cost_current)
        open_list.push(m)
      end
    end

    open_list.delete(n)
    close_list.push(n)
  end

  # Result
  if $route.found
    $route.get_path().each do |node|
      print node.id
      print node.id == :GOAL ? "\n" : " -> "
    end
  end

end