Commits

Anonymous committed 87e9938

more functors, less code in the find_path function

Comments (0)

Files changed (1)

 end
 
 module Maze = struct
-  let width maze = Array.length maze 
+  let width maze = Array.length maze
   let height maze = Array.length maze.(0)
   let get maze (x, y) = maze.(x).(y)
   let set maze (x, y) value = maze.(x).(y) <- value
     let nodes = if x < (width maze) - 1 then (x +1, y) :: nodes else nodes in
     let nodes = if y > 0 then (x, y -1) :: nodes else nodes in
     let nodes = if y < (height maze) - 1 then (x, y +1) :: nodes else nodes in
-    nodes  
+    nodes
 end
 
-exception No_path_found
+(***********************************************************)
 
-type cost = { steps: int; estimation: int; camefrom: int * int }
+module Pathfinding = struct 
+  type cost = { steps: int; estimation: int; camefrom: int * int }
+  
+  type node =
+    | Unknown
+    | Closed
+    | Exploring of cost
+    | Explored of cost
+  
+  exception No_path_found
+  
+  module type ExplorationType = sig
+    val matrix : node array array
+    val heuristic : int * int -> int
+  end
+  
+  module MakeExploration = functor(E: ExplorationType) ->
+    struct
+      let matrix = E.matrix
+      let heuristic = E.heuristic
+      
+      let is_explorable maze node =
+        if not (Maze.is_passable maze node) then false
+        else match matrix.(fst node).(snd node) with
+          | Closed -> false
+          | Unknown -> true
+          | Exploring _ -> true
+          | Explored _ -> false
+      
+      let get_steps node = match matrix.(fst node).(snd node) with
+        | Exploring cost -> cost.steps
+        | Explored cost -> cost.steps
+        | Closed -> max_int
+        | Unknown -> invalid_arg "unknown 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 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 
+        matrix.(fst node).(snd node) <- explored
 
-type node =
-  | Unknown
-  | Closed
-  | Exploring of cost
-  | Explored of cost
-
-let rec reconstruct_path matrix 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 matrix camefrom (node :: path)
-
-let find_path maze start goal =
-  let heuristic = Maze.distance in
-  let matrix = Array.make_matrix (Maze.width maze) (Maze.height maze) Unknown in
+		  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
   
-  let module CoordSet = Set.Make(
-    struct
+  module MakeOpenSet(E: ExplorationType) = Set.Make(struct
       type t = int * int
       let estimate node = match node with
         | Exploring cost -> cost.estimation
         | Closed -> max_int
       
       let compare a b =
-        let score_a = estimate matrix.(fst a).(snd a) in
-        let score_b = estimate matrix.(fst b).(snd b) in
+        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) in
+    end)
   
-  let openset = ref (CoordSet.singleton start) in
-  let select_best set = CoordSet.min_elt set in
-  let is_explorable maze node =
-    if not (Maze.is_passable maze node) then false
-    else match matrix.(fst node).(snd node) with
-      | Closed -> false
-      | Unknown -> true
-      | Exploring _ -> true
-      | Explored _ -> false
-  in
-  let get_steps node = match matrix.(fst node).(snd node) with
-    | Exploring cost -> cost.steps
-    | Explored cost -> cost.steps
-    | Closed -> max_int
-    | Unknown -> failwith "should not be called"
-  in
-  let set_exploring node steps camefrom =
-    matrix.(fst node).(snd node) <- Exploring { steps = steps; estimation = steps + (heuristic node goal); camefrom = camefrom };
-  in
-  let explored node = match matrix.(fst node).(snd node) with
-    | Exploring cost -> Explored cost
-    | _ -> failwith ("not really explored: " ^ string_of_int (fst node) ^ ", " ^ string_of_int (snd node))
-  in
-  set_exploring start 0 start;
-  let rec loop () =
-    if CoordSet.is_empty !openset then raise No_path_found
-    else
-      let current = select_best !openset in
-      if current = goal then List.rev (reconstruct_path matrix goal [])
-      else begin
-        openset := CoordSet.remove current !openset;
-        matrix.(fst current).(snd current) <- explored current;
-        
-        let neighbors = Maze.neighbor_nodes maze current in
-        let explore node =
-          if is_explorable maze node then
-            let tentative_steps = (get_steps current) + 1 in
-            if not (CoordSet.mem node !openset) then begin
-              set_exploring node tentative_steps current;
-              openset := CoordSet.add node !openset;
-            end
-            else if tentative_steps < (get_steps node) then set_exploring node tentative_steps current
-        in
-        List.iter explore neighbors;
-        loop ()
-      end
-  in
-  loop ()
+  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 
+      end)
+    in 
+    let module OpenSet = MakeOpenSet(Exploration) in
+    let openset = ref (OpenSet.singleton start) in
+    let select_best set = OpenSet.min_elt set in
+    Exploration.set_exploring start 0 start;
+    let rec loop () =
+      if OpenSet.is_empty !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
+    in
+    loop () 
+end
+
+let sample =
+  "S    ####               #####\n" ^
+  "   #          ######### #####\n" ^
+  "##  ######### ##           ##\n" ^
+  "#   ######### ## ######### ##\n" ^
+  "# ########### ## ######### ##\n" ^
+  "#                #######    G\n" ^
+  "############## ########  ####\n" ^
+  "##############          #####"
 
 let () =
   let maze, start, goal = MazeParse.from_file "maze/maze.txt" in
   let before = Unix.gettimeofday () in
-  let path = find_path maze start goal in
+  let path = Pathfinding.find_path maze start goal in
   
-  for i = 0 to 1000 do
-    ignore (find_path maze start goal)
+  for i = 0 to 1_000 do
+    ignore (Pathfinding.find_path maze start goal)
   done;
   
   let after = Unix.gettimeofday () 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