Commits

Anonymous committed bfb565a

faster bfs

Comments (0)

Files changed (2)

 module Pathfinding = struct
-    exception No_path_found
-
-  let build_path maze current visited = 
-    let rec loop current nodes path =  match nodes with
-      | [] ->  path
-      | (node, cost)::lst -> 
-        if Maze.get maze node <> '@' && cost = (snd current) - 1 
-          && Maze.distance node (fst current) = 1 
-          then loop (node, cost) lst (node::path)
-        else loop current lst path       
-    in    
-    loop current (List.rev visited) []
-    
+  exception No_path_found
   
-  let find_path maze (goal:int*int) ants = 
-    let root = (goal, 0) in
-    let rec loop queue visited = 
-      if queue = [] then raise No_path_found;
-      let current = List.hd queue and queue = List.tl queue in
-      let tile = Maze.get maze (fst current) in
+  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 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 queue visited
-        | _ -> 
-          let incr_cost node = node, (snd current) + 1 in
-          let not_visited node = not (List.mem node visited) in
-          let neighbors = List.map incr_cost (Maze.neighbor_nodes maze (fst current)) in
+      | '@' -> build_path maze current 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 neighbors = List.map incr_cost (Maze.neighbor_nodes maze current.coords) in
           let new_neighbors = List.filter not_visited neighbors in
-          loop (queue @ new_neighbors) (visited @ new_neighbors)
+          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;
+          loop new_visited
     in
-    loop [root] [root]
+    loop (VisitedSet.singleton root)
 end
 
 let sample =
   "    @####          @    #####\n" ^
   "@  #          ######### #####\n" ^
-  "##  #########@##    @      ##\n" ^
+  "##  ######### ##    @      ##\n" ^
   "#   ######### ## ######### ##\n" ^
   "# ########### ## ### ##### ##\n" ^
-  "#@                X  ### @   \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 1_000 do
+  for i = 0 to 1000 do
     ignore (Pathfinding.find_path maze goal ants)
   done;
   
   let after = Unix.gettimeofday () in
-  Maze.walk maze (List.rev path);
+  Maze.walk maze path;
   Maze.draw maze;
   Printf.printf "%d steps\n" (List.length path);
   let time = (after -. before) *. 1000. in
 let set maze (x, y) value = maze.(x).(y) <- value
 
 let walk maze path =
-  let walk_once coords = set maze coords '.' in
-  List.iter walk_once path;
-  set maze (List.hd (List.rev path)) 'X'
+  let walk_once coords = if get maze coords = ' ' then set maze coords '.' in
+  List.iter walk_once path
 
 let draw maze =
   for y = 0 to (height maze) -1 do
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.