astar.ml 10.3 KB
 POTTIER Francois committed Mar 24, 2017 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. *) (* *) (******************************************************************************) `````` POTTIER Francois committed Jul 06, 2015 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 `````` POTTIER Francois committed Jul 06, 2015 53 54 55 `````` (* The source node(s). *) val sources: (node -> unit) -> unit `````` POTTIER Francois committed Jul 06, 2015 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 `````` POTTIER Francois committed Jul 06, 2015 72 73 `````` (* Nodes with low priorities are dealt with first. *) type priority = cost `````` POTTIER Francois committed Jul 06, 2015 74 `````` `````` POTTIER Francois committed Jul 12, 2015 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 `````` POTTIER Francois committed Jul 06, 2015 90 `````` type inode = { `````` POTTIER Francois committed Jul 06, 2015 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. *) `````` POTTIER Francois committed Jul 12, 2015 98 `````` mutable path: path; `````` POTTIER Francois committed Jul 06, 2015 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; `````` POTTIER Francois committed Jul 06, 2015 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 `````` POTTIER Francois committed Apr 29, 2016 128 `````` `````` POTTIER Francois committed Jul 06, 2015 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 `````` POTTIER Francois committed Jul 06, 2015 148 149 `````` (* Retrieve a node with lowest priority of the queue. *) val get: unit -> inode option `````` POTTIER Francois committed Jul 06, 2015 150 151 152 `````` end = struct `````` POTTIER Francois committed Jul 08, 2015 153 `````` module InfiniteArray = MenhirLib.InfiniteArray `````` POTTIER Francois committed Jul 06, 2015 154 `````` `````` POTTIER Francois committed Jul 08, 2015 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 `````` POTTIER Francois committed Jul 06, 2015 160 `````` `````` POTTIER Francois committed Jul 08, 2015 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 `````` POTTIER Francois committed Jul 06, 2015 168 169 170 `````` (* Adjust node's priority and insert into doubly linked list. *) let add inode priority = `````` POTTIER Francois committed Jul 08, 2015 171 172 `````` assert (0 <= priority); cardinal := !cardinal + 1; `````` POTTIER Francois committed Jul 06, 2015 173 `````` inode.priority <- priority; `````` POTTIER Francois committed Jul 08, 2015 174 `````` match InfiniteArray.get a priority with `````` POTTIER Francois committed Jul 06, 2015 175 `````` | None -> `````` POTTIER Francois committed Apr 28, 2016 176 `````` InfiniteArray.set a priority (Some inode); `````` POTTIER Francois committed Jul 08, 2015 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); `````` POTTIER Francois committed Apr 28, 2016 180 `````` (* if priority < !best then best := priority *) `````` POTTIER Francois committed Jul 06, 2015 181 `````` | Some inode' -> `````` POTTIER Francois committed Apr 28, 2016 182 183 184 185 `````` inode.next <- inode'; inode.prev <- inode'.prev; inode'.prev.next <- inode; inode'.prev <- inode `````` POTTIER Francois committed Jul 06, 2015 186 `````` `````` POTTIER Francois committed Jul 08, 2015 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. *) `````` POTTIER Francois committed Jul 06, 2015 189 `````` let remove inode = `````` POTTIER Francois committed Jul 08, 2015 190 `````` cardinal := !cardinal - 1; `````` POTTIER Francois committed Jul 06, 2015 191 `````` if inode.next == inode then `````` POTTIER Francois committed Jul 08, 2015 192 `````` InfiniteArray.set a inode.priority None `````` POTTIER Francois committed Jul 06, 2015 193 `````` else begin `````` POTTIER Francois committed Jul 08, 2015 194 `````` InfiniteArray.set a inode.priority (Some inode.next); `````` POTTIER Francois committed Apr 28, 2016 195 196 197 198 `````` inode.next.prev <- inode.prev; inode.prev.next <- inode.next; inode.next <- inode; inode.prev <- inode `````` POTTIER Francois committed Jul 06, 2015 199 200 201 `````` end; inode.priority <- -1 `````` POTTIER Francois committed Jul 08, 2015 202 203 `````` let rec get () = if !cardinal = 0 then `````` POTTIER Francois committed Apr 28, 2016 204 `````` None `````` POTTIER Francois committed Jul 06, 2015 205 `````` else `````` POTTIER Francois committed Jul 08, 2015 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 `````` POTTIER Francois committed Jul 06, 2015 220 221 222 `````` let add_or_decrease inode priority = if inode.priority >= 0 then `````` POTTIER Francois committed Apr 28, 2016 223 `````` remove inode; `````` POTTIER Francois committed Jul 06, 2015 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 `````` POTTIER Francois committed Jul 06, 2015 235 236 237 238 239 `````` let () = G.sources (fun node -> let rec inode = { this = node; cost = 0; `````` 240 `````` estimate = estimate node; `````` POTTIER Francois committed Jul 12, 2015 241 `````` path = Source node; `````` POTTIER Francois committed Jul 06, 2015 242 243 244 245 246 247 248 `````` prev = inode; next = inode; priority = -1 } in M.add node inode; P.add inode inode.estimate ) `````` POTTIER Francois committed Jul 06, 2015 249 `````` `````` POTTIER Francois committed Jul 06, 2015 250 `````` (* Access to the search results (after the search is over). *) `````` POTTIER Francois committed Jul 06, 2015 251 `````` `````` POTTIER Francois committed Jul 06, 2015 252 253 `````` let distance node = try (M.get node).cost with Not_found -> max_int `````` POTTIER Francois committed Jul 06, 2015 254 `````` `````` POTTIER Francois committed Jul 06, 2015 255 256 `````` let path node = (M.get node).path (* let [Not_found] escape if no path was found *) `````` POTTIER Francois committed Jul 06, 2015 257 `````` `````` POTTIER Francois committed Jul 06, 2015 258 `````` (* Search. *) `````` POTTIER Francois committed Jul 06, 2015 259 `````` `````` POTTIER Francois committed Jul 06, 2015 260 `````` let rec search f = `````` POTTIER Francois committed Jul 06, 2015 261 `````` `````` POTTIER Francois committed Jul 06, 2015 262 263 `````` (* Pick the open node that currently has lowest fhat, that is, lowest estimated distance to a goal node. *) `````` POTTIER Francois committed Jul 06, 2015 264 `````` `````` POTTIER Francois committed Jul 06, 2015 265 266 267 268 `````` match P.get() with | None -> (* Finished. *) distance, path `````` POTTIER Francois committed Jul 06, 2015 269 `````` `````` POTTIER Francois committed Jul 06, 2015 270 271 `````` | Some inode -> let node = inode.this in `````` POTTIER Francois committed Jul 06, 2015 272 `````` `````` POTTIER Francois committed Jul 06, 2015 273 274 `````` (* Let the user know about this newly discovered node. *) f (node, inode.path); `````` POTTIER Francois committed Jul 06, 2015 275 `````` `````` POTTIER Francois committed Jul 06, 2015 276 277 `````` (* Otherwise, examine its successors. *) G.successors node (fun label edge_cost son -> `````` 278 `````` assert (0 <= edge_cost); (* failure means user error *) `````` POTTIER Francois committed Jul 06, 2015 279 `````` `````` POTTIER Francois committed Jul 06, 2015 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 *) `````` POTTIER Francois committed Jul 06, 2015 284 `````` `````` POTTIER Francois committed Jul 06, 2015 285 286 287 `````` try let ison = M.get son in if new_cost < ison.cost then begin `````` POTTIER Francois committed Jul 06, 2015 288 `````` `````` POTTIER Francois committed Jul 06, 2015 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. *) `````` POTTIER Francois committed Jul 06, 2015 294 `````` `````` POTTIER Francois committed Jul 06, 2015 295 `````` let new_fhat = new_cost + ison.estimate in `````` 296 `````` assert (0 <= new_fhat); (* failure means overflow *) `````` POTTIER Francois committed Jul 06, 2015 297 298 `````` P.add_or_decrease ison new_fhat; ison.cost <- new_cost; `````` POTTIER Francois committed Jul 12, 2015 299 `````` ison.path <- Edge (label, inode.path) `````` POTTIER Francois committed Jul 06, 2015 300 `````` `````` POTTIER Francois committed Jul 06, 2015 301 302 `````` end with Not_found -> `````` POTTIER Francois committed Jul 06, 2015 303 `````` `````` POTTIER Francois committed Jul 06, 2015 304 305 `````` (* This son was never visited before. Allocate a new status record for it and mark it as open. *) `````` POTTIER Francois committed Jul 06, 2015 306 `````` `````` POTTIER Francois committed Jul 06, 2015 307 308 309 `````` let rec ison = { this = son; cost = new_cost; `````` 310 `````` estimate = estimate son; `````` POTTIER Francois committed Jul 12, 2015 311 `````` path = Edge (label, inode.path); `````` POTTIER Francois committed Jul 06, 2015 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 `````` POTTIER Francois committed Jul 06, 2015 320 `````` `````` POTTIER Francois committed Jul 06, 2015 321 `````` ); `````` POTTIER Francois committed Jul 06, 2015 322 `````` `````` POTTIER Francois committed Jul 06, 2015 323 `````` search f `````` POTTIER Francois committed Jul 06, 2015 324 325 `````` end``````