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