# 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"
+ sys.stderr.write "a_star debug: {msg}\n"
end
end
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
+ # 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: N): Int do return 1
-
# Main functionnality, returns path from `self` to `dest`
- fun path_to(dest: Node, max_cost: Int, context: PathContext): nullable Path[N]
+ fun path_to(dest: N, max_cost: Int, context: PathContext): nullable Path[N]
do
- var cost: Int = 0
+ var cost = 0
- var nbr_buckets = context.worst_cost + 1 # graph.max_heuristic_cost
- var buckets = new Array[List[Node]].with_capacity(nbr_buckets)
+ 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
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
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
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]
+ var est_cost = peek_node.best_cost_up_to+context.heuristic_cost(link.from, peek_node)
+ var at_bucket = buckets[est_cost % nbr_buckets]
at_bucket.add(peek_node)
- 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)}"
+ debug "u putting {peek_node} at {est_cost} -> {est_cost % nbr_buckets} {at_bucket.hash}, {cost}+{context.cost(link)}"
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 N !with(n: N): Bool
- do
- if with(self) then return self
-
- var frontier = new List[N]
- 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
+# Link between two nodes and associated to a graph
class Link
type N: Node
type L: Link
fun worst_cost: Int is abstract
# Get cost of a link
- fun cost(link: Link): Int is abstract
+ fun cost(link: L): Int is abstract
# Is that link blocked?
- fun is_blocked(link: Link): Bool is abstract
+ fun is_blocked(link: L): Bool is abstract
# Heuristic
- fun heuristic_cost(a, b: Node): Int is abstract
+ fun heuristic_cost(a, b: N): Int is abstract
fun worst_heuristic_cost: Int is abstract
end
redef var worst_cost: Int
redef fun cost(l) do
- assert l isa L
return l.weight
end
redef fun is_blocked(l) do return false
nitlanguage / nit.git / blobdiff