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