A-star

meiro.rb

#!/usr/bin/env ruby
DATA = "**************************
*S* *                    *
* * *  *  *************  *
* *   *    ************  *
*    *                   *
************** ***********
*                        *
** ***********************
*      *              G  *
*  *      *********** *  *
*    *        ******* *  *
*       *                *
**************************"

def print_result (close, map)
  new_map = map
  close.each_key{|x,y|
    if new_map[y][x..x] != 'S' and new_map[y][x..x] != 'G' then
      new_map[y][x..x] = '$'
    end
  }
  new_map.each{|str|
    puts str
  }
end

def h (x, y, gx, gy)
  ret = (gx - x).abs + (gy - y).abs
  return ret
end

def get_status (map, x, y)
  ret = map[y].slice(x..x)
  return ret
end

def path? (map, xm, ym, x, y)
  if x >= 0 and x < xm and y >= 0 and y < ym then
    if get_status(map, x, y) == ' ' or get_status(map, x, y) == 'G' then
      return true
    end
  end
  return false
end

def set_nx (x, y)
 return [[x+1, y], [x-1, y], [x,y+1], [x, y-1]]
end

def min_f (node, xm, ym)
  min_value = 2 * (xm + ym)
  min_key = 0
  min_h = xm + ym
  node.each_pair{|key, value|
    if min_value > value[0] + value[1] or (min_value == value[0] + value[1] and min_h > value[1]) then
      min_value = value[0] + value[1]
      min_key = key
      min_h = value[1]
    end
  }
  return min_key
end

map = DATA.split(/\n/)

x_start = 0 
y_start = 0
x_goal = 0
y_goal = 0
x_max = 0
y_max = 0

i = 0
for str in map 
  if /(.*)S/=~str then
    x_start = $1.length
    y_start = i
  end
  if /(.*)G/=~str then
    x_goal = $1.length
    y_goal = i
  end
  if /^(.*\*)\s*$/=~str then
    if x_max < $1.length then
      x_max = $1.length
    end
  end
  y_max = i
  i = i + 1
end

open_node = Hash::new
close_node = Hash::new
open_node[[x_start,y_start]] = [0, h(x_start, y_start, x_goal, y_goal), [x_start,y_start]]

x = x_start
y = y_start

while x != x_goal or y != y_goal 
  if open_node.size == 0 then
    break
  end
  x = min_f(open_node, x_max, y_max)[0]
  y = min_f(open_node, x_max, y_max)[1]
  last_g = open_node[[x,y]][0]
  close_node[[x,y]] = open_node[[x,y]]
  open_node.delete([x,y])
  nx = set_nx(x, y)
  nx.each {|ix,iy|
    if path?(map, x_max, y_max, ix, iy) and !close_node.key?([ix,iy]) then
      open_node[[ix,iy]] = [last_g+1, h(ix, iy, x_goal, y_goal), [x,y]]
      #open_node[[ix,iy]] = [last_g+1, 0, [x,y]]
    end
  }
end

trace = Hash::new
while x != x_start or y != y_start 
  trace[ [x,y] ] = 1
  new_x = close_node[[x,y]][2][0]
  new_y = close_node[[x,y]][2][1]
  x = new_x
  y = new_y
end

print_result(trace,map)
print_result(close_node,map)