From 5f7e238ef56c3b8c9b163b7708aa33ec8addb0d6 Mon Sep 17 00:00:00 2001 From: tachsin Date: Thu, 13 Aug 2026 23:58:37 +0300 Subject: [PATCH 1/2] feat: add astar_reach for steppable A* search A* could not be interrupted or inspected mid-search. astar_reach yields each expansion so callers can step, visualize, or stop early. Co-authored-by: Cursor --- src/directed/astar.rs | 158 ++++++++++++++++++++++++++++++++++++++++++ tests/astar-reach.rs | 155 +++++++++++++++++++++++++++++++++++++++++ 2 files changed, 313 insertions(+) create mode 100644 tests/astar-reach.rs diff --git a/src/directed/astar.rs b/src/directed/astar.rs index d6503d46..d05ae8c5 100644 --- a/src/directed/astar.rs +++ b/src/directed/astar.rs @@ -3,6 +3,7 @@ use indexmap::map::Entry::{Occupied, Vacant}; use num_traits::Zero; +use rustc_hash::FxHashSet; use std::cmp::Ordering; use std::collections::BinaryHeap; use std::hash::Hash; @@ -401,3 +402,160 @@ impl Iterator for AstarSolution { } impl FusedIterator for AstarSolution {} + +/// Struct returned by [`astar_reach`]. +pub struct AstarReachable { + to_see: BinaryHeap>, + seen: FxHashSet, + parents: FxIndexMap, + successors: FN, + heuristic: FH, +} + +/// Information about a node reached by [`astar_reach`]. +#[derive(Debug, Hash, PartialEq, Eq, Clone)] +pub struct AstarReachableItem { + /// The node that was reached by [`astar_reach`]. + pub node: N, + /// The previous node that the current node came from. + /// If the node is the first node, there will be no parent. + pub parent: Option, + /// The total cost from the starting node (`g`). + pub total_cost: C, + /// The estimated cost of a path through this node (`f = g + h`). + pub estimated_cost: C, +} + +impl Iterator for AstarReachable +where + N: Eq + Hash + Clone, + C: Zero + Ord + Copy, + FN: FnMut(&N) -> IN, + IN: IntoIterator, + FH: FnMut(&N) -> C, +{ + type Item = AstarReachableItem; + + fn next(&mut self) -> Option { + while let Some(SmallestCostHolder { cost, index, .. }) = self.to_see.pop() { + let total_cost = self.parents.get_index(index).unwrap().1 .1; + // A node may have been inserted several times if a cheaper path + // was found later. Skip heap entries that are no longer best + // before recording the node as expanded. + if cost > total_cost { + continue; + } + if !self.seen.insert(index) { + continue; + } + let item; + let successors = { + let (node, &(parent_index, _)) = self.parents.get_index(index).unwrap(); + let estimated_cost = total_cost + (self.heuristic)(node); + item = Some(AstarReachableItem { + node: node.clone(), + parent: self.parents.get_index(parent_index).map(|x| x.0.clone()), + total_cost, + estimated_cost, + }); + (self.successors)(node) + }; + for (successor, move_cost) in successors { + let new_cost = cost + move_cost; + let h; + let n; + match self.parents.entry(successor) { + Vacant(e) => { + h = (self.heuristic)(e.key()); + n = e.index(); + e.insert((index, new_cost)); + } + Occupied(mut e) => { + if e.get().1 > new_cost { + h = (self.heuristic)(e.key()); + n = e.index(); + e.insert((index, new_cost)); + } else { + continue; + } + } + } + self.to_see.push(SmallestCostHolder { + estimated_cost: new_cost + h, + cost: new_cost, + index: n, + }); + } + return item; + } + None + } +} + +impl FusedIterator for AstarReachable +where + N: Eq + Hash + Clone, + C: Zero + Ord + Copy, + FN: FnMut(&N) -> IN, + IN: IntoIterator, + FH: FnMut(&N) -> C, +{ +} + +/// Visit all nodes reachable from `start` in A* expansion order. +/// +/// Nodes are yielded when they are expanded, in increasing `f = g + h` +/// order (with the same tie-break as [`astar`]: higher `g` first). Each +/// node is yielded at most once. Drop the iterator, or stop iterating, +/// to interrupt the search. +/// +/// - `start` is the starting node. +/// - `successors` returns a list of successors for a given node, along with the +/// cost for moving from the node to the successor. This cost must be non-negative. +/// - `heuristic` returns an approximation of the cost from a given node to the +/// goal. The approximation must not be greater than the real cost, or a wrong +/// shortest path may be returned. +/// +/// The start node is always yielded first. There is no built-in goal test: +/// stop from the outside with [`Iterator::find`], [`Iterator::take_while`], +/// or by dropping the iterator. +/// +/// # Example +/// +/// ``` +/// use pathfinding::prelude::astar_reach; +/// +/// let goal = 6_u32; +/// let reached = astar_reach(&0, |&n| vec![(n + 1, 1), (n + 2, 1)], |&n| goal.abs_diff(n)) +/// .find(|r| r.node == goal) +/// .expect("unreachable"); +/// assert_eq!(reached.total_cost, 3); +/// ``` +pub fn astar_reach( + start: &N, + successors: FN, + heuristic: FH, +) -> AstarReachable +where + N: Eq + Hash + Clone, + C: Zero + Ord + Copy, + FN: FnMut(&N) -> IN, + IN: IntoIterator, + FH: FnMut(&N) -> C, +{ + let mut to_see = BinaryHeap::new(); + to_see.push(SmallestCostHolder { + estimated_cost: Zero::zero(), + cost: Zero::zero(), + index: 0, + }); + let mut parents: FxIndexMap = FxIndexMap::default(); + parents.insert(start.clone(), (usize::MAX, Zero::zero())); + AstarReachable { + to_see, + seen: FxHashSet::default(), + parents, + successors, + heuristic, + } +} diff --git a/tests/astar-reach.rs b/tests/astar-reach.rs new file mode 100644 index 00000000..6c70ef39 --- /dev/null +++ b/tests/astar-reach.rs @@ -0,0 +1,155 @@ +use itertools::Itertools; +use pathfinding::prelude::{astar, astar_reach, dijkstra_reach, AstarReachableItem}; + +#[test] +fn astar_reach_graph() { + // 2 2 + // A --> B --> C + // \__________/ + // 5 + let mut graph = std::collections::HashMap::new(); + graph.insert("A", vec![("B", 2), ("C", 5)]); + graph.insert("B", vec![("C", 2)]); + graph.insert("C", vec![]); + + let reach = astar_reach(&"A", |prev| graph[prev].clone(), |_| 0).collect_vec(); + + assert_eq!( + reach, + vec![ + AstarReachableItem { + node: "A", + parent: None, + total_cost: 0, + estimated_cost: 0, + }, + AstarReachableItem { + node: "B", + parent: Some("A"), + total_cost: 2, + estimated_cost: 2, + }, + AstarReachableItem { + node: "C", + parent: Some("B"), + total_cost: 4, + estimated_cost: 4, + }, + ] + ); +} + +#[test] +fn zero_heuristic_matches_dijkstra_reach() { + let mut graph = std::collections::HashMap::new(); + graph.insert("A", vec![("B", 2), ("C", 5)]); + graph.insert("B", vec![("C", 2)]); + graph.insert("C", vec![]); + + let astar_items = astar_reach(&"A", |prev| graph[prev].clone(), |_| 0).collect_vec(); + let dijkstra_items = dijkstra_reach(&"A", |prev| graph[prev].clone()).collect_vec(); + + assert_eq!(astar_items.len(), dijkstra_items.len()); + for (a, d) in astar_items.iter().zip(&dijkstra_items) { + assert_eq!(a.node, d.node); + assert_eq!(a.parent, d.parent); + assert_eq!(a.total_cost, d.total_cost); + assert_eq!(a.estimated_cost, a.total_cost); + } +} + +#[test] +fn stops_at_goal_with_same_cost_as_astar() { + const GOAL: (i32, i32) = (4, 4); + + let successors = |&(x, y): &(i32, i32)| { + [(1, 0), (-1, 0), (0, 1), (0, -1)] + .into_iter() + .map(move |(dx, dy)| ((x + dx, y + dy), 1_u32)) + .filter(|&((nx, ny), _)| (0..=4).contains(&nx) && (0..=4).contains(&ny)) + }; + let heuristic = |&(x, y): &(i32, i32)| GOAL.0.abs_diff(x) + GOAL.1.abs_diff(y); + + let reached = astar_reach(&(0, 0), successors, heuristic) + .find(|r| r.node == GOAL) + .expect("goal not reached"); + let (path, cost) = astar(&(0, 0), successors, heuristic, |n| *n == GOAL).expect("no path"); + + assert_eq!(reached.total_cost, cost); + assert_eq!(reached.estimated_cost, cost); + assert_eq!(path.last(), Some(&GOAL)); +} + +#[test] +fn heuristic_expands_fewer_nodes_than_dijkstra() { + const GOAL: (i32, i32) = (6, 6); + + let successors = |&(x, y): &(i32, i32)| { + [(1, 0), (-1, 0), (0, 1), (0, -1)] + .into_iter() + .map(move |(dx, dy)| ((x + dx, y + dy), 1_u32)) + .filter(|&((nx, ny), _)| (0..=6).contains(&nx) && (0..=6).contains(&ny)) + }; + let heuristic = |&(x, y): &(i32, i32)| GOAL.0.abs_diff(x) + GOAL.1.abs_diff(y); + + let guided = astar_reach(&(0, 0), successors, heuristic) + .take_while(|r| r.node != GOAL) + .count(); + let unguided = astar_reach(&(0, 0), successors, |_| 0) + .take_while(|r| r.node != GOAL) + .count(); + + assert!( + guided < unguided, + "guided expansions {guided} should be fewer than unguided {unguided}" + ); +} + +#[test] +fn is_fused() { + let mut it = astar_reach( + &1, + |&n| vec![(n + 1, 1)].into_iter().filter(|&(x, _)| x <= 3), + |_| 0, + ); + assert!(it.next().is_some()); + assert!(it.next().is_some()); + assert!(it.next().is_some()); + for _ in 0..3 { + assert!(it.next().is_none()); + } +} + +#[test] +fn parent_chain_reaches_start() { + const GOAL: (i32, i32) = (3, 1); + let successors = |&(x, y): &(i32, i32)| { + [(1, 0), (-1, 0), (0, 1), (0, -1)] + .into_iter() + .map(move |(dx, dy)| ((x + dx, y + dy), 1_u32)) + .filter(|&((nx, ny), _)| (0..=3).contains(&nx) && (0..=3).contains(&ny)) + }; + let heuristic = |&(x, y): &(i32, i32)| GOAL.0.abs_diff(x) + GOAL.1.abs_diff(y); + + let mut by_node = std::collections::HashMap::new(); + for item in astar_reach(&(0, 0), successors, heuristic) { + let at_goal = item.node == GOAL; + by_node.insert(item.node, item); + if at_goal { + break; + } + } + + let mut path = vec![GOAL]; + let mut current = GOAL; + while let Some(parent) = by_node[¤t].parent { + path.push(parent); + current = parent; + } + path.reverse(); + + assert_eq!(path.first(), Some(&(0, 0))); + assert_eq!(path.last(), Some(&GOAL)); + let (_, cost) = astar(&(0, 0), successors, heuristic, |n| *n == GOAL).unwrap(); + assert_eq!(by_node[&GOAL].total_cost, cost); +} From 1be7e83b31966f9a0cf05155e56d5894307b1dd1 Mon Sep 17 00:00:00 2001 From: tachsin Date: Fri, 11 Sep 2026 20:45:12 +0300 Subject: [PATCH 2/2] fix(astar_reach): report the queued priority, and expand again when a cheaper path appears Two problems raised in review, both of which made the iterator disagree with `astar` on graphs `astar` handles. A closed set stopped a node being expanded a second time. `astar` requires only that the heuristic be admissible, and an admissible heuristic need not be consistent: when it drops by more than the cost of the edge travelled, a cheaper route to an already-expanded node turns up later and `astar` expands it again. Suppressing that does not merely hide the repeat, it reports costs that are wrong. On the graph from the review, with `S->A` 3, `S->B` 1, `B->A` 1, `A->G` 1 and `h(A)` 0 against `h(B)` 2, the iterator gave the goal a cost of 4 where `astar` returns 3. A node is now yielded again when a cheaper path to it is found, which is what `astar` does, and the documentation says so instead of promising each node once. `estimated_cost` was recomputed by calling the heuristic again for the node being expanded, rather than reported from the queue entry that selected it. The heuristic is an `FnMut` and may be stateful, so the extra call both reports a value that took no part in the search and perturbs the search being watched. The popped priority is now carried through. This also fixes the start node, which was queued with an estimate of zero rather than its own heuristic; nothing read that before, because expansion recomputed it. Both are covered by tests that fail against the previous code: the review's graph, checked against `astar` and asserting the node really is expanded twice, and a heuristic that answers differently the second time it is asked about a node. --- src/directed/astar.rs | 32 +++++++++------- tests/astar-reach.rs | 89 ++++++++++++++++++++++++++++++++++++++++++- 2 files changed, 107 insertions(+), 14 deletions(-) diff --git a/src/directed/astar.rs b/src/directed/astar.rs index d05ae8c5..4f4237e7 100644 --- a/src/directed/astar.rs +++ b/src/directed/astar.rs @@ -3,7 +3,6 @@ use indexmap::map::Entry::{Occupied, Vacant}; use num_traits::Zero; -use rustc_hash::FxHashSet; use std::cmp::Ordering; use std::collections::BinaryHeap; use std::hash::Hash; @@ -406,7 +405,6 @@ impl FusedIterator for AstarSolution {} /// Struct returned by [`astar_reach`]. pub struct AstarReachable { to_see: BinaryHeap>, - seen: FxHashSet, parents: FxIndexMap, successors: FN, heuristic: FH, @@ -437,21 +435,22 @@ where type Item = AstarReachableItem; fn next(&mut self) -> Option { - while let Some(SmallestCostHolder { cost, index, .. }) = self.to_see.pop() { - let total_cost = self.parents.get_index(index).unwrap().1 .1; + while let Some(SmallestCostHolder { + estimated_cost, + cost, + index, + }) = self.to_see.pop() + { + let total_cost = self.parents.get_index(index).unwrap().1.1; // A node may have been inserted several times if a cheaper path // was found later. Skip heap entries that are no longer best // before recording the node as expanded. if cost > total_cost { continue; } - if !self.seen.insert(index) { - continue; - } let item; let successors = { let (node, &(parent_index, _)) = self.parents.get_index(index).unwrap(); - let estimated_cost = total_cost + (self.heuristic)(node); item = Some(AstarReachableItem { node: node.clone(), parent: self.parents.get_index(parent_index).map(|x| x.0.clone()), @@ -505,9 +504,16 @@ where /// Visit all nodes reachable from `start` in A* expansion order. /// /// Nodes are yielded when they are expanded, in increasing `f = g + h` -/// order (with the same tie-break as [`astar`]: higher `g` first). Each -/// node is yielded at most once. Drop the iterator, or stop iterating, -/// to interrupt the search. +/// order (with the same tie-break as [`astar`]: higher `g` first). Drop the +/// iterator, or stop iterating, to interrupt the search. +/// +/// A node is yielded again if a cheaper path to it turns up after it was +/// expanded, exactly as [`astar`] expands it again. That cannot happen when the +/// heuristic is consistent — when it never drops by more than the cost of the +/// edge travelled — so with the heuristics most callers write, every node comes +/// out once. It can happen for a heuristic that is admissible but not +/// consistent, and suppressing it would report costs that are simply wrong: the +/// second expansion is how the cheaper route becomes visible. /// /// - `start` is the starting node. /// - `successors` returns a list of successors for a given node, along with the @@ -543,9 +549,10 @@ where IN: IntoIterator, FH: FnMut(&N) -> C, { + let mut heuristic = heuristic; let mut to_see = BinaryHeap::new(); to_see.push(SmallestCostHolder { - estimated_cost: Zero::zero(), + estimated_cost: heuristic(start), cost: Zero::zero(), index: 0, }); @@ -553,7 +560,6 @@ where parents.insert(start.clone(), (usize::MAX, Zero::zero())); AstarReachable { to_see, - seen: FxHashSet::default(), parents, successors, heuristic, diff --git a/tests/astar-reach.rs b/tests/astar-reach.rs index 6c70ef39..156ed0d3 100644 --- a/tests/astar-reach.rs +++ b/tests/astar-reach.rs @@ -1,5 +1,5 @@ use itertools::Itertools; -use pathfinding::prelude::{astar, astar_reach, dijkstra_reach, AstarReachableItem}; +use pathfinding::prelude::{AstarReachableItem, astar, astar_reach, dijkstra_reach}; #[test] fn astar_reach_graph() { @@ -153,3 +153,90 @@ fn parent_chain_reaches_start() { let (_, cost) = astar(&(0, 0), successors, heuristic, |n| *n == GOAL).unwrap(); assert_eq!(by_node[&GOAL].total_cost, cost); } + +/// An admissible heuristic need not be consistent, and when it is not, A* finds a cheaper route +/// to a node it has already expanded and expands it again. The iterator has to do the same, or +/// it reports costs that are simply wrong. +/// +/// Taken from the review of this pull request: `h(A) = 0` while `h(B) = 2`, so `h` drops by 2 +/// across the single edge `B -> A`, which costs 1. Reaching `A` looks best at cost 3 until `B` +/// is expanded and offers it at cost 2. +#[test] +fn inconsistent_heuristic_reopens_like_astar() { + const START: char = 'S'; + let successors = |&n: &char| match n { + 'S' => vec![('A', 3), ('B', 1)], + 'B' => vec![('A', 1)], + 'A' => vec![('G', 1)], + _ => vec![], + }; + let heuristic = |&n: &char| match n { + 'B' => 2, + _ => 0, + }; + + let by_astar = astar(&START, successors, heuristic, |&n| n == 'G').expect("no path"); + assert_eq!(by_astar.1, 3, "the cheapest route is S -> B -> A -> G"); + + // The iterator must agree with `astar` about the goal's cost. + let reached = astar_reach(&START, successors, heuristic) + .find(|item| item.node == 'G') + .expect("goal never reached"); + assert_eq!( + reached.total_cost, by_astar.1, + "iterator reported {} for the goal, astar says {}", + reached.total_cost, by_astar.1 + ); + + // `A` is expanded twice: once at 3, then again at 2 once `B` has been seen. + let costs_for_a = astar_reach(&START, successors, heuristic) + .filter(|item| item.node == 'A') + .map(|item| item.total_cost) + .collect::>(); + assert_eq!(costs_for_a, vec![3, 2]); +} + +/// `estimated_cost` must be the priority that actually selected the node, not a fresh call to +/// the heuristic when the node is expanded. +/// +/// The heuristic is an `FnMut`, so it is allowed to be stateful — instrumented to count calls, +/// or memoising something expensive. Calling it a second time for a node already queued both +/// reports a value that never took part in the search and perturbs the search being observed. +/// The heuristic here answers differently on a second call for the same node, which makes the +/// difference visible: the reported `f` must be the one computed when the node was queued. +#[test] +fn the_heuristic_is_not_called_again_for_expanded_nodes() { + // Only right and down, so every route to a cell is the same length and no cell is ever + // requeued more cheaply. Each is therefore queued exactly once. + let successors = |&(x, y): &(i32, i32)| { + [(1, 0), (0, 1)] + .into_iter() + .map(move |(dx, dy)| ((x + dx, y + dy), 1_u32)) + .filter(|&((nx, ny), _)| (0..4).contains(&nx) && (0..4).contains(&ny)) + }; + let goal = (3, 3); + #[expect(clippy::cast_sign_loss)] + let plain = move |&(x, y): &(i32, i32)| ((goal.0 - x) + (goal.1 - y)) as u32; + + let mut asked = std::collections::HashSet::new(); + let once_only = |n: &(i32, i32)| { + if asked.insert(*n) { + plain(n) + } else { + 0 // a second question about the same node gets a different answer + } + }; + let items = astar_reach(&(0, 0), successors, once_only).collect::>(); + + for item in &items { + assert_eq!( + item.estimated_cost, + item.total_cost + plain(&item.node), + "f for {:?} did not come from the queue", + item.node + ); + } + // The start node included: it used to be queued with an estimate of zero. + assert_eq!(items[0].node, (0, 0)); + assert_eq!(items[0].estimated_cost, plain(&(0, 0))); +}