-module Pathfinding = struct

+module Pathfinding = struct

type cost = { steps: int; estimation: int; camefrom: int * int }

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 in exploring state: " ^ string_of_int (fst node) ^ ", " ^ string_of_int (snd node))

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

- if camefrom = node then node :: path

- else reconstruct_path camefrom (node :: path)

+ let rec reconstruct_path node path =

+ let camefrom = match matrix.(fst node).(snd node) with

+ | Explored cost -> cost.camefrom

+ | Exploring cost -> cost.camefrom

+ if camefrom = node then node :: path

+ else reconstruct_path camefrom (node :: path)

+ module MakeOpenSet(E: ExplorationType) = Set.Make(struct

let estimate node = match node with

| Exploring cost -> cost.estimation

| Explored cost -> cost.estimation

- let pervasives = Pervasives.compare a b in

- if pervasives = 0 then 0

- 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

+ 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

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

- let openset = ref [start] 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;

- if !openset~~ = []~~ then raise No_path_found

+ if OpenSet.is_empty !openset then raise No_path_found

- openset := List.sort Exploration.compare !openset;

- let current = List.hd !openset in

- if current = goal then List.rev (Exploration.reconstruct_path goal [])

- openset := List.tl !openset;

- Exploration.set_explored current;

- let neighbors = Maze.neighbor_nodes maze current in

- 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

- else if tentative_steps < (Exploration.get_steps node) then Exploration.set_exploring node tentative_steps current

- List.iter explore neighbors;

+ let current = select_best !openset in

+ if current = goal then List.rev (Exploration.reconstruct_path goal [])

+ openset := OpenSet.remove current !openset;

+ Exploration.set_explored current;

+ let neighbors = Maze.neighbor_nodes maze current in

+ 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;

+ else if tentative_steps < (Exploration.get_steps node) then begin

+ (* remove & add are needed, as the score changes, the place in the tree changes too *)

+ openset := OpenSet.remove node !openset;

+ Exploration.set_exploring node tentative_steps current;

+ openset := OpenSet.add node !openset

+ List.iter explore neighbors;

let maze, goal, ants = Maze.Parser.from_file "maze/maze.txt" in