Source

ocaml-toys / maze / bfs.ml

Diff from to

maze/bfs.ml

       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
+        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
   
   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 =
       | '#' -> loop visited
       | _ ->
           let incr_cost coords = {coords=coords; cost=(succ current.cost)} in
-          let not_visited node = not (VisitedSet.mem node visited) 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
-          List.iter (fun elt -> Queue.add elt queue) new_neighbors;
+          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)
   "##  ######### ##    @      ##\n" ^
   "#   ######### ## ######### ##\n" ^
   "# ########### ## ### ##### ##\n" ^
-  "#@                   ### @   \n" ^
+  "#@                   ###  @  \n" ^
   "############## ##### ##  ####\n" ^
   "############## X        #####"