1 # This file is part of NIT (http://www.nitlanguage.org).
3 # Copyright 2011-2013 Alexis Laferrière <alexis.laf@xymus.net>
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
9 # http://www.apache.org/licenses/LICENSE-2.0
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.
17 # Services related to pathfinding of graphs using A*
18 # A single graph may have different properties according to the `PathContext` used
24 # # Weighted graph (letters are nodes, digits are weights):
32 # var graph = new Graph[Node,WeightedLink]
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)
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)
46 # var context = new WeightedPathContext(graph)
48 # var path = na.path_to(ne, 100, context)
49 # assert path != null else print "No possible path"
51 # assert path.step == nb
52 # assert path.step == nd
53 # assert path.step == ne
54 # assert path.at_end_of_path
59 protected fun debug_a_star
: Bool do return false
60 private fun debug
(msg
: String) do if debug_a_star
then
61 sys
.stderr
.write
"a_star debug: {msg}\n"
70 var graph
: Graph[N
, Link]
72 init(graph
: Graph[N
, Link])
79 var links
: Set[Link] = new HashSet[Link]
81 # used to check if node has been searched in one pathfinding
82 private var last_pathfinding_evocation
: Int = 0
84 # cost up to in current evocation
85 # lifetime limited to evocation of `path_to`
86 private var best_cost_up_to
: Int = 0
89 # lifetime limited to evocation of `path_to`
90 private var best_source
: nullable N
= null
92 # is in frontier or buckets
93 # lifetime limited to evocation of `path_to`
94 private var open
: Bool = false
96 # Main functionnality, returns path from `self` to `dest`
97 fun path_to
(dest
: N
, max_cost
: Int, context
: PathContext): nullable Path[N
]
99 return path_to_alts
(dest
, max_cost
, context
, null)
102 # Find a path to a possible `destination` or a node accepted by `alt_targets`
103 fun path_to_alts
(destination
: nullable N
, max_cost
: Int, context
: PathContext,
104 alt_targets
: nullable TargetCondition[N
]): nullable Path[N
]
108 var nbr_buckets
= context
.worst_cost
+ context
.worst_heuristic_cost
+ 1
109 var buckets
= new Array[List[N
]].with_capacity
(nbr_buckets
)
111 for i
in [0 .. nbr_buckets
[ do
112 buckets
.add
(new List[N
])
115 graph
.pathfinding_current_evocation
+= 1
120 self.last_pathfinding_evocation
= graph
.pathfinding_current_evocation
121 self.best_cost_up_to
= 0
124 var frontier_node
: nullable N
= null
126 var bucket_searched
: Int = 0
128 # find next valid node in frontier/buckets
130 var current_bucket
= buckets
[cost
% nbr_buckets
]
132 if current_bucket
.is_empty
then # move to next bucket
133 debug
"b {cost} {cost % nbr_buckets} {buckets[cost % nbr_buckets].hash}"
135 if cost
> max_cost
then return null
138 if bucket_searched
> nbr_buckets
then break
141 frontier_node
= current_bucket
.pop
143 if frontier_node
.open
then break
148 if frontier_node
== null then
152 else if frontier_node
== destination
or
153 (alt_targets
!= null and alt_targets
.accept
(frontier_node
)) then
154 debug
"picked {frontier_node}, is destination"
156 var path
= new Path[N
](cost
)
158 while frontier_node
!= self do
159 path
.nodes
.unshift
(frontier_node
)
160 frontier_node
= frontier_node
.best_source
.as(not null)
165 # adds all next nodes to frontier/buckets
167 frontier_node
.open
= false
169 debug
"w exploring adjacents of {frontier_node}"
171 for link
in frontier_node
.links
do
172 var peek_node
= link
.to
173 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)}"
174 if not context
.is_blocked
(link
) and
175 (peek_node
.last_pathfinding_evocation
!= graph
.pathfinding_current_evocation
or
177 peek_node
.best_cost_up_to
> cost
+ context
.cost
(link
)))
179 peek_node
.open
= true
180 peek_node
.last_pathfinding_evocation
= graph
.pathfinding_current_evocation
181 peek_node
.best_cost_up_to
= cost
+ context
.cost
(link
)
182 peek_node
.best_source
= frontier_node
185 if destination
!= null then
186 est_cost
= peek_node
.best_cost_up_to
+ context
.heuristic_cost
(peek_node
, destination
)
187 else if alt_targets
!= null then
188 est_cost
= peek_node
.best_cost_up_to
+ alt_targets
.heuristic_cost
(peek_node
, link
)
191 var at_bucket
= buckets
[est_cost
% nbr_buckets
]
192 at_bucket
.add
(peek_node
)
194 debug
"u putting {peek_node} at {est_cost} -> {est_cost % nbr_buckets} {at_bucket.hash}, {cost}+{context.cost(link)}"
202 # Link between two nodes and associated to a graph
207 var graph
: Graph[N
, L
]
212 init(graph
: Graph[N
, L
], from
, to
: N
)
223 class Graph[N
: Node, L
: Link]
224 var nodes
: Set[N
] = new HashSet[N
]
225 var links
: Set[L
] = new HashSet[L
]
227 fun add_node
(node
: N
): N
234 fun add_link
(link
: L
): L
238 link
.from
.links
.add
(link
)
243 # used to check if nodes have been searched in one pathfinding
244 var pathfinding_current_evocation
: Int = 0
247 # Result from pathfinding, a walkable path
252 var nodes
= new List[N
]
254 init (cost
: Int) do total_cost
= cost
259 assert nodes
.length
>= at
else print
"a_star::Path::step failed, is at_end_of_path"
267 fun peek_step
: N
do return nodes
[at
]
269 fun at_end_of_path
: Bool do return at
>= nodes
.length
272 # Context related to an evocation of pathfinding
277 var graph
: Graph[N
, L
]
279 # Worst cost of all the link's costs
280 fun worst_cost
: Int is abstract
283 fun cost
(link
: L
): Int is abstract
285 # Is that link blocked?
286 fun is_blocked
(link
: L
): Bool is abstract
289 fun heuristic_cost
(a
, b
: N
): Int is abstract
291 fun worst_heuristic_cost
: Int is abstract
295 ### Additionnal classes, may be useful
298 # Simple context with constant cost on each links
299 # Warning: A* is not optimize for such a case
300 class ConstantPathContext
303 redef fun worst_cost
do return 1
304 redef fun cost
(l
) do return 1
305 redef fun is_blocked
(l
) do return false
306 redef fun heuristic_cost
(a
, b
) do return 0
307 redef fun worst_heuristic_cost
do return 0
310 class WeightedPathContext
313 redef type L
: WeightedLink
315 init(graph
: Graph[N
, L
])
320 for l
in graph
.links
do
322 if cost
>= worst_cost
then worst_cost
= cost
+ 1
324 self.worst_cost
= worst_cost
327 redef var worst_cost
: Int
332 redef fun is_blocked
(l
) do return false
333 redef fun heuristic_cost
(a
, b
) do return 0
334 redef fun worst_heuristic_cost
do return 0
342 init(graph
: Graph[N
, L
], from
, to
: N
, weight
: Int)
350 # Advanced path conditions with customizable accept states
351 class TargetCondition[N
: Node]
352 # Should the pathfinding accept `node` as a goal?
353 fun accept
(node
: N
): Bool is abstract
355 # Approximate cost from `node` to an accept state
356 fun heuristic_cost
(node
: N
, link
: Link): Int is abstract