fix(astar_bag): do not hang on graphs with a zero-cost cycle

src/directed/astar.rs

@@ -189,6 +189,10 @@ where
     // A node has only as many optimal parents as it has incoming edges, and a goal is reached
     // only once, so plain vectors are both cheaper to fill and cheaper to walk than hash sets.
     let mut sinks: Vec<usize> = Vec::new();
+    // Costs are non-negative, so a cycle among optimal parents needs every edge on it to cost
+    // nothing. If no such edge is ever seen, the solution walk cannot loop and does not need to
+    // guard against it.
+    let mut zero_cost_edge = false;
     to_see.push(SmallestCostHolder {
         estimated_cost: Zero::zero(),
         cost: Zero::zero(),
@@ -223,6 +227,7 @@ where
             successors(node)
         };
         for (successor, move_cost) in successors {
+            zero_cost_edge |= move_cost.is_zero();
             let new_cost = cost + move_cost;
             let h; // heuristic(&successor)
             let n; // index for successor
@@ -267,6 +272,9 @@ where
         (
             AstarSolution {
                 sinks,
+                // The start is inserted into `parents` before anything else.
+                start: 0,
+                may_loop: zero_cost_edge,
                 parents,
                 current: vec![],
                 terminated: false,
@@ -346,17 +354,55 @@ impl<K: Ord> Ord for SmallestCostHolder<K> {
 #[derive(Clone)]
 pub struct AstarSolution<N> {
     sinks: Vec<usize>,
+    /// Index of the start vertex, which is where every path ends when walked backwards.
+    start: usize,
+    /// Whether any edge costing nothing was relaxed, which is what makes a vertex able to be
+    /// its own optimal parent. Without one, the walk back cannot loop.
+    may_loop: bool,
     parents: Vec<(N, Vec<usize>)>,
     current: Vec<Vec<usize>>,
     terminated: bool,
 }
 
 impl<N: Clone + Eq + Hash> AstarSolution<N> {
-    fn complete(&mut self) {
+    /// Extend the partial path backwards until it reaches the start.
+    ///
+    /// Returns `false` if the choices made so far cannot be extended to the start, in which case
+    /// the caller has to backtrack and try the next alternative.
+    ///
+    /// Two things make this more than a walk up the parent links. A parent already on the path
+    /// being built is skipped, because an edge costing nothing makes a vertex an optimal parent
+    /// of itself, or of a vertex it forms a zero-cost cycle with, and following those never
+    /// ends. And the walk stops at the start vertex rather than at a vertex without parents,
+    /// because a zero-cost cycle through the start gives the start parents of its own.
+    /// Extend the partial path backwards until it reaches the start.
+    ///
+    /// Returns `false` if the choices made so far cannot be extended to the start, in which case
+    /// the caller has to backtrack and try the next alternative.
+    fn complete(&mut self) -> bool {
+        if self.may_loop {
+            self.complete_without_looping()
+        } else {
+            self.complete_directly();
+            true
+        }
+    }
+
+    /// The common case, where no edge costs nothing.
+    ///
+    /// A cycle among optimal parents needs every edge on it to cost nothing, so here the walk
+    /// back cannot loop and every vertex can simply follow its parents to the start.
+    fn complete_directly(&mut self) {
         loop {
             let ps = match self.current.last() {
                 None => self.sinks.clone(),
-                Some(last) => self.parents(*last.last().unwrap()).clone(),
+                Some(last) => {
+                    let tail = *last.last().unwrap();
+                    if tail == self.start {
+                        break;
+                    }
+                    self.parents(tail).clone()
+                }
             };
             if ps.is_empty() {
                 break;
@@ -365,6 +411,42 @@ impl<N: Clone + Eq + Hash> AstarSolution<N> {
         }
     }
 
+    /// The case where some edge costs nothing.
+    ///
+    /// Such an edge makes a vertex an optimal parent of itself, or of a vertex it forms a
+    /// zero-cost cycle with, so parents already on the path being built have to be skipped or
+    /// the walk never ends. Doing that can leave a vertex with nowhere to go, which is a dead
+    /// end for this combination of choices rather than a result.
+    fn complete_without_looping(&mut self) -> bool {
+        loop {
+            let ps = match self.current.last() {
+                None => self.sinks.clone(),
+                Some(last) => {
+                    let tail = *last.last().unwrap();
+                    if tail == self.start {
+                        return true;
+                    }
+                    self.parents(tail)
+                        .iter()
+                        .copied()
+                        .filter(|p| !self.chosen().any(|c| c == *p))
+                        .collect::<Vec<_>>()
+                }
+            };
+            if ps.is_empty() {
+                return false;
+            }
+            self.current.push(ps);
+        }
+    }
+
+    /// The vertices picked so far, one per level, from the goal backwards.
+    fn chosen(&self) -> impl Iterator<Item = usize> + '_ {
+        self.current
+            .iter()
+            .filter_map(|level| level.last().copied())
+    }
+
     fn next_vec(&mut self) {
         while self.current.pop_if(|v| v.len() == 1).is_some() {}
         self.current.last_mut().map(Vec::pop);
@@ -383,20 +465,28 @@ impl<N: Clone + Eq + Hash> Iterator for AstarSolution<N> {
     type Item = Vec<N>;
 
     fn next(&mut self) -> Option<Self::Item> {
-        if self.terminated {
-            return None;
+        loop {
+            if self.terminated {
+                return None;
+            }
+            if !self.complete() {
+                // This combination of choices cannot reach the start. Step to the next one and
+                // try again, rather than reporting a path that stops short.
+                self.next_vec();
+                self.terminated = self.current.is_empty();
+                continue;
+            }
+            let path = self
+                .current
+                .iter()
+                .rev()
+                .map(|v| v.last().copied().unwrap())
+                .map(|i| self.node(i).clone())
+                .collect::<Vec<_>>();
+            self.next_vec();
+            self.terminated = self.current.is_empty();
+            return Some(path);
         }
-        self.complete();
-        let path = self
-            .current
-            .iter()
-            .rev()
-            .map(|v| v.last().copied().unwrap())
-            .map(|i| self.node(i).clone())
-            .collect::<Vec<_>>();
-        self.next_vec();
-        self.terminated = self.current.is_empty();
-        Some(path)
     }
 }
 

tests/astar_bag_zero_cost.rs

@@ -0,0 +1,186 @@
+//! `astar_bag` used to walk back through optimal parents until it found one with no parents.
+//! An edge costing nothing makes a node an optimal parent of itself, or of a node it forms a
+//! zero-cost cycle with, so that walk never ended and the first solution never arrived.
+
+use pathfinding::prelude::{astar, astar_bag};
+
+/// The smallest failing case: a single zero-cost self loop.
+#[test]
+fn a_zero_cost_self_loop_still_yields_the_path() {
+    let succ = |&n: &u32| match n {
+        0 => vec![(1, 1u32)],
+        1 => vec![(1, 0), (2, 1)],
+        _ => vec![],
+    };
+    let (solutions, cost) = astar_bag(&0, succ, |_| 0, |&n| n == 2).expect("a path exists");
+    assert_eq!(cost, 2);
+    assert_eq!(solutions.collect::<Vec<_>>(), vec![vec![0, 1, 2]]);
+}
+
+#[test]
+fn a_zero_cost_cycle_still_yields_the_path() {
+    let succ = |&n: &u32| match n {
+        0 => vec![(1, 1u32)],
+        1 => vec![(2, 1), (3, 0)],
+        3 => vec![(1, 0)],
+        _ => vec![],
+    };
+    let (solutions, cost) = astar_bag(&0, succ, |_| 0, |&n| n == 2).expect("a path exists");
+    assert_eq!(cost, 2);
+    assert_eq!(solutions.collect::<Vec<_>>(), vec![vec![0, 1, 2]]);
+}
+
+/// Zero-cost edges that do not form a cycle were never a problem, and must keep working.
+#[test]
+fn zero_cost_edges_without_a_cycle_are_unaffected() {
+    let succ = |&n: &u32| match n {
+        0 => vec![(1, 0u32), (2, 0)],
+        1 | 2 => vec![(3, 1)],
+        _ => vec![],
+    };
+    let (solutions, cost) = astar_bag(&0, succ, |_| 0, |&n| n == 3).expect("a path exists");
+    assert_eq!(cost, 1);
+    let mut found = solutions.collect::<Vec<_>>();
+    found.sort_unstable();
+    assert_eq!(found, vec![vec![0, 1, 3], vec![0, 2, 3]]);
+}
+
+struct Rng(u64);
+
+impl Rng {
+    const fn next(&mut self) -> u64 {
+        self.0 = self
+            .0
+            .wrapping_mul(6_364_136_223_846_793_005)
+            .wrapping_add(1_442_695_040_888_963_407);
+        self.0 >> 33
+    }
+
+    fn below(&mut self, n: usize) -> usize {
+        let n = u64::try_from(n).expect("orders used here are small");
+        usize::try_from(self.next() % n).expect("a value below n fits in a usize")
+    }
+
+    fn cost(&mut self) -> u32 {
+        u32::try_from(self.next() % 4).expect("a value below 4 fits in a u32")
+    }
+}
+
+/// Random graphs, deliberately including zero-cost edges, parallel edges and self loops.
+fn random_graph(order: usize, rng: &mut Rng) -> Vec<Vec<(usize, u32)>> {
+    let mut out = vec![Vec::new(); order];
+    for edges in &mut out {
+        for _ in 0..rng.below(3) {
+            edges.push((rng.below(order), rng.cost()));
+        }
+    }
+    out
+}
+
+/// Every distinct simple path from `start` to `goal` of minimum cost, found by brute force.
+fn shortest_simple_paths(
+    graph: &[Vec<(usize, u32)>],
+    start: usize,
+    goal: usize,
+) -> (Option<u32>, Vec<Vec<usize>>) {
+    let mut best: Option<u32> = None;
+    let mut paths: Vec<Vec<usize>> = Vec::new();
+    let mut stack = vec![(vec![start], 0u32)];
+    while let Some((path, cost)) = stack.pop() {
+        let last = *path.last().expect("paths are never empty");
+        if last == goal {
+            match best {
+                Some(b) if cost > b => {}
+                Some(b) if cost == b => paths.push(path),
+                _ => {
+                    best = Some(cost);
+                    paths = vec![path];
+                }
+            }
+            continue;
+        }
+        for &(next, edge) in &graph[last] {
+            if !path.contains(&next) {
+                let mut extended = path.clone();
+                extended.push(next);
+                stack.push((extended, cost + edge));
+            }
+        }
+    }
+    paths.sort_unstable();
+    paths.dedup();
+    (best, paths)
+}
+
+#[test]
+fn agrees_with_brute_force_on_random_graphs_with_zero_costs() {
+    let mut rng = Rng(0xABCD_0007);
+    let mut checked = 0;
+    for trial in 0..400 {
+        let order = 2 + rng.below(7);
+        let graph = random_graph(order, &mut rng);
+        let (start, goal) = (rng.below(order), rng.below(order));
+        if start == goal {
+            continue;
+        }
+        checked += 1;
+
+        let (want_cost, want_paths) = shortest_simple_paths(&graph, start, goal);
+        let got = astar_bag(&start, |&u| graph[u].clone(), |_| 0, |&u| u == goal);
+
+        match (want_cost, got) {
+            (None, None) => {}
+            (Some(want), Some((solutions, cost))) => {
+                assert_eq!(cost, want, "trial {trial}: wrong cost");
+                let mut found = solutions.collect::<Vec<_>>();
+                let before = found.len();
+                found.sort_unstable();
+                found.dedup();
+                assert_eq!(found.len(), before, "trial {trial}: duplicate solutions");
+                assert_eq!(
+                    found, want_paths,
+                    "trial {trial}: wrong set of shortest paths from {start} to {goal}"
+                );
+            }
+            (want, got) => panic!(
+                "trial {trial}: brute force {want:?} but astar_bag {:?}",
+                got.map(|(_, c)| c)
+            ),
+        }
+    }
+    // start == goal is skipped, and with graphs this small it comes up often.
+    assert!(
+        checked > 250,
+        "expected most trials to be usable, only {checked} were"
+    );
+}
+
+/// Whatever `astar_bag` reports must match what `astar` reports on the same graph.
+#[test]
+fn agrees_with_astar_on_random_graphs_with_zero_costs() {
+    let mut rng = Rng(0x5EED_9001);
+    for trial in 0..400 {
+        let order = 2 + rng.below(10);
+        let graph = random_graph(order, &mut rng);
+        let (start, goal) = (rng.below(order), rng.below(order));
+
+        let single = astar(&start, |&u| graph[u].clone(), |_| 0, |&u| u == goal);
+        let bag = astar_bag(&start, |&u| graph[u].clone(), |_| 0, |&u| u == goal);
+
+        match (single, bag) {
+            (None, None) => {}
+            (Some((_, c1)), Some((solutions, c2))) => {
+                assert_eq!(c1, c2, "trial {trial}: astar {c1} but astar_bag {c2}");
+                assert!(
+                    solutions.take(1).count() == 1,
+                    "trial {trial}: astar_bag produced no solution despite reporting a cost"
+                );
+            }
+            (a, b) => panic!(
+                "trial {trial}: astar {:?} but astar_bag {:?}",
+                a.map(|(_, c)| c),
+                b.map(|(_, c)| c)
+            ),
+        }
+    }
+}