Source

ocaml-toys / maze / astar.ml

Diff from to

maze/astar.ml

-module Pathfinding = struct 
+module Pathfinding = struct
   type cost = { steps: int; estimation: int; camefrom: int * int }
   
   type node =
       let set_exploring node steps camefrom =
         matrix.(fst node).(snd node) <- Exploring { steps = steps; estimation = steps + (heuristic node); camefrom = camefrom }
       
-      let set_explored node = 
+      let set_explored node =
         let explored = match matrix.(fst node).(snd node) with
           | Exploring cost -> Explored cost
-          | _ -> invalid_arg ("not really explored: " ^ string_of_int (fst node) ^ ", " ^ string_of_int (snd node))
-        in 
+          | _ -> invalid_arg ("not in exploring state: " ^ string_of_int (fst node) ^ ", " ^ string_of_int (snd node))
+        in
         matrix.(fst node).(snd node) <- explored
-
-		  let rec reconstruct_path node path =
-		    let camefrom = match matrix.(fst node).(snd node) with
-		      | Explored cost -> cost.camefrom
-		      | Exploring cost -> cost.camefrom
-		      | _ -> node
-		    in
-		    if camefrom = node then node :: path
-		    else reconstruct_path camefrom (node :: path)
-        
-    end
-  
-  module MakeOpenSet(E: ExplorationType) = Set.Make(struct
-      type t = int * int
+      
+      let rec reconstruct_path node path =
+        let camefrom = match matrix.(fst node).(snd node) with
+          | Explored cost -> cost.camefrom
+          | Exploring cost -> cost.camefrom
+          | _ -> node
+        in
+        if camefrom = node then node :: path
+        else reconstruct_path camefrom (node :: path)
+      
       let estimate node = match node with
         | Exploring cost -> cost.estimation
         | Explored cost -> cost.estimation
         | Closed -> max_int
       
       let compare a b =
-        let score_a = estimate E.matrix.(fst a).(snd a) in
-        let score_b = estimate E.matrix.(fst b).(snd b) in
-        let diff = score_a - score_b in
-        if diff = 0 then Pervasives.compare a b else diff
-    end)
+        let pervasives = Pervasives.compare a b in
+        if pervasives = 0 then 0
+        else
+          let score_a = estimate matrix.(fst a).(snd a) in
+          let score_b = estimate matrix.(fst b).(snd b) in
+          let diff = score_a - score_b in
+          if diff = 0 then pervasives else diff
+      
+    end
   
   let find_path maze start goal =
-    let module Exploration = MakeExploration(struct 
-      let matrix = Array.make_matrix (Maze.width maze) (Maze.height maze) Unknown 
-      let heuristic = Maze.distance goal 
+    let module Exploration = MakeExploration(struct
+        let matrix = Array.make_matrix (Maze.width maze) (Maze.height maze) Unknown
+        let heuristic = Maze.distance goal
       end)
-    in 
-    let module OpenSet = MakeOpenSet(Exploration) in
-    let openset = ref (OpenSet.singleton start) in
-    let select_best set = OpenSet.min_elt set in
+    in
+    let openset = ref [start] in
     Exploration.set_exploring start 0 start;
     let rec loop () =
-      if OpenSet.is_empty !openset then raise No_path_found
+      if !openset = [] then raise No_path_found
       else
-        let current = select_best !openset in
-        if current = goal then List.rev (Exploration.reconstruct_path goal [])
-        else begin
-          openset := OpenSet.remove current !openset;
-          Exploration.set_explored current;
-          
-          let neighbors = Maze.neighbor_nodes maze current in
-          let explore node =
-            if Exploration.is_explorable maze node then
-              let tentative_steps = (Exploration.get_steps current) + 1 in
-              if not (OpenSet.mem node !openset) then begin
-                Exploration.set_exploring node tentative_steps current;
-                openset := OpenSet.add node !openset;
-              end
-              else if tentative_steps < (Exploration.get_steps node) then Exploration.set_exploring node tentative_steps current
-          in
-          List.iter explore neighbors;
-          loop ()
-        end
+        openset := List.sort Exploration.compare !openset;
+      let current = List.hd !openset in
+      if current = goal then List.rev (Exploration.reconstruct_path goal [])
+      else begin
+        openset := List.tl !openset;
+        Exploration.set_explored current;
+        
+        let neighbors = Maze.neighbor_nodes maze current in
+        let explore node =
+          if Exploration.is_explorable maze node then
+            let tentative_steps = (Exploration.get_steps current) + 1 in
+            if not (List.mem node !openset) then begin
+              Exploration.set_exploring node tentative_steps current;
+              openset := node :: !openset
+            end
+            else if tentative_steps < (Exploration.get_steps node) then Exploration.set_exploring node tentative_steps current
+        in
+        List.iter explore neighbors;
+        loop ()
+      end
     in
-    loop () 
+    loop ()
 end
 
-let sample =
-  "@    ####               #####\n" ^
-  "   #          ######### #####\n" ^
-  "##  ######### ##           ##\n" ^
-  "#   ######### ## ######### ##\n" ^
-  "# ########### ## ######### ##\n" ^
-  "#                #######    X\n" ^
-  "############## ########  ####\n" ^
-  "##############          #####"
-  
-(*
 let () =
   let maze, goal, ants = Maze.Parser.from_file "maze/maze.txt" in
   let start = List.hd ants in
   Maze.draw maze;
   Printf.printf "%d steps\n" (List.length path);
   let time = (after -. before) *. 1000. in
-  Printf.printf "%f ms\n" time
-*)
+  Printf.printf "%f ms\n" time