ocaml-toys / maze / bfs.ml

module Pathfinding = struct
  exception No_path_found
  
  type visited = {coords:int*int; cost:int}
  
  module VisitedSet = Set.Make(struct
      type t = visited
      
      let compare a b =
        let cost_diff = b.cost - a.cost in
        if cost_diff <> 0 then cost_diff
        else Pervasives.compare a.coords b.coords     
    end)
     
  let build_path maze current visited =
    let rec loop current nodes path =
      if VisitedSet.is_empty nodes then path
      else
        let node = VisitedSet.min_elt nodes in
        let nodes = VisitedSet.remove node nodes in
        if Maze.get maze node.coords <> '#' && node.cost = current.cost - 1
        && Maze.distance node.coords current.coords = 1
        then loop node nodes (node.coords :: path)
        else loop current nodes path
    in
    loop current visited []
  
  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 = Queue.create () in
    Queue.push root queue;
    let rec loop visited =
      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 visited
      | _ ->
          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
          let new_visited = List.fold_left (fun set elt -> VisitedSet.add elt set) visited new_neighbors in
          let mark node = 
            Queue.add node queue; 
            matrix.(fst node.coords).(snd node.coords) <- true 
          in
          List.iter mark new_neighbors;
          loop new_visited
    in
    loop (VisitedSet.singleton root)
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
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.