astar.ml 7.77 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39
(* This module implements A* search, following Hart, Nilsson,
   and Raphael (1968).

   To each visited graph node, the algorithm associates an
   internal record, carrying various information. For this
   reason, the algorithm's space complexity is, in the worst
   case, linear in the size of the graph.

   The mapping of nodes to internal records is implemented
   via a hash table, while the converse mapping is direct
   (via a record field).

   Nodes that remain to be examined are kept in a priority
   queue, where the priority of a node is the cost of the
   shortest known path from the start node to it plus the
   estimated cost of a path from this node to a goal node.
   (Lower priority nodes are considered first).

   It is the use of the second summand that makes A* more
   efficient than Dijkstra's standard algorithm for finding
   shortest paths in an arbitrary graph. In fact, when
   [G.estimate] is the constant zero function, A* coincides
   with Dijkstra's algorithm. One should note that A* is
   faster than Dijkstra's algorithm only when a path to some
   goal node exists. Otherwise, both algorithms explore the
   entire graph, and have similar time requirements.

   The priority queue is implemented as an array of doubly
   linked lists. *)

module Make (G : sig

  (* Graph nodes. *)
  type node
  include Hashtbl.HashedType with type t := node

  (* Edge labels. *)
  type label

40 41 42
  (* The source node(s). *)

  val sources: (node -> unit) -> unit
43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67

  (* Whether a node is a goal node. *)
  val is_goal: node -> bool

  (* [successors n f] presents each of [n]'s successors, in
     an arbitrary order, to [f], together with the cost of
     the edge that was followed. *)
  val successors: node -> (label -> int -> node -> unit) -> unit

  (* An estimate of the cost of the shortest path from the
     supplied node to some goal node. For algorithms such as
     A* and IDA* to find shortest paths, this estimate must
     be a correct under-approximation of the actual cost. *)
  val estimate: node -> int

end) = struct

  type cost = int

  type priority = cost            (* Nodes with low priorities are dealt with first. *)

  type inode = {

      this: G.node;               (* Graph node associated with this internal record. *)

68
      mutable cost: cost;         (* Cost of the best known path from a source node to this node. (ghat) *)
69 70 71

      estimate: cost;             (* Estimated cost of the best path from this node to a goal node. (hhat) *)

72
      mutable father: inode;      (* Last node on the best known path from a source node to this node. *)
73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191

      mutable prev: inode;        (* Previous node on doubly linked priority list *)

      mutable next: inode;        (* Next node on doubly linked priority list *)

      mutable priority: priority; (* The node's priority, if the node is in the queue; -1 otherwise *)

  }

  (* This auxiliary module maintains a mapping of graph nodes
     to internal records. *)

  module M : sig

    (* Adds a binding to the mapping. *)
    val add: G.node -> inode -> unit

    (* Retrieves the internal record for this node.
       Raises [Not_found] no such record exists. *)
    val get: G.node -> inode

  end = struct

    module H =
      Hashtbl.Make(struct include G type t = node end)

    let t = H.create 100003

    let add node inode =
      H.add t node inode
       
    let get node =
      H.find t node

  end

  (* This auxiliary module maintains a priority queue of
     internal records. *)

  module P : sig

    (* Adds this node to the queue. *)
    val add: inode -> priority -> unit

    (* Adds this node to the queue, or changes its
       priority, if it already was in the queue. It
       is assumed, in the second case, that the priority
       can only decrease. *)
    val add_or_decrease: inode -> priority -> unit

    (* Retrieve a node with lowest priority of the queue.
       Raises [Not_found] if the queue is empty. *)
    val get: unit -> inode

  end = struct

    (* Maximum allowed priority. *)
    let max = 264

    (* Array of pointers to the doubly linked lists,
       indexed by priorities. *)
    let a = Array.make max None

    (* Index of lowest nonempty list. *)
    let best = ref max

    (* Adjust node's priority and insert into doubly linked list. *)
    let add inode priority =
      assert (priority < max);
      inode.priority <- priority;
      match a.(priority) with
      | None ->
	  a.(priority) <- Some inode;
	  if priority < !best then
	    best := priority
      | Some inode' ->
	  inode.next <- inode';
	  inode.prev <- inode'.prev;
	  inode'.prev.next <- inode;
	  inode'.prev <- inode

    (* Takes a node off its doubly linked list. Does not adjust
       [best]. *)
    let remove inode =
      if inode.next == inode then
	a.(inode.priority) <- None
      else begin
        a.(inode.priority) <- Some inode.next;
	inode.next.prev <- inode.prev;
	inode.prev.next <- inode.next;
	inode.next <- inode;
	inode.prev <- inode
      end;
      inode.priority <- -1

    let get () =
      if !best = max then
	raise Not_found (* queue is empty *)
      else
	match a.(!best) with
	| None ->
	    assert false
	| Some inode ->
            remove inode;
            (* look for next nonempty bucket *)
            while (!best < max) && (a.(!best) = None) do
	      incr best
	    done;
	    inode

    let add_or_decrease inode priority =
      if inode.priority >= 0 then
	remove inode;
      add inode priority

  end

  (* Initialization. *)

192 193 194 195 196 197 198 199 200 201 202 203 204 205
  let () =
    G.sources (fun node ->
      let rec inode = {
        this = node;
        cost = 0;
        estimate = G.estimate node;
        father = inode;
        prev = inode;
        next = inode;
        priority = -1
      } in
      M.add node inode;
      P.add inode inode.estimate
    )
206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297

  let expanded =
    ref 0

  (* Search. *)

  let rec search () =

    (* Pick the open node that currently has lowest fhat, (* TEMPORARY resolve ties in favor of goal nodes *)
       that is, lowest estimated distance to a goal node. *)

    let inode = P.get () in (* may raise Not_found; then, no goal node is reachable *)
    let node = inode.this in

    (* If it is a goal node, we are done. *)
    if G.is_goal node then
      inode
    else begin

      (* Monitoring. *)
      incr expanded;
      
      (* Otherwise, examine its successors. *)
      G.successors node (fun _ edge_cost son ->

        (* Determine the cost of the best known path from the
	   start node, through this node, to this son. *)
        let new_cost = inode.cost + edge_cost in

        try
          let ison = M.get son in
          if new_cost < ison.cost then begin

            (* This son has been visited before, but this new
	       path to it is shorter. If it was already open
	       and waiting in the priority queue, increase its
	       priority; otherwise, mark it as open and insert
	       it into the queue. *)

            let new_fhat = new_cost + ison.estimate in
	    P.add_or_decrease ison new_fhat;
            ison.cost <- new_cost;
            ison.father <- inode

          end
        with Not_found ->

          (* This son was never visited before. Allocate a new
             status record for it and mark it as open. *)

	  let e = G.estimate son in
          let rec ison = {
	    this = son;
            cost = new_cost;
	    estimate = e;
	    father = inode;
	    prev = ison;
	    next = ison;
            priority = -1
	  } in
	  M.add son ison;
	  P.add ison (new_cost + e)

      );

      search()

    end

  (* Main function. *)

  let path () =

    (* Find the nearest goal node. *)

    let goal = search() in

    (* Build the shortest path back to the start node. *)

    let rec build path inode =
      let path = inode.this :: path in
      let father = inode.father in
      if father == inode then
	path
      else
	build path father

    in
    let path = build [] goal in
    path

end