From f54a9c04f568d3cb4e214123c9b04337edbd813b Mon Sep 17 00:00:00 2001 From: tachsin Date: Fri, 11 Sep 2026 19:28:28 +0300 Subject: [PATCH] feat: add theta_star, an any-angle search MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit `astar` returns paths made of graph edges, so on a grid it can only travel in the directions the grid offers and crosses open ground as a staircase. Theta* keeps the same search order but, on reaching a node, first asks whether the node it came from can see the successor directly, and joins them in a straight line when it can. The geometry stays with the caller. The algorithm needs one thing `astar` does not — given two nodes that may not be neighbours, can you travel straight between them and at what cost — and that is a single closure, so `N` remains an opaque hashable value and `C` remains `Zero + Ord + Copy`. Nothing here gains a dependency on coordinates or floating point. Two ways this differs from the rest of the crate, both documented on the function: the path is not optimal, and consecutive nodes of the path need not be neighbours, since the segments between waypoints are straight lines rather than edges. `SmallestCostHolder` is shared with `astar` rather than duplicated; it becomes visible to the module, with no change to the public API. Tests cover the three properties that matter: with sight always blocked the result is exactly what `astar` returns, on open ground the path collapses to its endpoints, and on random maps the path is never longer than `astar`'s while every segment is a clear line whose lengths add up to the cost reported. --- src/directed/astar.rs | 9 +- src/directed/mod.rs | 1 + src/directed/theta_star.rs | 150 +++++++++++++++++++++++++++ src/lib.rs | 3 + tests/theta_star.rs | 207 +++++++++++++++++++++++++++++++++++++ 5 files changed, 366 insertions(+), 4 deletions(-) create mode 100644 src/directed/theta_star.rs create mode 100644 tests/theta_star.rs diff --git a/src/directed/astar.rs b/src/directed/astar.rs index d6503d46..128248f8 100644 --- a/src/directed/astar.rs +++ b/src/directed/astar.rs @@ -313,10 +313,11 @@ where /// `estimated_cost`, the highest `cost` will be favored, as it may /// indicate that the goal is nearer, thereby requiring fewer /// exploration steps. -struct SmallestCostHolder { - estimated_cost: K, - cost: K, - index: usize, +/// Shared with [`theta_star`](super::theta_star), which orders its queue the same way. +pub(super) struct SmallestCostHolder { + pub(super) estimated_cost: K, + pub(super) cost: K, + pub(super) index: usize, } impl PartialEq for SmallestCostHolder { diff --git a/src/directed/mod.rs b/src/directed/mod.rs index 7e585338..d25dc19f 100644 --- a/src/directed/mod.rs +++ b/src/directed/mod.rs @@ -14,6 +14,7 @@ pub mod fringe; pub mod idastar; pub mod iddfs; pub mod strongly_connected_components; +pub mod theta_star; pub mod topological_sort; pub mod yen; diff --git a/src/directed/theta_star.rs b/src/directed/theta_star.rs new file mode 100644 index 00000000..9737ef7e --- /dev/null +++ b/src/directed/theta_star.rs @@ -0,0 +1,150 @@ +//! Compute a path that is not constrained to the edges of the graph, using the [Theta\* +//! algorithm](https://arxiv.org/abs/1401.3843). + +use indexmap::map::Entry::{Occupied, Vacant}; +use num_traits::Zero; +use std::hash::Hash; + +use super::astar::SmallestCostHolder; +use super::reverse_path; +use crate::FxIndexMap; +use std::collections::BinaryHeap; + +/// Compute a path from `start` to a node for which `success` returns `true`, allowing the path +/// to leave the edges of the graph wherever a straight line is available. +/// +/// [`astar`](super::astar::astar) returns a path made of graph edges, so on a grid it can only +/// travel in the directions the grid offers and crosses open ground as a staircase. Theta\* +/// keeps the same search order, but each time it reaches a node it asks whether the node before +/// it can see the successor directly, and if so joins them in a straight line instead. The +/// waypoints are still graph nodes; the segments between them are not. +/// +/// - `start` is the starting node. +/// - `successors` returns the neighbours of a node with the cost of moving to each of them. +/// - `heuristic` approximates the cost from a node to the goal. It must not overestimate it. +/// - `sight` returns the cost of travelling straight from one node to another, or `None` when +/// the line between them is blocked. It is asked about nodes that are not neighbours. +/// - `success` checks whether the goal has been reached. +/// +/// # Differences from the rest of this crate +/// +/// **The path is not optimal.** It is never longer than the one [`astar`](super::astar::astar) +/// would return for the same graph, and is usually close to the shortest any-angle path, but +/// unlike `astar` and [`dijkstra`](super::dijkstra::dijkstra) there is no guarantee: finding the +/// true optimum needs a visibility graph rather than a grid. +/// +/// **Consecutive nodes of the path need not be neighbours.** The path is a list of waypoints +/// joined by straight lines, so `path.windows(2)` are not necessarily edges of `successors`. +/// That is the point of the algorithm, but it differs from every other path this crate returns. +/// +/// `sight` must agree with `successors`: a straight line may never cost more than walking the +/// same way through intermediate nodes. Otherwise the shortcut can make a path worse, and the +/// cost returned stops matching the path. +/// +/// # Example +/// +/// Five nodes in a row, each a unit step from the next. Every node can see every other, so the +/// intermediate ones carry no information and the path collapses to its endpoints — at the same +/// cost `astar` would report for walking all five. +/// +/// ``` +/// use pathfinding::prelude::theta_star; +/// +/// let (path, cost) = theta_star( +/// &0, +/// |&n: &i32| (n < 4).then(|| (n + 1, 1)), +/// |&n| 4 - n, +/// |&a: &i32, &b: &i32| Some((b - a).abs()), +/// |&n| n == 4, +/// ) +/// .expect("no path found"); +/// +/// assert_eq!(cost, 4); +/// assert_eq!(path, vec![0, 4]); +/// ``` +#[expect(clippy::missing_panics_doc)] +pub fn theta_star( + start: &N, + mut successors: FN, + mut heuristic: FH, + mut sight: FL, + mut success: FS, +) -> Option<(Vec, C)> +where + N: Eq + Hash + Clone, + C: Zero + Ord + Copy, + FN: FnMut(&N) -> IN, + IN: IntoIterator, + FH: FnMut(&N) -> C, + FL: FnMut(&N, &N) -> Option, + FS: FnMut(&N) -> bool, +{ + 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())); + while let Some(SmallestCostHolder { cost, index, .. }) = to_see.pop() { + let successors = { + let (node, &(_, c)) = parents.get_index(index).unwrap(); // Cannot fail + if success(node) { + let path = reverse_path(&parents, |&(p, _)| p, index); + return Some((path, cost)); + } + // We may have inserted a node several time into the binary heap if we found + // a better way to access it. Ensure that we are currently dealing with the + // best path and discard the others. + if cost > c { + continue; + } + successors(node) + }; + // The node the path reached this one from, which is the one a shortcut would start at. + // The starting node has no predecessor and stands in for its own. + let previous = match parents.get_index(index).unwrap().1.0 { + usize::MAX => index, + parent => parent, + }; + for (successor, move_cost) in successors { + // Prefer a straight line from the previous waypoint. It can never be dearer than + // going through this node, so the step through this node is only used when the + // line is blocked. + let shortcut = { + let (from, &(_, from_cost)) = parents.get_index(previous).unwrap(); + sight(from, &successor).map(|line_cost| from_cost + line_cost) + }; + let (new_parent, new_cost) = match shortcut { + Some(total) => (previous, total), + None => (index, cost + move_cost), + }; + let h; // heuristic(&successor) + let n; // index for successor + match parents.entry(successor) { + Vacant(e) => { + h = heuristic(e.key()); + n = e.index(); + e.insert((new_parent, new_cost)); + } + Occupied(mut e) => { + if e.get().1 > new_cost { + h = heuristic(e.key()); + n = e.index(); + e.insert((new_parent, new_cost)); + } else { + continue; + } + } + } + + to_see.push(SmallestCostHolder { + estimated_cost: new_cost + h, + cost: new_cost, + index: n, + }); + } + } + None +} diff --git a/src/lib.rs b/src/lib.rs index 95dfc73c..6f33acd3 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -26,6 +26,7 @@ //! - [IDDFS](directed/iddfs/index.html): explore longer and longer paths in an unweighted graph at the cost of multiple similar examinations ([⇒ Wikipedia][IDDFS]) //! - [paths counting](directed/count_paths/index.html): count the paths to the destination in an acyclic graph //! - [strongly connected components](directed/strongly_connected_components/index.html): find strongly connected components in a directed graph ([⇒ Wikipedia][Strongly connected components]) +//! - [Theta*](directed/theta_star/index.html): find a path not constrained to the edges of the graph ([⇒ arXiv][Theta*]) //! - [topological sorting](directed/topological_sort/index.html): find an acceptable topological order in a directed graph ([⇒ Wikipedia][Topological sorting]) //! - [Yen](directed/yen/index.html): find k-shortest paths using Dijkstra ([⇒ Wikipedia][Yen]) //! @@ -89,6 +90,7 @@ //! The minimum supported Rust version (MSRV) is Rust 1.88.0. //! //! [A*]: https://en.wikipedia.org/wiki/A*_search_algorithm +//! [Theta*]: https://arxiv.org/abs/1401.3843 //! [BFS]: https://en.wikipedia.org/wiki/Breadth-first_search //! [Bidirectional search]: https://en.wikipedia.org/wiki/Bidirectional_search //! [Brent]: https://en.wikipedia.org/wiki/Cycle_detection#Brent's_algorithm @@ -142,6 +144,7 @@ pub mod prelude { pub use crate::directed::idastar::*; pub use crate::directed::iddfs::*; pub use crate::directed::strongly_connected_components::*; + pub use crate::directed::theta_star::*; pub use crate::directed::topological_sort::*; pub use crate::directed::yen::*; pub use crate::grid::*; diff --git a/tests/theta_star.rs b/tests/theta_star.rs new file mode 100644 index 00000000..e87df321 --- /dev/null +++ b/tests/theta_star.rs @@ -0,0 +1,207 @@ +use noisy_float::prelude::{Float, R64, r64}; +use pathfinding::prelude::{astar, theta_star}; +use rand::{RngExt as _, SeedableRng as _}; +use rand_xorshift::XorShiftRng; + +const SIDE: i32 = 24; + +fn cells() -> usize { + usize::try_from(SIDE * SIDE).unwrap() +} + +/// A square grid with some cells blocked, moving in the eight compass directions. +struct Map { + blocked: Vec, +} + +impl Map { + fn random(rng: &mut XorShiftRng, blocked_percent: u32) -> Self { + let mut blocked = vec![false; cells()]; + for cell in &mut blocked { + *cell = rng.random_range(0..100u32) < blocked_percent; + } + blocked[0] = false; + let last = cells() - 1; + blocked[last] = false; + Self { blocked } + } + + fn open(&self, (column, row): (i32, i32)) -> bool { + let Ok(index) = usize::try_from(row * SIDE + column) else { + return false; + }; + (0..SIDE).contains(&column) && (0..SIDE).contains(&row) && !self.blocked[index] + } + + #[expect(clippy::trivially_copy_pass_by_ref)] + fn successors(&self, &(column, row): &(i32, i32)) -> Vec<((i32, i32), R64)> { + let mut out = Vec::new(); + for dx in -1..=1 { + for dy in -1..=1 { + if (dx, dy) == (0, 0) { + continue; + } + let next = (column + dx, row + dy); + if self.open(next) { + out.push((next, distance((column, row), next))); + } + } + } + out + } + + /// Cost of a straight line, or `None` if it crosses a blocked cell. Every cell the segment + /// touches is sampled, densely enough that no blocked cell can be stepped over. + #[expect(clippy::trivially_copy_pass_by_ref)] + fn sight(&self, &from: &(i32, i32), &to: &(i32, i32)) -> Option { + let steps = ((from.0 - to.0).abs().max((from.1 - to.1).abs()) * 4).max(1); + for step in 0..=steps { + let along = f64::from(step) / f64::from(steps); + let column = f64::from(from.0) + along * f64::from(to.0 - from.0); + let row = f64::from(from.1) + along * f64::from(to.1 - from.1); + if !self.open((round_to_cell(column), round_to_cell(row))) { + return None; + } + } + Some(distance(from, to)) + } +} + +/// Coordinates are small and the value is a rounded position on the grid, so this cannot +/// truncate in any way that matters. +#[expect(clippy::cast_possible_truncation)] +fn round_to_cell(value: f64) -> i32 { + value.round() as i32 +} + +fn distance(from: (i32, i32), to: (i32, i32)) -> R64 { + r64(f64::from(from.0 - to.0).hypot(f64::from(from.1 - to.1))) +} + +#[test] +fn blocked_sight_degenerates_to_astar() { + // With no line of sight ever available, every shortcut is refused and the search has to + // behave exactly like `astar`. + let mut rng = XorShiftRng::from_seed([11; 16]); + for round in 0..40 { + let map = Map::random(&mut rng, 25); + let goal = (SIDE - 1, SIDE - 1); + let by_astar = astar( + &(0, 0), + |n| map.successors(n), + |&n| distance(n, goal), + |&n| n == goal, + ); + let by_theta = theta_star( + &(0, 0), + |n| map.successors(n), + |&n| distance(n, goal), + |_: &(i32, i32), _: &(i32, i32)| None, + |&n| n == goal, + ); + assert_eq!( + by_astar.as_ref().map(|(p, c)| (p.clone(), *c)), + by_theta, + "round {round}" + ); + } +} + +#[test] +fn never_longer_than_astar_and_the_path_is_walkable() { + let mut rng = XorShiftRng::from_seed([29; 16]); + let mut shortened = 0; + for round in 0..60 { + let map = Map::random(&mut rng, 20); + let goal = (SIDE - 1, SIDE - 1); + let Some((_, astar_cost)) = astar( + &(0, 0), + |n| map.successors(n), + |&n| distance(n, goal), + |&n| n == goal, + ) else { + continue; + }; + let (path, cost) = theta_star( + &(0, 0), + |n| map.successors(n), + |&n| distance(n, goal), + |a, b| map.sight(a, b), + |&n| n == goal, + ) + .unwrap_or_else(|| panic!("round {round}: astar found a path and theta_star did not")); + + assert_eq!(path.first(), Some(&(0, 0)), "round {round}"); + assert_eq!(path.last(), Some(&goal), "round {round}"); + assert!( + cost <= astar_cost, + "round {round}: theta_star returned {cost}, longer than astar's {astar_cost}" + ); + if cost < astar_cost { + shortened += 1; + } + + // Every segment must be a clear straight line, and they must add up to the cost. + let mut total = r64(0.0); + for step in path.windows(2) { + let segment = map + .sight(&step[0], &step[1]) + .unwrap_or_else(|| panic!("round {round}: {:?} cannot see {:?}", step[0], step[1])); + total += segment; + } + assert!( + (total - cost).abs() < r64(1e-9), + "round {round}: segments total {total} but cost is {cost}" + ); + + let mut seen = path; + seen.sort_unstable(); + let len = seen.len(); + seen.dedup(); + assert_eq!(seen.len(), len, "round {round}: path repeats a waypoint"); + } + // The whole point is that it usually does better than the grid-constrained path. + assert!( + shortened > 30, + "only {shortened} of the paths were shorter than astar's" + ); +} + +#[test] +fn open_ground_gives_a_straight_line() { + // Nothing blocked, so the start can see the goal and the answer is two waypoints. + let map = Map { + blocked: vec![false; (SIDE * SIDE) as usize], + }; + let goal = (SIDE - 1, SIDE - 1); + let (path, cost) = theta_star( + &(0, 0), + |n| map.successors(n), + |&n| distance(n, goal), + |a, b| map.sight(a, b), + |&n| n == goal, + ) + .expect("no path found"); + assert_eq!(path, vec![(0, 0), goal]); + assert!((cost - distance((0, 0), goal)).abs() < r64(1e-9)); +} + +#[test] +fn no_path_when_the_goal_is_walled_off() { + let mut blocked = vec![false; cells()]; + for row in 0..SIDE { + blocked[usize::try_from(row * SIDE + SIDE / 2).unwrap()] = true; + } + let map = Map { blocked }; + let goal = (SIDE - 1, SIDE - 1); + assert_eq!( + theta_star( + &(0, 0), + |n| map.successors(n), + |&n| distance(n, goal), + |a, b| map.sight(a, b), + |&n| n == goal, + ), + None + ); +}