Source

ocaml-toys / maze / bfs.ml

module Pathfinding = struct
  exception No_path_found
  
  type visited = { coords: int * int; cost: int }
  
  let build_path maze current visited =
    let rec loop current path =
      if Stack.is_empty visited then path
      else
        let node = Stack.pop visited in
        if Maze.get maze node.coords <> '#' && node.cost = current.cost - 1
          && Maze.distance node.coords current.coords = 1
        then loop node (node.coords :: path)
        else loop current path
    in
    loop current []
  
  let find_path maze goal ants =
    let root = { coords = goal; cost =0 } in
    let matrix = Array.make_matrix (Maze.width maze) (Maze.height maze) false in
    let queue, visited = Queue.create(), Stack.create() in
    let mark node =
      Queue.add node queue;
      Stack.push node visited;
      matrix.(fst node.coords).(snd node.coords) <- true
    in
    mark root;
    let rec loop () =
      if Queue.is_empty queue then raise No_path_found;
      let current = Queue.take queue in
      let tile = Maze.get maze current.coords in
      match tile with
      | '@' -> build_path maze current visited
      | '#' -> loop ()
      | _ ->
          let incr_cost coords = { coords = coords; cost = (succ current.cost) } in
          let not_visited node = not (matrix.(fst node.coords).(snd node.coords)) in
          let neighbors = List.map incr_cost (Maze.neighbor_nodes maze current.coords) in
          let new_neighbors = List.filter not_visited neighbors in
          List.iter mark new_neighbors;
          loop ()
    in
    loop ()
end

let sample =
  "    @####          @    #####\n" ^
  "@  #          ######### #####\n" ^
  "##  ######### ##    @      ##\n" ^
  "#   ######### ## ######### ##\n" ^
  "# ########### ## ### ##### ##\n" ^
  "#@                   ###  @  \n" ^
  "############## ##### ##  ####\n" ^
  "############## X        #####"

let () =
  let maze, goal, ants = Maze.Parser.from_string sample in
  let before = Unix.gettimeofday () in
  let path = Pathfinding.find_path maze goal ants in
  
  for i = 0 to 1000 do
    ignore (Pathfinding.find_path maze goal ants)
  done;
  
  let after = Unix.gettimeofday () in
  Maze.walk maze path;
  Maze.draw maze;
  Printf.printf "%d steps\n" (List.length path);
  let time = (after -. before) *. 1000. in
  Printf.printf "%f ms\n" time