経路探索アルゴリズムA*をRubyで実装してみた

前回書いた経路探索アルゴリズムA* - gan2 の Ruby 勉強日記が
たくさんブクマされててちょっとびっくりです(；ﾟДﾟ)

実装はFlash(Action Script)でやろうと思っていたのですが、
その前にRubyで書いてみることにしました。
途中、アルゴリズムの理解が不十分だったせいもあり、
多少てこづりましたがとりえず完成しました！
ソースはあんまり整理してないけども、
あまり気にせずに貼り付けておきます(ﾉ∀｀)

=begin
**** 経路探索アルゴリズムA*(エースター) a-star.rb

**** アルゴリズムの概要
スタートノードから、あるノード n を通って、
ゴールまで辿り着くときの最短路経路を考える。
このとき、最短経路のコスト f(n) を次の式で表す。
f(n) = g(n) + h(n)
ここで、g(n) はスタートノードから n までの最小コスト。
h(n) は n からゴールノードまでの最短コストである。

g(n) と h(n) が一意に決まるのであれば、
全体の最短経路は容易に求まる。
しかし実際には、g (n) と h (n) をあらかじめ与えることは出来ない。
そこで、f(n) を
f*(n) = g*(n) + h*(n)
という推定値に置き換えて探索を行う。 

**** 参考
A* - Wikipedia
http://ja.wikipedia.org/w/index.php?title=A%2A

=end

# マップ
class Map
  # マップデータの書き込まれたファイルを読み込む
  def initialize(filename)
    @map    = []
    @width  = nil
    @height = nil

    x = 0
    y = 0
    open(filename) do |file|
      y = 0

      file.each do |line|
        x = 0
        ary = []

        line.chomp.split("").each do |c|
          node = Node.new(c, x, y)
          ary.push(node)
          if (c == "S")
            $startnode = node
          elsif (c == "G")
            $gallnode = node
          end

          x += 1            
        end

        @map.push(ary)
        y += 1
      end
    end

    @width  = x
    @height = y

    # スタートノード S の最短経路コストの推定値 f*(S) を求める。
    # 生成のとき g*(S) = 0 だから、
    # f*(S) = g*(S) + h*(S) より、f*(S) = h*(S) となる。
    $startnode.f_star = $startnode.h_star
  end
  
  attr_reader :width, :height

  # マップの表示
  def view
    @map.each do |m|
      m.each do |n|
        print n.type
      end
      puts
    end
    puts
  end

  # (デバッグ用) マップ上にあるノードの f* を表示
  def view_f_star
    @map.each do |m|
      m.each do |n|
        print n.f_star, " "
      end
      puts
    end    
  end
  
  def [](idx)
    @map[idx]
  end
end

# ノード
class Node
  # type はノードの種類("S", "G", "o", "x" のどれか)
  # x, y はマップ上の座標
  def initialize(type, x, y)
    @type = type
    @x = x
    @y = y
    @f_star = " "
    @parent = nil
  end

  attr_accessor :type, :x, :y, :f_star #, :parent
  attr_reader :parent
  
  # スタートノードから n までの最小コストの推定値 f*(n)
  def g_star
    @f_star - h_star
  end
  
  # n からゴールノードまでの最小コストの推定値 h*(n)
  # h*(n) には通常、n からゴールノードまでの直線距離を与えることが多い。
  # しかし、今回は斜めの移動を考慮しないため、
  # 縦と横の移動だけを使ったコストを求めたい。
  # よって、直線距離ではなく、n からゴールノードまでの
  # 縦と横の距離それぞれを足したものを h*(n) とする。
  def h_star
    cost($gallnode)
  end
  
  # ゴールノードであるかどうかの判定
  def is_gall_node?
    @type == "G"
  end

  # スタートノードであるかどうかの判定
  def is_start_node?
    @type == "S"
  end
  
  # 隣接しているノードを含む配列を返す
  # ここで、隣接しているとは縦横いずれかに隣り合っていることを指す。
  # よって、斜めに隣り合っているノードは結果の配列に含まれない。
  def adjacent_nodes
    m_list = []
    m = nil
    
    # 上
    if (@y - 1 >= 0 && $map[@y-1][@x].type != "x")
      m_list.push($map[@y-1][@x])
    end

    # 下
    if (@y + 1 < $map.height && $map[@y+1][@x].type != "x")
      m_list.push($map[@y+1][@x])
    end

    # 左
    if (@x - 1 >= 0 && $map[@y][@x-1].type != "x")
      m_list.push($map[@y][@x-1])
    end

    # 右
    if (@x + 1 < $map.width && $map[@y][@x+1].type != "x")
      m_list.push($map[@y][@x+1])
    end

    m_list
  end

  # ノード n から m へ移動するときのコスト
  # これは n, m 間の縦横の距離を足したものである。
  def cost(m)
    dis_x = (@x - m.x).abs
    dis_y = (@y - m.y).abs
    dis_x + dis_y
    
    # 直線距離をコストとする場合
    # 0.5(1/2)乗は平方根を求めるのと同じ
    # dis = (dis_x ** 2 + dis_y ** 2) ** 0.5    
  end

  # list 中に含まれているかどうかを調べる
  def is_included_in?(list)
    #puts "self #{self}"
    list.each do |l|
      #puts "l #{l}"
      # どうやらここが問題みたいだなぁ。
      return true if l == self
    end
    
    false
  end
  
  def parent=(p)
    @parent = p
    # puts "#{self}(#{@x}, #{@y}) の親は #{p}(#{p.x}, #{p.y})"
  end
end

# Openリストに格納されているノードの内、
# 最小の f*(n) を持つノード n を取り出す
def get_smallest_cost_node
  smallest = $openlist[0]
  index = 0

  $openlist.each_with_index do |n, idx|
    if n.f_star < smallest.f_star
      smallest = n 
      index = idx
    end
  end

  # Openリストから n を除いておく
  $openlist.delete_at(index)
  smallest
end

#### 
s = "S" # スタート
g = "G" # ゴール
o = "o" # 通行可能
x = "x" # 通行不能
# スタート・ゴールノード
# 値が入るのはマップを生成したとき
$gallnode  = nil
$startnode = nil
# マップの生成
$map = Map.new("map.txt")
$map.view
$openlist  = [] # 計算中のノード
$closelist = [] # 計算済のノード

# スタートノードをOpenリストに格納
$openlist.push($startnode)
#puts $startnode.f_star

# デバッグ用変数
count = 0
# Openリストが空になるまで探索
while ($openlist.size > 0)
  # Openリスト中で最小の f*(n) を持つノード n
  # このときOpenリストから n は外される
  n = get_smallest_cost_node
  #puts n.f_star
  #pp $openlist
  #puts "----"
  
  # n がゴールノードであれば探索を終了する
  if (n.is_gall_node?)
    puts "ゴールノードを発見しました。"
    puts
    break
  else 
    # それ以外の場合はCloseリストに n を入れる
    $closelist.push(n)
  end
  
  ## n に隣接している全てのノード m に対して以下の操作を行う
  m_list = n.adjacent_nodes # n に隣接している全てのノードを持つ配列
  m_list.each do |m|
    # f'(m) = g*(n) + h*(m) + COST(n,m) を計算する。
    # ここで COST(n,m) はノード n から m へ移動するときのコストである。
    # また g*(n) は g*(n) = f*(n) - h*(n) で求めることができる。
    f_dash = n.g_star + m.h_star + n.cost(m)
    #puts "m.x: #{m.x}/tm.y: #{m.y}"

    if (m.is_included_in?($openlist)) # m がOpenリストにある場合
      # f'(m) < f*(m) なら
      if f_dash < m.f_star
        # f*(m) = f'(m) に置き換えて
        m.f_star = f_dash 
        # m の親を n に更新
        m.parent = n
      end
    elsif (m.is_included_in?($closelist)) # m がCloseリストにある場合
      if f_dash < m.f_star
        m.f_star = f_dash 
        m.parent = n

        # m をCloseリストから外し、Openリストに移動
        index = nil
        $closelist.each_with_index do |c, idx|
          index = idx if c == m
        end
        $closelist.delete_at(index)
        $openlist.push(m)
      end
    else # どちらのリストにもない場合
      m.f_star = f_dash
      m.parent = n
      # m をOpenリストに追加
      $openlist.push(m)
    end
  end

  # デバッグのために無限ループ回避
  if count < 100
#     puts "count: #{count}"
#     puts "openlist: #{$openlist}"
#     puts "closelist: #{$closelist}"
#     puts "----"
#     count += 1
  else
    $map.view_f_star
    exit
  end
end

# puts $gallnode.parent
# puts $startnode.parent

pathlist = []
node = $gallnode
pathlist.push(node)

while (node.parent)
  node = node.parent
  pathlist.push(node)
end

pathlist = pathlist.reverse
# スタートノードを取り出す
pathlist.shift
pathlist.each do |n|
  puts "x: #{n.x}\ty: #{n.y}"
  # 通った道筋を "o" から "." に変える
  n.type = "."
end
puts
# 上の処理だとゴールノードも "." になってしまうので
# ここで "G" に戻しておく
pathlist.last.type = "G"

$map.view

上記のプログラムはmap.txtを読み込んで最短経路を探索します。
例えば以下のようなmap.txtを用意します。
Sがスタート。Gがゴール。
oが通れるところで、xが通れないところです。
SからGまでの最短経路を探索することになります。

ooooxxxG
oooooooo
xxooxxxx
oooooooo
oxxxxxoo
oooooooo
xxxoxxxo
Sooooooo

コマンドラインから実行すると、こんな結果が出力されます。

$ ruby a-star.rb
ooooxxxG
oooooooo
xxooxxxx
oooooooo
oxxxxxoo
oooooooo
xxxoxxxo
Sooooooo

ゴールノードを発見しました。

x: 1	y: 7
x: 2	y: 7
x: 3	y: 7
x: 3	y: 6
x: 3	y: 5
x: 4	y: 5
x: 5	y: 5
x: 6	y: 5
x: 6	y: 4
x: 6	y: 3
x: 5	y: 3
x: 4	y: 3
x: 3	y: 3
x: 3	y: 2
x: 3	y: 1
x: 4	y: 1
x: 5	y: 1
x: 6	y: 1
x: 7	y: 1
x: 7	y: 0

ooooxxxG
ooo.....
xxo.xxxx
ooo....o
oxxxxx.o
ooo....o
xxx.xxxo
S...oooo

今度はFlashで実装してみようと思います。