]>
Commit | Line | Data |
---|---|---|
83c7162d XL |
1 | use std::collections::{ |
2 | HashMap, | |
3 | BinaryHeap, | |
4 | }; | |
5 | use std::collections::hash_map::Entry::{ | |
6 | Occupied, | |
7 | Vacant, | |
8 | }; | |
9 | ||
10 | use std::hash::Hash; | |
11 | ||
12 | use scored::MinScored; | |
13 | use super::visit::{ | |
14 | EdgeRef, | |
15 | GraphBase, | |
16 | IntoEdges, | |
17 | VisitMap, | |
18 | Visitable, | |
19 | }; | |
20 | ||
21 | use algo::Measure; | |
22 | ||
23 | /// [Generic] A* shortest path algorithm. | |
24 | /// | |
25 | /// Computes the shortest path from `start` to `finish`, including the total path cost. | |
26 | /// | |
27 | /// `finish` is implicitly given via the `is_goal` callback, which should return `true` if the | |
28 | /// given node is the finish node. | |
29 | /// | |
30 | /// The function `edge_cost` should return the cost for a particular edge. Edge costs must be | |
31 | /// non-negative. | |
32 | /// | |
33 | /// The function `estimate_cost` should return the estimated cost to the finish for a particular | |
34 | /// node. For the algorithm to find the actual shortest path, it should be admissible, meaning that | |
35 | /// it should never overestimate the actual cost to get to the nearest goal node. Estimate costs | |
36 | /// must also be non-negative. | |
37 | /// | |
38 | /// The graph should be `Visitable` and implement `IntoEdges`. | |
39 | /// | |
40 | /// ``` | |
41 | /// use petgraph::Graph; | |
42 | /// use petgraph::algo::astar; | |
43 | /// | |
44 | /// let mut g = Graph::new(); | |
45 | /// let a = g.add_node((0., 0.)); | |
46 | /// let b = g.add_node((2., 0.)); | |
47 | /// let c = g.add_node((1., 1.)); | |
48 | /// let d = g.add_node((0., 2.)); | |
49 | /// let e = g.add_node((3., 3.)); | |
50 | /// let f = g.add_node((4., 2.)); | |
51 | /// g.extend_with_edges(&[ | |
52 | /// (a, b, 2), | |
53 | /// (a, d, 4), | |
54 | /// (b, c, 1), | |
55 | /// (b, f, 7), | |
56 | /// (c, e, 5), | |
57 | /// (e, f, 1), | |
58 | /// (d, e, 1), | |
59 | /// ]); | |
60 | /// | |
61 | /// let path = astar(&g, a, |finish| finish == f, |e| *e.weight(), |_| 0); | |
62 | /// assert_eq!(path, Some((6, vec![a, d, e, f]))); | |
63 | /// ``` | |
64 | /// | |
65 | /// Returns the total cost + the path of subsequent `NodeId` from start to finish, if one was | |
66 | /// found. | |
67 | pub fn astar<G, F, H, K, IsGoal>(graph: G, start: G::NodeId, mut is_goal: IsGoal, | |
68 | mut edge_cost: F, mut estimate_cost: H) | |
69 | -> Option<(K, Vec<G::NodeId>)> | |
70 | where G: IntoEdges + Visitable, | |
71 | IsGoal: FnMut(G::NodeId) -> bool, | |
72 | G::NodeId: Eq + Hash, | |
73 | F: FnMut(G::EdgeRef) -> K, | |
74 | H: FnMut(G::NodeId) -> K, | |
75 | K: Measure + Copy, | |
76 | { | |
77 | let mut visited = graph.visit_map(); | |
78 | let mut visit_next = BinaryHeap::new(); | |
79 | let mut scores = HashMap::new(); | |
80 | let mut path_tracker = PathTracker::<G>::new(); | |
81 | ||
82 | let zero_score = K::default(); | |
83 | scores.insert(start, zero_score); | |
84 | visit_next.push(MinScored(estimate_cost(start), start)); | |
85 | ||
86 | while let Some(MinScored(_, node)) = visit_next.pop() { | |
87 | if is_goal(node) { | |
88 | let path = path_tracker.reconstruct_path_to(node); | |
89 | let cost = scores[&node]; | |
90 | return Some((cost, path)); | |
91 | } | |
92 | ||
93 | // Don't visit the same node several times, as the first time it was visited it was using | |
94 | // the shortest available path. | |
95 | if !visited.visit(node) { | |
96 | continue | |
97 | } | |
98 | ||
99 | // This lookup can be unwrapped without fear of panic since the node was necessarily scored | |
100 | // before adding him to `visit_next`. | |
101 | let node_score = scores[&node]; | |
102 | ||
103 | for edge in graph.edges(node) { | |
104 | let next = edge.target(); | |
105 | if visited.is_visited(&next) { | |
106 | continue | |
107 | } | |
108 | ||
109 | let mut next_score = node_score + edge_cost(edge); | |
110 | ||
111 | match scores.entry(next) { | |
112 | Occupied(ent) => { | |
113 | let old_score = *ent.get(); | |
114 | if next_score < old_score { | |
115 | *ent.into_mut() = next_score; | |
116 | path_tracker.set_predecessor(next, node); | |
117 | } else { | |
118 | next_score = old_score; | |
119 | } | |
120 | }, | |
121 | Vacant(ent) => { | |
122 | ent.insert(next_score); | |
123 | path_tracker.set_predecessor(next, node); | |
124 | } | |
125 | } | |
126 | ||
127 | let next_estimate_score = next_score + estimate_cost(next); | |
128 | visit_next.push(MinScored(next_estimate_score, next)); | |
129 | } | |
130 | } | |
131 | ||
132 | None | |
133 | } | |
134 | ||
135 | struct PathTracker<G> | |
136 | where G: GraphBase, | |
137 | G::NodeId: Eq + Hash, | |
138 | { | |
139 | came_from: HashMap<G::NodeId, G::NodeId>, | |
140 | } | |
141 | ||
142 | impl<G> PathTracker<G> | |
143 | where G: GraphBase, | |
144 | G::NodeId: Eq + Hash, | |
145 | { | |
146 | fn new() -> PathTracker<G> { | |
147 | PathTracker { | |
148 | came_from: HashMap::new(), | |
149 | } | |
150 | } | |
151 | ||
152 | fn set_predecessor(&mut self, node: G::NodeId, previous: G::NodeId) { | |
153 | self.came_from.insert(node, previous); | |
154 | } | |
155 | ||
156 | fn reconstruct_path_to(&self, last: G::NodeId) -> Vec<G::NodeId> { | |
157 | let mut path = vec![last]; | |
158 | ||
159 | let mut current = last; | |
b7449926 XL |
160 | while let Some(&previous) = self.came_from.get(¤t) { |
161 | path.push(previous); | |
162 | current = previous; | |
83c7162d XL |
163 | } |
164 | ||
165 | path.reverse(); | |
166 | ||
167 | path | |
168 | } | |
169 | } |