diff --git a/src/directed/astar.rs b/src/directed/astar.rs index d6503d46..61ea64d6 100644 --- a/src/directed/astar.rs +++ b/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 = 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 Ord for SmallestCostHolder { #[derive(Clone)] pub struct AstarSolution { sinks: Vec, + /// 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)>, current: Vec>, terminated: bool, } impl AstarSolution { - 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 AstarSolution { } } + /// 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::>() + } + }; + 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 + '_ { + 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 Iterator for AstarSolution { type Item = Vec; fn next(&mut self) -> Option { - 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::>(); + 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::>(); - self.next_vec(); - self.terminated = self.current.is_empty(); - Some(path) } } diff --git a/tests/astar_bag_zero_cost.rs b/tests/astar_bag_zero_cost.rs new file mode 100644 index 00000000..619b2ed2 --- /dev/null +++ b/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![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![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::>(); + 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> { + 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, Vec>) { + let mut best: Option = None; + let mut paths: Vec> = 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::>(); + 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) + ), + } + } +}