X-Git-Url: http://nitlanguage.org diff --git a/lib/a_star.nit b/lib/a_star.nit index a110f03..2cbd7cf 100644 --- a/lib/a_star.nit +++ b/lib/a_star.nit @@ -17,93 +17,92 @@ # Services related to pathfinding of graphs using A* # A single graph may have different properties according to the `PathContext` used # +# # Usage: # -# # Weighted graph (letters are nodes, digits are weights): -# # -# # a -2- b -# # / / -# # 3 1 -# # / / -# # c -3- d -8- e -# # -# var graph = new Graph[Node,WeigthedLink[Node]] +# ~~~ +# # Weighted graph (letters are nodes, digits are weights): +# # +# # a -2- b +# # / / +# # 3 1 +# # / / +# # c -3- d -8- e +# # +# var graph = new Graph[Node,WeightedLink] # -# var na = new Node(graph) -# var nb = new Node(graph) -# var nc = new Node(graph) -# var nd = new Node(graph) -# var ne = new Node(graph) +# var na = new Node(graph) +# var nb = new Node(graph) +# var nc = new Node(graph) +# var nd = new Node(graph) +# var ne = new Node(graph) # -# var lab = new WeigthedLink[Node](graph, na, nb, 2) -# var lac = new WeigthedLink[Node](graph, na, nc, 3) -# var lbd = new WeigthedLink[Node](graph, nb, nd, 1) -# var lcd = new WeigthedLink[Node](graph, nc, nd, 3) -# var lde = new WeigthedLink[Node](graph, nd, ne, 8) +# var lab = new WeightedLink(graph, na, nb, 2) +# var lac = new WeightedLink(graph, na, nc, 3) +# var lbd = new WeightedLink(graph, nb, nd, 1) +# var lcd = new WeightedLink(graph, nc, nd, 3) +# var lde = new WeightedLink(graph, nd, ne, 8) # -# var context = new WeightedPathContext[Node, WeigthedLink[Node]](graph) +# var context = new WeightedPathContext(graph) # -# var path = na.path_to(ne, 100, context) -# assert path != null else print "No possible path" +# var path = na.path_to(ne, 100, context) +# assert path != null else print "No possible path" # -# while not path.at_end_of_path do -# print path.step -# end +# assert path.step == nb +# assert path.step == nd +# assert path.step == ne +# assert path.at_end_of_path +# ~~~ module a_star -redef class Object - protected fun debug_a_star: Bool do return false - private fun debug(msg: String) do if debug_a_star then - stderr.write "a_star debug: {msg}\n" - end -end - # General graph node class Node - type E: Node + # Type of the others nodes in the `graph` + type N: Node # parent graph - var graph: Graph[E, Link[E]] + var graph: Graph[N, Link] - init(graph: Graph[E, Link[E]]) + init do - self.graph = graph graph.add_node(self) end # adjacent nodes - var links: Set[Link[E]] = new HashSet[Link[E]] + var links: Set[Link] = new HashSet[Link] # used to check if node has been searched in one pathfinding private var last_pathfinding_evocation: Int = 0 # cost up to in current evocation - # lifetime limited to evocation of path_to + # lifetime limited to evocation of `path_to` private var best_cost_up_to: Int = 0 # source node - # lifetime limited to evocation of path_to - private var best_source: nullable E = null + # lifetime limited to evocation of `path_to` + private var best_source: nullable N = null # is in frontier or buckets - # lifetime limited to evocation of path_to + # lifetime limited to evocation of `path_to` private var open: Bool = false - # Heuristic, to redef - protected fun cost_to(other: E): Int do return 1 - # Main functionnality, returns path from `self` to `dest` - fun path_to(dest: Node, max_cost: Int, context: PathContext[E, Link[E]]): nullable Path[E] + fun path_to(dest: N, max_cost: Int, context: PathContext): nullable AStarPath[N] do - var cost: Int = 0 + return path_to_alts(dest, max_cost, context, null) + end - var nbr_buckets = context.worst_cost + 1 # graph.max_heuristic_cost - var buckets = new Array[List[Node]].with_capacity(nbr_buckets) + # Find a path to a possible `destination` or a node accepted by `alt_targets` + fun path_to_alts(destination: nullable N, max_cost: Int, context: PathContext, + alt_targets: nullable TargetCondition[N]): nullable AStarPath[N] + do + var cost = 0 + + var nbr_buckets = context.worst_cost + context.worst_heuristic_cost + 1 + var buckets = new Array[List[N]].with_capacity(nbr_buckets) - for i in [0 .. nbr_buckets [ do - var l = new List[Node] - buckets.add(l) - #print l.hash + for i in [0 .. nbr_buckets[ do + buckets.add(new List[N]) end graph.pathfinding_current_evocation += 1 @@ -114,23 +113,22 @@ class Node self.last_pathfinding_evocation = graph.pathfinding_current_evocation self.best_cost_up_to = 0 - while cost < max_cost do - var frontier_node: nullable Node = null + loop + var frontier_node: nullable N = null - var bucket_searched: Int = 0 + var bucket_searched = 0 # find next valid node in frontier/buckets loop var current_bucket = buckets[cost % nbr_buckets] if current_bucket.is_empty then # move to next bucket - debug "b {cost} {cost % nbr_buckets} {buckets[cost % nbr_buckets].hash}" cost += 1 + if cost > max_cost then return null bucket_searched += 1 if bucket_searched > nbr_buckets then break else # found a node - debug "c {cost}" frontier_node = current_bucket.pop if frontier_node.open then break @@ -142,10 +140,10 @@ class Node return null # at destination - else if frontier_node == dest then - debug "picked {frontier_node}, is destination" + else if frontier_node == destination or + (alt_targets != null and alt_targets.accept(frontier_node)) then - var path = new Path[E](cost) + var path = new AStarPath[N](cost) while frontier_node != self do path.nodes.unshift(frontier_node) @@ -158,96 +156,66 @@ class Node else frontier_node.open = false - debug "w exploring adjacents of {frontier_node}" - for link in frontier_node.links do var peek_node = link.to - 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)}" if not context.is_blocked(link) and (peek_node.last_pathfinding_evocation != graph.pathfinding_current_evocation or (peek_node.open and peek_node.best_cost_up_to > cost + context.cost(link))) then - peek_node.open = true peek_node.last_pathfinding_evocation = graph.pathfinding_current_evocation peek_node.best_cost_up_to = cost + context.cost(link) peek_node.best_source = frontier_node - var at_bucket = buckets[(peek_node.best_cost_up_to+peek_node.cost_to(dest)) % nbr_buckets] - at_bucket.add(peek_node) + var est_cost + if destination != null then + est_cost = peek_node.best_cost_up_to + context.heuristic_cost(peek_node, destination) + else if alt_targets != null then + est_cost = peek_node.best_cost_up_to + alt_targets.heuristic_cost(peek_node, link) + else est_cost = 0 - 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)}" + var at_bucket = buckets[est_cost % nbr_buckets] + at_bucket.add(peek_node) end end end end - - # costs over max - return null - end - - # Find closes node with matching caracteristic - # TODO remove closures - fun find_closest(max_to_search: Int): nullable E !with(n: E): Bool - do - if with(self) then return self - - var frontier = new List[E] - graph.pathfinding_current_evocation += 1 - var current_evocation = graph.pathfinding_current_evocation - - frontier.add(self) - self.last_pathfinding_evocation = current_evocation - - var i = 0 - while not frontier.is_empty do - var node = frontier.shift - - for link in node.links do - var to = link.to - if to.last_pathfinding_evocation != current_evocation then - if with(to) then return to - - frontier.add(to) - to.last_pathfinding_evocation = current_evocation - end - end - - i += 1 - if i > max_to_search then return null - end - - return null end end -class Link[N:Node] - type L: Link[N] +# Link between two nodes and associated to a graph +class Link + # Type of the nodes in `graph` + type N: Node + # Type of the other links in `graph` + type L: Link + + # The graph to which belongs `self` var graph: Graph[N, L] + # Origin of this link var from: N + + # Endpoint of this link var to: N - init(graph: Graph[N, L], from, to: N) + init do - self.graph = graph - self.from = from - self.to = to - graph.add_link(self) end end # General graph -class Graph[N:Node, L:Link[N]] +class Graph[N: Node, L: Link] + # Nodes in this graph var nodes: Set[N] = new HashSet[N] - var links: Set[L] = new HashSet[L] - #var max_link_cost: Int = 0 - #var max_heuristic_cost: Int = 0 + # Links in this graph + var links: Set[L] = new HashSet[L] + # Add a `node` to this graph fun add_node(node: N): N do nodes.add(node) @@ -255,34 +223,35 @@ class Graph[N:Node, L:Link[N]] return node end + # Add a `link` to this graph fun add_link(link: L): L do links.add(link) - #if link.cost > max_link_cost then max_link_cost = link.cost - link.from.links.add(link) return link end - # used to check if nodes have been searched in one pathfinding - var pathfinding_current_evocation: Int = 0 + # Used to check if nodes have been searched in one pathfinding + private var pathfinding_current_evocation: Int = 0 end -# Result from pathfinding, a walkable path -class Path[N] +# Result from path finding and a walkable path +class AStarPath[N] + # The total cost of this path var total_cost: Int + # The list of nodes composing this path var nodes = new List[N] - init (cost: Int) do total_cost = cost + private var at: Int = 0 - var at: Int = 0 + # Step on the path and get the next node to travel fun step: N do - assert nodes.length >= at else print "a_star::Path::step failed, is at_end_of_path" + assert nodes.length >= at else print "a_star::AStarPath::step failed, is at_end_of_path" var s = nodes[at] at += 1 @@ -290,28 +259,39 @@ class Path[N] return s end + # Peek at the next step of the path fun peek_step: N do return nodes[at] + # Are we at the end of this path? fun at_end_of_path: Bool do return at >= nodes.length end # Context related to an evocation of pathfinding -class PathContext[N: Node, L: Link[N]] +class PathContext + # Type of the nodes in `graph` + type N: Node + + # Type of the links in `graph` + type L: Link + + # Graph to which is associated `self` var graph: Graph[N, L] # Worst cost of all the link's costs fun worst_cost: Int is abstract # Get cost of a link - fun cost(link: Link[N]): Int is abstract + fun cost(link: L): Int is abstract # Is that link blocked? - fun is_blocked(link: Link[N]): Bool is abstract + fun is_blocked(link: L): Bool is abstract # Heuristic - fun heuristic_cost(a, b: Node): Int is abstract -end + fun heuristic_cost(a, b: N): Int is abstract + # The worst cost suggested by the heuristic + fun worst_heuristic_cost: Int is abstract +end # ### Additionnal classes, may be useful @@ -319,49 +299,57 @@ end # Simple context with constant cost on each links # Warning: A* is not optimize for such a case -class ConstantPathContext[N: Node, L: Link[N]] - super PathContext[N, L] +class ConstantPathContext + super PathContext redef fun worst_cost do return 1 redef fun cost(l) do return 1 redef fun is_blocked(l) do return false - redef fun heuristic_cost(a, b) do return 1 # TODO + redef fun heuristic_cost(a, b) do return 0 + redef fun worst_heuristic_cost do return 0 end -class WeightedPathContext[N: Node, L: WeigthedLink[N]] - super PathContext[N,L] +# A `PathContext` for graphs with `WeightedLink` +class WeightedPathContext + super PathContext - init(graph: Graph[N,L]) + redef type L: WeightedLink + + init do super var worst_cost = 0 for l in graph.links do var cost = l.weight - if cost > worst_cost then worst_cost = cost + if cost >= worst_cost then worst_cost = cost + 1 end self.worst_cost = worst_cost end - redef var worst_cost: Int + redef var worst_cost: Int is noinit redef fun cost(l) do - assert l isa L return l.weight end redef fun is_blocked(l) do return false - redef fun heuristic_cost(a, b) do return 10 # TODO + redef fun heuristic_cost(a, b) do return 0 + redef fun worst_heuristic_cost do return 0 end -class WeigthedLink[N: Node] - super Link[N] +# A `Link` with a `weight` +class WeightedLink + super Link + # The `weight`, or cost, of this link var weight: Int +end - init(graph: Graph[N,L], from, to: N, weight: Int) - do - super +# Advanced path conditions with customizable accept states +class TargetCondition[N: Node] + # Should the pathfinding accept `node` as a goal? + fun accept(node: N): Bool is abstract - self.weight = weight - end + # Approximate cost from `node` to an accept state + fun heuristic_cost(node: N, link: Link): Int is abstract end