condor_grid/algorithms/

theta_star.rs

//! Any-angle [`AnyAnglePathfinder`]: online Theta*.
//!
//! Each grid-aligned-vertex query is independent; no-corner-cut neighbor moves and
//! parent shortcuts use [`has_line_of_sight`]. Euclidean
//! segment length is the path cost, with the usual invalid/found/no-path any-angle
//! outcome. Prefer [`crate::PreparedAnyAngleGrid`] for repeated exact static queries,
//! or [`super::anya::Anya`] for the curated any-angle entrypoint.

use std::cmp::Ordering;
use std::collections::BinaryHeap;

use crate::{
    Grid, Point,
    any_angle::geometry::canonicalize_grid_vertex,
    any_angle::{
        AnyAnglePath, AnyAnglePathfinder, AnyAngleSearchRequest, AnyAngleSearchResult,
        has_line_of_sight,
    },
};
use condor_core::Point2;

/// Online [`AnyAnglePathfinder`] using Theta* vertex expansion.
///
/// Parent shortcuts require line-of-sight; cost is Euclidean segment length. Prefer
/// when any-angle polylines are needed without preprocess; prefer prepared any-angle
/// for multi-query exact amortization, Lazy Theta* when deferred LOS is acceptable.
#[derive(Debug, Clone, Copy, Default)]
pub struct ThetaStar;

impl AnyAnglePathfinder for ThetaStar {
    fn name(&self) -> &'static str {
        "theta-star"
    }

    fn search(&self, grid: &Grid, request: AnyAngleSearchRequest) -> AnyAngleSearchResult {
        let Some(start) = canonicalize_grid_vertex(request.start) else {
            return Err(crate::AnyAngleSearchError::InvalidStart {
                point: request.start,
            });
        };
        let Some(goal) = canonicalize_grid_vertex(request.goal) else {
            return Err(crate::AnyAngleSearchError::InvalidGoal {
                point: request.goal,
            });
        };
        let Some(start_v) = Vertex::from_point2(start) else {
            return Err(crate::AnyAngleSearchError::InvalidStart { point: start });
        };
        let Some(goal_v) = Vertex::from_point2(goal) else {
            return Err(crate::AnyAngleSearchError::InvalidGoal { point: goal });
        };

        if !is_vertex_valid(grid, start_v) {
            return Err(crate::AnyAngleSearchError::InvalidStart { point: start });
        }
        if !is_vertex_valid(grid, goal_v) {
            return Err(crate::AnyAngleSearchError::InvalidGoal { point: goal });
        }

        if start_v == goal_v {
            return crate::any_angle::found(
                AnyAnglePath::from_points(vec![start, goal])
                    .expect("any-angle path contains at least one point"),
                1,
            );
        }

        let width = grid.width() + 1;
        let height = grid.height() + 1;
        let mut g_costs = vec![f64::INFINITY; width * height];
        let mut parents = vec![None; width * height];
        let mut visited_nodes = 0;
        let watch = crate::search::BudgetWatch::start(request.budget);

        let start_idx = vertex_index(start_v, width);
        let goal_idx = vertex_index(goal_v, width);

        g_costs[start_idx] = 0.0;
        parents[start_idx] = Some(start_idx);

        let mut frontier = BinaryHeap::new();
        frontier.push(FrontierEntry {
            vertex: start_v,
            f_cost: start.distance_to(goal),
        });

        while let Some(current_entry) = frontier.pop() {
            let current_v = current_entry.vertex;
            let current_idx = vertex_index(current_v, width);

            if current_entry.f_cost
                > g_costs[current_idx] + current_v.to_point2().distance_to(goal) + 1e-9
            {
                continue;
            }

            visited_nodes += 1;

            if current_v == goal_v {
                break;
            }

            if let Err(reason) = watch.check(visited_nodes) {
                return Err(crate::any_angle::budget_error(reason));
            }

            for neighbor_v in neighbors(grid, current_v) {
                let neighbor_idx = vertex_index(neighbor_v, width);
                let parent_idx = parents[current_idx].unwrap();
                let parent_v = vertex_from_index(parent_idx, width);

                if has_line_of_sight(grid, parent_v.to_point2(), neighbor_v.to_point2()) {
                    let new_g = g_costs[parent_idx]
                        + parent_v.to_point2().distance_to(neighbor_v.to_point2());
                    if new_g < g_costs[neighbor_idx] {
                        g_costs[neighbor_idx] = new_g;
                        parents[neighbor_idx] = Some(parent_idx);
                        frontier.push(FrontierEntry {
                            vertex: neighbor_v,
                            f_cost: new_g + neighbor_v.to_point2().distance_to(goal),
                        });
                    }
                } else {
                    let new_g = g_costs[current_idx]
                        + current_v.to_point2().distance_to(neighbor_v.to_point2());
                    if new_g < g_costs[neighbor_idx] {
                        g_costs[neighbor_idx] = new_g;
                        parents[neighbor_idx] = Some(current_idx);
                        frontier.push(FrontierEntry {
                            vertex: neighbor_v,
                            f_cost: new_g + neighbor_v.to_point2().distance_to(goal),
                        });
                    }
                }
            }
        }

        if g_costs[goal_idx] == f64::INFINITY {
            return crate::any_angle::not_found(visited_nodes);
        }

        let mut points = vec![goal];
        let mut curr_idx = goal_idx;
        while curr_idx != start_idx {
            let next_idx = parents[curr_idx].unwrap();
            if next_idx == curr_idx {
                break;
            }
            let p = vertex_from_index(next_idx, width).to_point2();
            if points
                .last()
                .is_some_and(|last| p.distance_to(*last) > 1e-9)
            {
                points.push(p);
            }
            curr_idx = next_idx;
        }
        if points
            .last()
            .is_some_and(|last| start.distance_to(*last) > 1e-9)
        {
            points.push(start);
        }
        points.reverse();

        crate::any_angle::found(
            AnyAnglePath::from_points(points).expect("any-angle path contains at least one point"),
            visited_nodes,
        )
    }
}

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
struct Vertex {
    x: usize,
    y: usize,
}

impl Vertex {
    fn from_point2(p: Point2) -> Option<Self> {
        if p.x < 0.0
            || p.y < 0.0
            || !is_grid_vertex_coordinate(p.x)
            || !is_grid_vertex_coordinate(p.y)
        {
            return None;
        }

        Some(Self {
            x: p.x.round() as usize,
            y: p.y.round() as usize,
        })
    }

    fn to_point2(self) -> Point2 {
        Point2::new(self.x as f64, self.y as f64)
    }
}

fn is_grid_vertex_coordinate(value: f64) -> bool {
    (value - value.round()).abs() <= 1e-9
}

fn vertex_index(v: Vertex, width: usize) -> usize {
    v.y * width + v.x
}

fn vertex_from_index(idx: usize, width: usize) -> Vertex {
    Vertex {
        x: idx % width,
        y: idx / width,
    }
}

fn is_vertex_valid(grid: &Grid, v: Vertex) -> bool {
    v.x <= grid.width() && v.y <= grid.height()
}

fn neighbors(grid: &Grid, v: Vertex) -> Vec<Vertex> {
    let mut neighbors = Vec::with_capacity(8);
    let x = v.x as i64;
    let y = v.y as i64;

    for dx in -1..=1 {
        for dy in -1..=1 {
            if dx == 0 && dy == 0 {
                continue;
            }
            let nx = x + dx;
            let ny = y + dy;

            if nx < 0 || nx > grid.width() as i64 || ny < 0 || ny > grid.height() as i64 {
                continue;
            }

            let nv = Vertex {
                x: nx as usize,
                y: ny as usize,
            };

            if is_move_legal(grid, v, nv) {
                neighbors.push(nv);
            }
        }
    }
    neighbors
}

/// No-corner-cut vertex adjacency on the dual of blocked cells.
///
/// Cardinal steps need either adjacent cell open along the shared edge.
/// Diagonal steps require the single cell the diagonal crosses to be open
/// (the cell whose lower-left corner is `min` of the two vertices in each axis).
fn is_move_legal(grid: &Grid, v1: Vertex, v2: Vertex) -> bool {
    let x_min = v1.x.min(v2.x);
    let x_max = v1.x.max(v2.x);
    let y_min = v1.y.min(v2.y);
    let y_max = v1.y.max(v2.y);

    if x_min == x_max {
        let x = x_min;
        let y = y_min;
        let left_open = if x > 0 {
            grid.is_walkable(Point::new(x - 1, y))
        } else {
            false
        };
        let right_open = if x < grid.width() {
            grid.is_walkable(Point::new(x, y))
        } else {
            false
        };
        left_open || right_open
    } else if y_min == y_max {
        let x = x_min;
        let y = y_min;
        let above_open = if y > 0 {
            grid.is_walkable(Point::new(x, y - 1))
        } else {
            false
        };
        let below_open = if y < grid.height() {
            grid.is_walkable(Point::new(x, y))
        } else {
            false
        };
        above_open || below_open
    } else {
        let cx = if v2.x > v1.x { v1.x } else { v1.x - 1 };
        let cy = if v2.y > v1.y { v1.y } else { v1.y - 1 };

        grid.is_walkable(Point::new(cx, cy))
    }
}

#[derive(Debug, PartialEq)]
struct FrontierEntry {
    vertex: Vertex,
    f_cost: f64,
}

impl Eq for FrontierEntry {}

impl Ord for FrontierEntry {
    fn cmp(&self, other: &Self) -> Ordering {
        other
            .f_cost
            .partial_cmp(&self.f_cost)
            .unwrap_or(Ordering::Equal)
    }
}

impl PartialOrd for FrontierEntry {
    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
        Some(self.cmp(other))
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::grid::Cell;

    #[test]
    fn theta_star_finds_direct_path_in_open_field() {
        let grid = Grid::new(10, 10).expect("grid dimensions are valid");
        let request = AnyAngleSearchRequest::new(Point2::new(0.0, 0.0), Point2::new(9.0, 9.0));
        let result = ThetaStar.search(&grid, request);

        assert!(result.as_ref().expect("valid search request").is_found());
        let path = result
            .as_ref()
            .expect("valid search request")
            .path()
            .unwrap();
        assert_eq!(path.points().len(), 2);
        assert!((path.cost() - (9.0 * 2.0f64.sqrt())).abs() <= 1e-9);
    }

    #[test]
    fn theta_star_detours_around_wall() {
        let mut grid = Grid::new(10, 10).expect("grid dimensions are valid");
        for x in 0..8 {
            grid.set_cell(Point::new(x, 5), Cell::Blocked)
                .expect("valid grid edit");
        }

        let request = AnyAngleSearchRequest::new(Point2::new(0.0, 0.0), Point2::new(0.0, 9.0));
        let result = ThetaStar.search(&grid, request);

        assert!(result.as_ref().expect("valid search request").is_found());
        let path = result
            .as_ref()
            .expect("valid search request")
            .path()
            .unwrap();
        assert!(path.points().len() >= 3);
        assert!(path.cost() > 9.0);
    }

    #[test]
    fn theta_star_rejects_non_grid_aligned_inputs() {
        let grid = Grid::new(10, 10).expect("grid dimensions are valid");
        let request = AnyAngleSearchRequest::new(Point2::new(0.25, 0.0), Point2::new(9.0, 9.0));
        let result = ThetaStar.search(&grid, request);

        assert_eq!(
            result,
            Err(crate::AnyAngleSearchError::InvalidStart {
                point: Point2::new(0.25, 0.0),
            })
        );
    }
}