A*探索アルゴリズム

最近、A*という単語を目にすることがあったので、ちょっとWikipediaで調べてみたところ、その正体は経路探索のアルゴリズムでした。

経路探索なんてチャレンジしたことも無いので、これも何かの縁と思い、頑張って書いてみることにしました。googleってみたのですが、何だか初心者向けの情報が少ないなぁ…ということで結局、A* - Wikipediaを参考に。

type field = O | X
let maze = [|
  [|O;O;O;O;O;O;O;O;O;O;O;O;O;O;O;O;O;X;O;O;O|];
  [|O;O;O;X;X;O;O;X;X;X;X;O;O;O;O;O;O;X;O;O;O|];
  [|O;X;O;O;X;O;O;X;O;O;X;O;X;X;X;X;X;X;O;O;O|];
  [|O;X;X;X;X;O;O;O;O;O;X;O;O;O;O;O;O;O;O;O;O|];
  [|O;O;O;O;O;O;O;O;O;O;X;O;O;O;O;O;X;X;X;X;O|];
  [|O;O;O;X;X;X;O;X;O;O;X;O;O;O;O;O;O;O;O;X;O|];
  [|O;O;O;O;O;X;O;X;X;X;X;O;O;O;O;O;O;O;O;X;O|];
  [|O;X;O;O;O;X;O;O;O;O;O;O;O;O;O;X;X;X;X;X;O|];
  [|O;X;X;X;X;X;O;O;O;X;X;X;X;X;O;O;O;O;O;O;O|];
  [|O;O;O;O;O;O;O;O;O;O;O;O;O;X;O;O;X;X;X;X;X|];
  [|O;O;O;O;O;O;O;O;O;O;O;O;O;X;O;O;O;O;O;O;O|];
  |]

type pos  = {x: int; y: int}
type node = {pos: pos; cost: float; parent: pos}

let max_x = Array.length maze.(0) - 1
let max_y = Array.length maze     - 1
let start = {x = 0; y = 0}
let goal  = {x = max_x; y = max_y}

(* for debug *)
let strpos pos = Printf.sprintf "pos_x[%02d] pos_y[%02d]" pos.x pos.y
let plist list =
  List.iter (fun a -> Printf.printf "<%s cost[%f] parent[%s]>\n"
                        (strpos a.pos) a.cost (strpos a.parent)) list

let distance src dst =
  let sx = float_of_int src.x in
  let sy = float_of_int src.y in
  let dx = float_of_int dst.x in
  let dy = float_of_int dst.y in
  sqrt((dx -. sx) ** 2.0 +. (dy -. sy) ** 2.0)

let cost pos =
  (distance start pos) +. (distance pos goal) +. 1.0

let valid_pos pos =
  pos.y >= 0 && pos.x >= 0 && pos.y <= max_y && pos.x <= max_x &&
  maze.(pos.y).(pos.x) = O

let remove_list list pos =
  List.filter (fun a -> a.pos <> pos) list

let sort_list list =
  List.sort (fun a b -> if a.cost > b.cost then 1 else -1) list

let find_list list pos =
  try Some (List.find (fun a -> pos = a.pos) list)
  with Not_found -> None

let search o c pos parent =
  if valid_pos pos
  then (
    let cost = cost pos in
    match find_list o pos with
    | Some n when cost < n.cost -> (n::(remove_list o pos), c)
    | Some n -> (o, c)
    | None -> (
      match find_list c pos with
      | Some n when cost < n.cost -> (n::o, remove_list c pos)
      | Some n -> (o, c)
      | None ->
          let node = {pos = pos; cost = cost; parent = parent} in
          (node::o, c)
    )
  )
  else (o, c)

let rec engine o c =
  let o = sort_list o in
  match o with
  | hd::tl when hd.pos = goal ->
      print_endline "GOOOOAL!"; (o, c)
  | hd::tl ->
      let pos = hd.pos in
      Printf.printf "pos => %s\n" (strpos pos); flush stdout;
      let (o, c) = (tl, hd::c) in
      let (o, c) = search o c {x = pos.x; y = pos.y + 1} pos in
      let (o, c) = search o c {x = pos.x; y = pos.y - 1} pos in
      let (o, c) = search o c {x = pos.x + 1; y = pos.y} pos in
      let (o, c) = search o c {x = pos.x - 1; y = pos.y} pos in
      engine o c
  | _ -> failwith "no route"

let print_route c =
  let rec loop node rt = function
    | [] -> node::rt
    | hd::tl when hd.pos = node.parent -> loop hd (node::rt) tl
    | hd::tl -> loop node rt tl in
  match c with
  | [] -> failwith "empty route"
  | hd::tl ->
      let rt = loop hd [] tl in
      plist rt; flush stdout

let main () =
  let (opn, cls) =
    engine [{pos = start; cost = (cost start); parent = start}] [] in
  print_route cls

let _ = main ()

こんな感じにしておいて実行させると

mitsu@garlic$ ocaml astar.ml
pos => pos_x[00] pos_y[00]
pos => pos_x[01] pos_y[00]
pos => pos_x[01] pos_y[01]
pos => pos_x[02] pos_y[01]
pos => pos_x[02] pos_y[02]
pos => pos_x[03] pos_y[02]
pos => pos_x[02] pos_y[00]
pos => pos_x[03] pos_y[00]
pos => pos_x[04] pos_y[00]
pos => pos_x[00] pos_y[01]
pos => pos_x[05] pos_y[00]
pos => pos_x[05] pos_y[01]
pos => pos_x[05] pos_y[02]
pos => pos_x[05] pos_y[03]
pos => pos_x[06] pos_y[03]
pos => pos_x[07] pos_y[03]
pos => pos_x[07] pos_y[04]
pos => pos_x[08] pos_y[04]
pos => pos_x[09] pos_y[04]
pos => pos_x[09] pos_y[05]
pos => pos_x[08] pos_y[05]
pos => pos_x[08] pos_y[03]
pos => pos_x[06] pos_y[04]
pos => pos_x[06] pos_y[02]
pos => pos_x[09] pos_y[03]
pos => pos_x[05] pos_y[04]
pos => pos_x[08] pos_y[02]
pos => pos_x[06] pos_y[05]
pos => pos_x[06] pos_y[01]
pos => pos_x[04] pos_y[04]
pos => pos_x[09] pos_y[02]
pos => pos_x[03] pos_y[04]
pos => pos_x[06] pos_y[06]
pos => pos_x[06] pos_y[00]
pos => pos_x[07] pos_y[00]
pos => pos_x[02] pos_y[04]
pos => pos_x[06] pos_y[07]
pos => pos_x[07] pos_y[07]
pos => pos_x[08] pos_y[07]
pos => pos_x[09] pos_y[07]
pos => pos_x[10] pos_y[07]
pos => pos_x[11] pos_y[07]
pos => pos_x[11] pos_y[06]
pos => pos_x[12] pos_y[06]
pos => pos_x[11] pos_y[05]
pos => pos_x[13] pos_y[06]
pos => pos_x[13] pos_y[07]
pos => pos_x[14] pos_y[07]
pos => pos_x[12] pos_y[05]
pos => pos_x[12] pos_y[07]
pos => pos_x[14] pos_y[06]
pos => pos_x[14] pos_y[08]
pos => pos_x[15] pos_y[08]
pos => pos_x[16] pos_y[08]
pos => pos_x[17] pos_y[08]
pos => pos_x[11] pos_y[04]
pos => pos_x[18] pos_y[08]
pos => pos_x[13] pos_y[05]
pos => pos_x[15] pos_y[06]
pos => pos_x[15] pos_y[09]
pos => pos_x[12] pos_y[04]
pos => pos_x[14] pos_y[05]
pos => pos_x[14] pos_y[09]
pos => pos_x[16] pos_y[06]
pos => pos_x[11] pos_y[03]
pos => pos_x[13] pos_y[04]
pos => pos_x[19] pos_y[08]
pos => pos_x[15] pos_y[05]
pos => pos_x[12] pos_y[03]
pos => pos_x[17] pos_y[06]
pos => pos_x[15] pos_y[10]
pos => pos_x[16] pos_y[10]
pos => pos_x[17] pos_y[10]
pos => pos_x[18] pos_y[10]
pos => pos_x[19] pos_y[10]
GOOOOAL!
<pos_x[00] pos_y[00] cost[23.360680] parent[pos_x[00] pos_y[00]]>
<pos_x[01] pos_y[00] cost[23.470911] parent[pos_x[00] pos_y[00]]>
<pos_x[02] pos_y[00] cost[23.591260] parent[pos_x[01] pos_y[00]]>
<pos_x[03] pos_y[00] cost[23.723083] parent[pos_x[02] pos_y[00]]>
<pos_x[04] pos_y[00] cost[23.867962] parent[pos_x[03] pos_y[00]]>
<pos_x[05] pos_y[00] cost[24.027756] parent[pos_x[04] pos_y[00]]>
<pos_x[05] pos_y[01] cost[23.591875] parent[pos_x[05] pos_y[00]]>
<pos_x[05] pos_y[02] cost[23.385165] parent[pos_x[05] pos_y[01]]>
<pos_x[05] pos_y[03] cost[23.383897] parent[pos_x[05] pos_y[02]]>
<pos_x[06] pos_y[03] cost[23.360680] parent[pos_x[05] pos_y[03]]>
<pos_x[06] pos_y[04] cost[23.442649] parent[pos_x[06] pos_y[03]]>
<pos_x[06] pos_y[05] cost[23.676318] parent[pos_x[06] pos_y[04]]>
<pos_x[06] pos_y[06] cost[24.045501] parent[pos_x[06] pos_y[05]]>
<pos_x[06] pos_y[07] cost[24.537366] parent[pos_x[06] pos_y[06]]>
<pos_x[07] pos_y[07] cost[24.241159] parent[pos_x[06] pos_y[07]]>
<pos_x[08] pos_y[07] cost[23.999463] parent[pos_x[07] pos_y[07]]>
<pos_x[09] pos_y[07] cost[23.803509] parent[pos_x[08] pos_y[07]]>
<pos_x[10] pos_y[07] cost[23.646862] parent[pos_x[09] pos_y[07]]>
<pos_x[11] pos_y[07] cost[23.525238] parent[pos_x[10] pos_y[07]]>
<pos_x[11] pos_y[06] cost[23.378822] parent[pos_x[11] pos_y[07]]>
<pos_x[12] pos_y[06] cost[23.360680] parent[pos_x[11] pos_y[06]]>
<pos_x[13] pos_y[06] cost[23.380079] parent[pos_x[12] pos_y[06]]>
<pos_x[13] pos_y[07] cost[23.380596] parent[pos_x[13] pos_y[06]]>
<pos_x[14] pos_y[07] cost[23.360680] parent[pos_x[13] pos_y[07]]>
<pos_x[14] pos_y[08] cost[23.449071] parent[pos_x[14] pos_y[07]]>
<pos_x[15] pos_y[08] cost[23.385165] parent[pos_x[14] pos_y[08]]>
<pos_x[15] pos_y[09] cost[23.591875] parent[pos_x[15] pos_y[08]]>
<pos_x[15] pos_y[10] cost[24.027756] parent[pos_x[15] pos_y[09]]>
<pos_x[16] pos_y[10] cost[23.867962] parent[pos_x[15] pos_y[10]]>
<pos_x[17] pos_y[10] cost[23.723083] parent[pos_x[16] pos_y[10]]>
<pos_x[18] pos_y[10] cost[23.591260] parent[pos_x[17] pos_y[10]]>
<pos_x[19] pos_y[10] cost[23.470911] parent[pos_x[18] pos_y[10]]>

あんまり、しっかり確認してないのだけど、最短ルートの表示を見る限り、それらしく動いているみたい。と思ったら、ちょい回り道してた… orz

実のところ最初は、「計算中のノードを格納しておくためのリスト（Openリスト）と、計算済みのノードを格納しておくリスト（Closeリスト）」は参照リストを置いておいて、代入しまくりのコードを書いていて動作確認まで終わったのだけど、どうにもこうにもコードが汚くなってしまったので、泣きながら参照しないように修正したのでした。

そしたら、ずっと分かりやすくなって、これからは参照とかしないよ絶対と思いました。