astar.ml 10.3 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13
(******************************************************************************)
(*                                                                            *)
(*                                   Menhir                                   *)
(*                                                                            *)
(*                       François Pottier, Inria Paris                        *)
(*              Yann Régis-Gianas, PPS, Université Paris Diderot              *)
(*                                                                            *)
(*  Copyright Inria. All rights reserved. This file is distributed under the  *)
(*  terms of the GNU General Public License version 2, as described in the    *)
(*  file LICENSE.                                                             *)
(*                                                                            *)
(******************************************************************************)

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 40 41 42 43 44 45 46 47 48 49 50 51 52
(* 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

53 54 55
  (* The source node(s). *)

  val sources: (node -> unit) -> unit
56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71

  (* [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

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

75 76 77 78 79 80 81 82 83 84 85 86 87 88 89
  (* Paths back to a source (visible by the user). *)
  type path =
    | Edge of G.label * path
    | Source of G.node

  let rec follow labels path =
    match path with
    | Source node ->
        node, labels
    | Edge (label, path) ->
        follow (label :: labels) path

  let reverse path =
    follow [] path

90
  type inode = {
91 92 93 94 95 96 97
      (* Graph node associated with this internal record. *)
      this: G.node;
      (* Cost of the best known path from a source node to this node. (ghat) *)
      mutable cost: cost;
      (* Estimated cost of the best path from this node to a goal node. (hhat) *)
      estimate: cost;
      (* Best known path from a source node to this node. *)
98
      mutable path: path;
99 100 101 102 103 104
      (* Previous node on doubly linked priority list *)
      mutable prev: inode;
      (* Next node on doubly linked priority list *)
      mutable next: inode;
      (* The node's priority, if the node is in the queue; -1 otherwise *)
      mutable priority: priority;
105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127
  }

  (* 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
128

129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147
    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

148 149
    (* Retrieve a node with lowest priority of the queue. *)
    val get: unit -> inode option
150 151 152

  end = struct

153
    module InfiniteArray = MenhirLib.InfiniteArray
154

155 156 157 158 159
    (* Array of pointers to the doubly linked lists, indexed by priorities.
       There is no a priori bound on the size of this array -- its size is
       increased if needed. It is up to the user to use a graph where paths
       have reasonable lengths. *)
    let a = InfiniteArray.make None
160

161 162 163 164 165 166 167
    (* Index of lowest nonempty list, if there is one; or lower (sub-optimal,
       but safe). If the queue is empty, [best] is arbitrary. *)
    let best = ref 0

    (* Current number of elements in the queue. Used in [get] to stop the
       search for a nonempty bucket. *)
    let cardinal = ref 0
168 169 170

    (* Adjust node's priority and insert into doubly linked list. *)
    let add inode priority =
171 172
      assert (0 <= priority);
      cardinal := !cardinal + 1;
173
      inode.priority <- priority;
174
      match InfiniteArray.get a priority with
175
      | None ->
176
          InfiniteArray.set a priority (Some inode);
177 178 179
          (* Decrease [best], if necessary, so as not to miss the new element.
             In the special case of A*, this never happens. *)
          assert (!best <= priority);
180
          (* if priority < !best then best := priority *)
181
      | Some inode' ->
182 183 184 185
          inode.next <- inode';
          inode.prev <- inode'.prev;
          inode'.prev.next <- inode;
          inode'.prev <- inode
186

187 188
    (* Takes a node off its doubly linked list. Does not adjust [best],
       as this is not necessary in order to preserve the invariant. *)
189
    let remove inode =
190
      cardinal := !cardinal - 1;
191
      if inode.next == inode then
192
        InfiniteArray.set a inode.priority None
193
      else begin
194
        InfiniteArray.set a inode.priority (Some inode.next);
195 196 197 198
        inode.next.prev <- inode.prev;
        inode.prev.next <- inode.next;
        inode.next <- inode;
        inode.prev <- inode
199 200 201
      end;
      inode.priority <- -1

202 203
    let rec get () =
      if !cardinal = 0 then
204
        None
205
      else
206 207 208 209 210 211 212 213 214 215 216 217 218 219
        get_nonempty()

    and get_nonempty () =
      (* Look for next nonempty bucket. We know there is one. This may
         seem inefficient, because it is a linear search. However, in
         A*, [best] never decreases, so the total cost of this loop is
         the maximum priority ever used. *)
      match InfiniteArray.get a !best with
      | None ->
          best := !best + 1;
          get_nonempty()
      | Some inode as result ->
          remove inode;
          result
220 221 222

    let add_or_decrease inode priority =
      if inode.priority >= 0 then
223
        remove inode;
224 225 226 227 228 229
      add inode priority

  end

  (* Initialization. *)

230 231 232 233 234
  let estimate node =
    let e = G.estimate node in
    assert (0 <= e); (* failure means user error *)
    e

235 236 237 238 239
  let () =
    G.sources (fun node ->
      let rec inode = {
        this = node;
        cost = 0;
240
        estimate = estimate node;
241
        path = Source node;
242 243 244 245 246 247 248
        prev = inode;
        next = inode;
        priority = -1
      } in
      M.add node inode;
      P.add inode inode.estimate
    )
249

250
  (* Access to the search results (after the search is over). *)
251

252 253
  let distance node =
    try (M.get node).cost with Not_found -> max_int
254

255 256
  let path node =
    (M.get node).path (* let [Not_found] escape if no path was found *)
257

258
  (* Search. *)
259

260
  let rec search f =
261

262 263
    (* Pick the open node that currently has lowest fhat,
       that is, lowest estimated distance to a goal node. *)
264

265 266 267 268
    match P.get() with
    | None ->
       (* Finished. *)
       distance, path
269

270 271
    | Some inode ->
        let node = inode.this in
272

273 274
        (* Let the user know about this newly discovered node. *)
        f (node, inode.path);
275

276 277
        (* Otherwise, examine its successors. *)
        G.successors node (fun label edge_cost son ->
278
          assert (0 <= edge_cost); (* failure means user error *)
279

280 281 282
          (* 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
283
          assert (0 <= new_cost); (* failure means overflow *)
284

285 286 287
          try
            let ison = M.get son in
            if new_cost < ison.cost then begin
288

289 290 291 292 293
              (* 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. *)
294

295
              let new_fhat = new_cost + ison.estimate in
296
              assert (0 <= new_fhat); (* failure means overflow *)
297 298
              P.add_or_decrease ison new_fhat;
              ison.cost <- new_cost;
299
              ison.path <- Edge (label, inode.path)
300

301 302
            end
          with Not_found ->
303

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

307 308 309
            let rec ison = {
              this = son;
              cost = new_cost;
310
              estimate = estimate son;
311
              path = Edge (label, inode.path);
312 313 314 315 316
              prev = ison;
              next = ison;
              priority = -1
            } in
            M.add son ison;
317 318 319
            let fhat = new_cost + ison.estimate in
            assert (0 <= fhat); (* failure means overflow *)
            P.add ison fhat
320

321
        );
322

323
        search f
324 325

end