lib/a_star.nit

   1 # This file is part of NIT (http://www.nitlanguage.org).
   2 #
   3 # Copyright 2011-2013 Alexis Laferrière <alexis.laf@xymus.net>
   4 #
   5 # Licensed under the Apache License, Version 2.0 (the "License");
   6 # you may not use this file except in compliance with the License.
   7 # You may obtain a copy of the License at
   8 #
   9 #     http://www.apache.org/licenses/LICENSE-2.0
  10 #
  11 # Unless required by applicable law or agreed to in writing, software
  12 # distributed under the License is distributed on an "AS IS" BASIS,
  13 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  14 # See the License for the specific language governing permissions and
  15 # limitations under the License.
  16
  17 # Services related to pathfinding of graphs using A*
  18 # A single graph may have different properties according to the `PathContext` used
  19 #
  20 # Usage:
  21 #
  22 #    # Weighted graph (letters are nodes, digits are weights):
  23 #    #
  24 #    #     a -2- b
  25 #    #    /     /
  26 #    #   3     1
  27 #    #  /     /
  28 #    # c -3- d -8- e
  29 #    #
  30 #    var graph = new Graph[Node,WeigthedLink[Node]]
  31 #
  32 #    var na = new Node(graph)
  33 #    var nb = new Node(graph)
  34 #    var nc = new Node(graph)
  35 #    var nd = new Node(graph)
  36 #    var ne = new Node(graph)
  37 #
  38 #    var lab = new WeightedLink(graph, na, nb, 2)
  39 #    var lac = new WeightedLink(graph, na, nc, 3)
  40 #    var lbd = new WeightedLink(graph, nb, nd, 1)
  41 #    var lcd = new WeightedLink(graph, nc, nd, 3)
  42 #    var lde = new WeightedLink(graph, nd, ne, 8)
  43 #
  44 #    var context = new WeightedPathContext(graph)
  45 #
  46 #    var path = na.path_to(ne, 100, context)
  47 #    assert path != null else print "No possible path"
  48 #
  49 #    assert path.step == nb
  50 #    assert path.step == nd
  51 #    assert path.step == ne
  52 #    assert path.at_end_of_path
  53 module a_star
  54
  55 redef class Object
  56         protected fun debug_a_star: Bool do return false
  57         private fun debug(msg: String) do if debug_a_star then
  58                 stderr.write "a_star debug: {msg}\n"
  59         end
  60 end
  61
  62 # General graph node
  63 class Node
  64         type N: Node
  65
  66         # parent graph
  67         var graph: Graph[N, Link]
  68
  69         init(graph: Graph[N, Link])
  70         do
  71                 self.graph = graph
  72                 graph.add_node(self)
  73         end
  74
  75         # adjacent nodes
  76         var links: Set[Link] = new HashSet[Link]
  77
  78         # used to check if node has been searched in one pathfinding
  79         private var last_pathfinding_evocation: Int = 0
  80
  81         # cost up to in current evocation
  82         # lifetime limited to evocation of path_to
  83         private var best_cost_up_to: Int = 0
  84
  85         # source node
  86         # lifetime limited to evocation of path_to
  87         private var best_source: nullable N = null
  88
  89         # is in frontier or buckets
  90         # lifetime limited to evocation of path_to
  91         private var open: Bool = false
  92
  93
  94         # Main functionnality, returns path from `self` to `dest`
  95         fun path_to(dest: Node, max_cost: Int, context: PathContext): nullable Path[N]
  96         do
  97                 var cost = 0
  98
  99                 var nbr_buckets = context.worst_cost + context.worst_heuristic_cost + 1
 100                 var buckets = new Array[List[Node]].with_capacity(nbr_buckets)
 101
 102                 for i in [0 .. nbr_buckets[ do
 103                         buckets.add(new List[Node])
 104                 end
 105
 106                 graph.pathfinding_current_evocation += 1
 107
 108                 # open source node
 109                 buckets[0].add(self)
 110                 open = true
 111                 self.last_pathfinding_evocation = graph.pathfinding_current_evocation
 112                 self.best_cost_up_to = 0
 113
 114                 loop
 115                         var frontier_node: nullable Node = null
 116
 117                         var bucket_searched: Int = 0
 118
 119                         # find next valid node in frontier/buckets
 120                         loop
 121                                 var current_bucket = buckets[cost % nbr_buckets]
 122
 123                                 if current_bucket.is_empty then # move to next bucket
 124                                         debug "b {cost} {cost % nbr_buckets} {buckets[cost % nbr_buckets].hash}"
 125                                         cost += 1
 126                                         if cost > max_cost then return null
 127                                         bucket_searched += 1
 128
 129                                         if bucket_searched > nbr_buckets then break
 130                                 else # found a node
 131                                         debug "c {cost}"
 132                                         frontier_node = current_bucket.pop
 133
 134                                         if frontier_node.open then break
 135                                 end
 136                         end
 137
 138                         # no path possible
 139                         if frontier_node == null then
 140                                 return null
 141
 142                         # at destination
 143                         else if frontier_node == dest then
 144                                 debug "picked {frontier_node}, is destination"
 145
 146                                 var path = new Path[N](cost)
 147
 148                                 while frontier_node != self do
 149                                         path.nodes.unshift(frontier_node)
 150                                         frontier_node = frontier_node.best_source.as(not null)
 151                                 end
 152
 153                                 return path
 154
 155                         # adds all next nodes to frontier/buckets
 156                         else
 157                                 frontier_node.open = false
 158
 159                                 debug "w exploring adjacents of {frontier_node}"
 160
 161                                 for link in frontier_node.links do
 162                                         var peek_node = link.to
 163                                         debug "v {context.is_blocked(link)} {peek_node.last_pathfinding_evocation != graph.pathfinding_current_evocation} {peek_node.best_cost_up_to > frontier_node.best_cost_up_to + context.cost(link)}, {peek_node.best_cost_up_to} > {frontier_node.best_cost_up_to} + {context.cost(link)}"
 164                                         if not context.is_blocked(link) and
 165                                          (peek_node.last_pathfinding_evocation != graph.pathfinding_current_evocation or
 166                                            (peek_node.open and
 167                                              peek_node.best_cost_up_to > cost + context.cost(link)))
 168                                         then
 169
 170                                                 peek_node.open = true
 171                                                 peek_node.last_pathfinding_evocation = graph.pathfinding_current_evocation
 172                                                 peek_node.best_cost_up_to = cost + context.cost(link)
 173                                                 peek_node.best_source = frontier_node
 174
 175                                                 var est_cost = peek_node.best_cost_up_to+context.heuristic_cost(link.from, peek_node)
 176                                                 var at_bucket = buckets[est_cost % nbr_buckets]
 177                                                 at_bucket.add(peek_node)
 178
 179                                                 debug "u putting {peek_node} at {est_cost} -> {est_cost % nbr_buckets} {at_bucket.hash}, {cost}+{context.cost(link)}"
 180                                         end
 181                                 end
 182                         end
 183                 end
 184         end
 185 end
 186
 187 # Link between two nodes and associated to a graph
 188 class Link
 189         type N: Node
 190         type L: Link
 191
 192         var graph: Graph[N, L]
 193
 194         var from: N
 195         var to: N
 196
 197         init(graph: Graph[N, L], from, to: N)
 198         do
 199                 self.graph = graph
 200                 self.from = from
 201                 self.to = to
 202
 203                 graph.add_link(self)
 204         end
 205 end
 206
 207 # General graph
 208 class Graph[N: Node, L: Link]
 209         var nodes: Set[N] = new HashSet[N]
 210         var links: Set[L] = new HashSet[L]
 211
 212         fun add_node(node: N): N
 213         do
 214                 nodes.add(node)
 215
 216                 return node
 217         end
 218
 219         fun add_link(link: L): L
 220         do
 221                 links.add(link)
 222
 223                 link.from.links.add(link)
 224
 225                 return link
 226         end
 227
 228         # used to check if nodes have been searched in one pathfinding
 229         var pathfinding_current_evocation: Int = 0
 230 end
 231
 232 # Result from pathfinding, a walkable path
 233 class Path[N]
 234
 235         var total_cost: Int
 236
 237         var nodes = new List[N]
 238
 239         init (cost: Int) do total_cost = cost
 240
 241         var at: Int = 0
 242         fun step: N
 243         do
 244                 assert nodes.length >= at else print "a_star::Path::step failed, is at_end_of_path"
 245
 246                 var s = nodes[at]
 247                 at += 1
 248
 249                 return s
 250         end
 251
 252         fun peek_step: N do return nodes[at]
 253
 254         fun at_end_of_path: Bool do return at >= nodes.length
 255 end
 256
 257 # Context related to an evocation of pathfinding
 258 class PathContext
 259         type N: Node
 260         type L: Link
 261
 262         var graph: Graph[N, L]
 263
 264         # Worst cost of all the link's costs
 265         fun worst_cost: Int is abstract
 266
 267         # Get cost of a link
 268         fun cost(link: L): Int is abstract
 269
 270         # Is that link blocked?
 271         fun is_blocked(link: L): Bool is abstract
 272
 273         # Heuristic
 274         fun heuristic_cost(a, b: N): Int is abstract
 275
 276         fun worst_heuristic_cost: Int is abstract
 277 end
 278
 279 #
 280 ### Additionnal classes, may be useful
 281 #
 282
 283 # Simple context with constant cost on each links
 284 # Warning: A* is not optimize for such a case
 285 class ConstantPathContext
 286         super PathContext
 287
 288         redef fun worst_cost do return 1
 289         redef fun cost(l) do return 1
 290         redef fun is_blocked(l) do return false
 291         redef fun heuristic_cost(a, b) do return 0
 292         redef fun worst_heuristic_cost do return 0
 293 end
 294
 295 class WeightedPathContext
 296         super PathContext
 297
 298         redef type L: WeightedLink
 299
 300         init(graph: Graph[N, L])
 301         do
 302                 super
 303
 304                 var worst_cost = 0
 305                 for l in graph.links do
 306                         var cost = l.weight
 307                         if cost >= worst_cost then worst_cost = cost + 1
 308                 end
 309                 self.worst_cost = worst_cost
 310         end
 311
 312         redef var worst_cost: Int
 313
 314         redef fun cost(l) do
 315                 return l.weight
 316         end
 317         redef fun is_blocked(l) do return false
 318         redef fun heuristic_cost(a, b) do return 0
 319         redef fun worst_heuristic_cost do return 0
 320 end
 321
 322 class WeightedLink
 323         super Link
 324
 325         var weight: Int
 326
 327         init(graph: Graph[N, L], from, to: N, weight: Int)
 328         do
 329                 super
 330
 331                 self.weight = weight
 332         end
 333 end