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.
astar<G, F, H, K, IsGoal>(graph: G, start: G::NodeId, mut is_goal: IsGoal, mut edge_cost: F, mut estimate_cost: H) -> Option<(K, Vec<G::NodeId>)> where G: IntoEdges + Visitable, IsGoal: FnMut(G::NodeId) -> bool, G::NodeId: Eq + Hash, F: FnMut(G::EdgeRef) -> K, H: FnMut(G::NodeId) -> K, K: Measure + Copy,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 {
new() -> PathTracker<G>146 fn new() -> PathTracker<G> {
147 PathTracker {
148 came_from: HashMap::new(),
149 }
150 }
151
set_predecessor(&mut self, node: G::NodeId, previous: G::NodeId)152 fn set_predecessor(&mut self, node: G::NodeId, previous: G::NodeId) {
153 self.came_from.insert(node, previous);
154 }
155
reconstruct_path_to(&self, last: G::NodeId) -> Vec<G::NodeId>156 fn reconstruct_path_to(&self, last: G::NodeId) -> Vec<G::NodeId> {
157 let mut path = vec![last];
158
159 let mut current = last;
160 while let Some(&previous) = self.came_from.get(¤t) {
161 path.push(previous);
162 current = previous;
163 }
164
165 path.reverse();
166
167 path
168 }
169 }
170