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 # A* pathfinding in graphs
  18 #
  19 # A single graph may have different properties according to the `PathContext` used
  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,WeightedLink]
  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 import serialization
  59
  60 # General graph node
  61 class Node
  62         super Serializable
  63
  64         # Type of the others nodes in the `graph`
  65         type N: Node
  66
  67         # parent graph
  68         var graph: Graph[N, Link]
  69
  70         init
  71         do
  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         # Main functionality, returns path from `self` to `dest`
  94         fun path_to(dest: N, max_cost: Int, context: PathContext): nullable AStarPath[N]
  95         do
  96                 return path_to_alts(dest, max_cost, context, null)
  97         end
  98
  99         # Find a path to a possible `destination` or a node accepted by `alt_targets`
 100         fun path_to_alts(destination: nullable N, max_cost: Int, context: PathContext,
 101                 alt_targets: nullable TargetCondition[N]): nullable AStarPath[N]
 102         do
 103                 var cost = 0
 104
 105                 var nbr_buckets = context.worst_cost + context.worst_heuristic_cost + 1
 106                 var buckets = new Array[List[N]].with_capacity(nbr_buckets)
 107
 108                 for i in [0 .. nbr_buckets[ do
 109                         buckets.add(new List[N])
 110                 end
 111
 112                 graph.pathfinding_current_evocation += 1
 113
 114                 # open source node
 115                 buckets[0].add(self)
 116                 open = true
 117                 self.last_pathfinding_evocation = graph.pathfinding_current_evocation
 118                 self.best_cost_up_to = 0
 119
 120                 loop
 121                         var frontier_node: nullable N = null
 122
 123                         var bucket_searched = 0
 124
 125                         # find next valid node in frontier/buckets
 126                         loop
 127                                 var current_bucket = buckets[cost % nbr_buckets]
 128
 129                                 if current_bucket.is_empty then # move to next bucket
 130                                         cost += 1
 131                                         if cost > max_cost then return null
 132                                         bucket_searched += 1
 133
 134                                         if bucket_searched > nbr_buckets then break
 135                                 else # found a node
 136                                         frontier_node = current_bucket.pop
 137
 138                                         if frontier_node.open then break
 139                                 end
 140                         end
 141
 142                         # no path possible
 143                         if frontier_node == null then
 144                                 return null
 145
 146                         # at destination
 147                         else if frontier_node == destination or
 148                              (alt_targets != null and alt_targets.accept(frontier_node)) then
 149
 150                                 var path = new AStarPath[N](cost)
 151
 152                                 while frontier_node != self do
 153                                         path.nodes.unshift(frontier_node)
 154                                         frontier_node = frontier_node.best_source
 155                                         assert frontier_node != null
 156                                 end
 157
 158                                 return path
 159
 160                         # adds all next nodes to frontier/buckets
 161                         else
 162                                 frontier_node.open = false
 163
 164                                 for link in frontier_node.links do
 165                                         var peek_node = link.to
 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                                                 peek_node.open = true
 172                                                 peek_node.last_pathfinding_evocation = graph.pathfinding_current_evocation
 173                                                 peek_node.best_cost_up_to = cost + context.cost(link)
 174                                                 peek_node.best_source = frontier_node
 175
 176                                                 var est_cost
 177                                                 if destination != null then
 178                                                         est_cost = peek_node.best_cost_up_to + context.heuristic_cost(peek_node, destination)
 179                                                 else if alt_targets != null then
 180                                                         est_cost = peek_node.best_cost_up_to + alt_targets.heuristic_cost(peek_node, link)
 181                                                 else est_cost = 0
 182
 183                                                 var at_bucket = buckets[est_cost % nbr_buckets]
 184                                                 at_bucket.add(peek_node)
 185                                         end
 186                                 end
 187                         end
 188                 end
 189         end
 190
 191         # Find the closest node accepted by `cond` under `max_cost`
 192         fun find_closest(max_cost: Int, context: PathContext, cond: nullable TargetCondition[N]): nullable N
 193         do
 194                 var path = path_to_alts(null, max_cost, context, cond)
 195                 if path == null then return null
 196                 return path.nodes.last
 197         end
 198
 199         # We customize the serialization process to avoid problems with recursive
 200         # serialization engines. These engines, such as `JsonSerializer`,
 201         # are at danger to serialize the graph as a very deep tree.
 202         # With a large graph it can cause a stack overflow.
 203         #
 204         # Instead, we serialize the nodes first and then the links.
 205         redef fun core_serialize_to(serializer)
 206         do
 207                 serializer.serialize_attribute("graph", graph)
 208         end
 209
 210         redef init from_deserializer(deserializer)
 211         do
 212                 deserializer.notify_of_creation self
 213
 214                 var graph = deserializer.deserialize_attribute("graph")
 215                 assert graph isa Graph[N, Link]
 216                 self.graph = graph
 217         end
 218 end
 219
 220 # Link between two nodes and associated to a graph
 221 class Link
 222         serialize
 223
 224         # Type of the nodes in `graph`
 225         type N: Node
 226
 227         # Type of the other links in `graph`
 228         type L: Link
 229
 230         # The graph to which belongs `self`
 231         var graph: Graph[N, L]
 232
 233         # Origin of this link
 234         var from: N
 235
 236         # Endpoint of this link
 237         var to: N
 238
 239         init
 240         do
 241                 graph.add_link(self)
 242         end
 243 end
 244
 245 # General graph
 246 class Graph[N: Node, L: Link]
 247         super Serializable
 248
 249         # Nodes in this graph
 250         var nodes: Set[N] = new HashSet[N]
 251
 252         # Links in this graph
 253         var links: Set[L] = new HashSet[L]
 254
 255         # Add a `node` to this graph
 256         fun add_node(node: N): N
 257         do
 258                 nodes.add(node)
 259
 260                 return node
 261         end
 262
 263         # Add a `link` to this graph
 264         fun add_link(link: L): L
 265         do
 266                 links.add(link)
 267
 268                 link.from.links.add(link)
 269
 270                 return link
 271         end
 272
 273         # Used to check if nodes have been searched in one pathfinding
 274         private var pathfinding_current_evocation: Int = 0
 275
 276         redef fun core_serialize_to(serializer)
 277         do
 278                 serializer.serialize_attribute("nodes", nodes)
 279                 serializer.serialize_attribute("links", links)
 280         end
 281
 282         redef init from_deserializer(deserializer)
 283         do
 284                 deserializer.notify_of_creation self
 285
 286                 var nodes = deserializer.deserialize_attribute("nodes")
 287                 assert nodes isa HashSet[N]
 288                 self.nodes = nodes
 289
 290                 var links = deserializer.deserialize_attribute("links")
 291                 assert links isa HashSet[L]
 292                 for link in links do add_link link
 293         end
 294 end
 295
 296 # Result from path finding and a walkable path
 297 class AStarPath[N]
 298         serialize
 299
 300         # Total cost of this path
 301         var total_cost: Int
 302
 303         # Nodes composing this path
 304         var nodes = new List[N]
 305
 306         private var at: Int = 0
 307
 308         # Step on the path and get the next node to travel
 309         fun step: N
 310         do
 311                 assert nodes.length >= at else print "a_star::AStarPath::step failed, is at_end_of_path"
 312
 313                 var s = nodes[at]
 314                 at += 1
 315
 316                 return s
 317         end
 318
 319         # Peek at the next step of the path
 320         fun peek_step: N do return nodes[at]
 321
 322         # Are we at the end of this path?
 323         fun at_end_of_path: Bool do return at >= nodes.length
 324 end
 325
 326 # Context related to an evocation of pathfinding
 327 abstract class PathContext
 328         serialize
 329
 330         # Type of the nodes in `graph`
 331         type N: Node
 332
 333         # Type of the links in `graph`
 334         type L: Link
 335
 336         # Graph to which is associated `self`
 337         var graph: Graph[N, L]
 338
 339         # Worst cost of all the link's costs
 340         fun worst_cost: Int is abstract
 341
 342         # Get cost of a link
 343         fun cost(link: L): Int is abstract
 344
 345         # Is that link blocked?
 346         fun is_blocked(link: L): Bool is abstract
 347
 348         # Heuristic
 349         fun heuristic_cost(a, b: N): Int is abstract
 350
 351         # The worst cost suggested by the heuristic
 352         fun worst_heuristic_cost: Int is abstract
 353 end
 354
 355 #
 356 ### Additionnal classes, may be useful
 357 #
 358
 359 # Simple context with constant cost on each links
 360 # Warning: A* is not optimize for such a case
 361 class ConstantPathContext
 362         super PathContext
 363         serialize
 364
 365         redef fun worst_cost do return 1
 366         redef fun cost(l) do return 1
 367         redef fun is_blocked(l) do return false
 368         redef fun heuristic_cost(a, b) do return 0
 369         redef fun worst_heuristic_cost do return 0
 370 end
 371
 372 # A `PathContext` for graphs with `WeightedLink`
 373 class WeightedPathContext
 374         super PathContext
 375         serialize
 376
 377         redef type L: WeightedLink
 378
 379         init
 380         do
 381                 super
 382
 383                 var worst_cost = 0
 384                 for l in graph.links do
 385                         var cost = l.weight
 386                         if cost >= worst_cost then worst_cost = cost + 1
 387                 end
 388                 self.worst_cost = worst_cost
 389         end
 390
 391         redef var worst_cost is noinit
 392
 393         redef fun cost(l) do
 394                 return l.weight
 395         end
 396         redef fun is_blocked(l) do return false
 397         redef fun heuristic_cost(a, b) do return 0
 398         redef fun worst_heuristic_cost do return 0
 399 end
 400
 401 # A `Link` with a `weight`
 402 class WeightedLink
 403         super Link
 404         serialize
 405
 406         # The `weight`, or cost, of this link
 407         var weight: Int
 408 end
 409
 410 # Advanced path conditions with customizable accept states
 411 abstract class TargetCondition[N: Node]
 412         serialize
 413
 414         # Should the pathfinding accept `node` as a goal?
 415         fun accept(node: N): Bool is abstract
 416
 417         # Approximate cost from `node` to an accept state
 418         fun heuristic_cost(node: N, link: Link): Int is abstract
 419 end