condor_grid/algorithms/

lazy_theta_star.rs

//! Any-angle [`AnyAnglePathfinder`]: online Lazy Theta*.
//!
//! Each grid-aligned-vertex query is independent. It shares Theta*'s no-corner-cut
//! vertex model but defers line-of-sight repair until expansion. Path cost is Euclidean
//! segment length and outcomes follow the any-angle invalid/found/no-path contract.
//! Prefer [`super::theta_star::ThetaStar`] for immediate LOS checks, or
//! [`crate::PreparedAnyAngleGrid`] for repeated exact static queries.

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;

const EPSILON: f64 = 1e-9;

/// Online [`AnyAnglePathfinder`] with deferred LOS (Lazy Theta*).
///
/// Same vertex grid and Euclidean cost model as [`super::theta_star::ThetaStar`]; parent
/// chains are repaired at expansion when a popped vertex lacks LOS to its parent.
/// Prefer when LOS is expensive relative to expansions; not a multi-query prepared map.
#[derive(Debug, Clone, Copy, Default)]
pub struct LazyThetaStar;

impl AnyAnglePathfinder for LazyThetaStar {
    fn name(&self) -> &'static str {
        "lazy-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 closed = vec![false; 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 closed[current_idx] {
                continue;
            }

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

            set_vertex(
                grid,
                current_v,
                current_idx,
                width,
                &mut g_costs,
                &mut parents,
                &closed,
            );
            if g_costs[current_idx].is_infinite() {
                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));
            }

            closed[current_idx] = true;

            for neighbor_v in neighbors(grid, current_v) {
                let neighbor_idx = vertex_index(neighbor_v, width);
                if closed[neighbor_idx] {
                    continue;
                }

                let current_parent_idx = parents[current_idx].unwrap_or(current_idx);
                let current_parent_v = vertex_from_index(current_parent_idx, width);

                let shortcut_g = g_costs[current_parent_idx]
                    + current_parent_v
                        .to_point2()
                        .distance_to(neighbor_v.to_point2());
                let edge_g = g_costs[current_idx]
                    + current_v.to_point2().distance_to(neighbor_v.to_point2());

                let (candidate_g, candidate_parent_idx) = if shortcut_g < edge_g {
                    (shortcut_g, current_parent_idx)
                } else {
                    (edge_g, current_idx)
                };

                if candidate_g + EPSILON < g_costs[neighbor_idx] {
                    g_costs[neighbor_idx] = candidate_g;
                    parents[neighbor_idx] = Some(candidate_parent_idx);
                    frontier.push(FrontierEntry {
                        vertex: neighbor_v,
                        f_cost: candidate_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 current_idx = goal_idx;
        while current_idx != start_idx {
            let next_idx = parents[current_idx].unwrap();
            if next_idx == current_idx {
                break;
            }
            let point = vertex_from_index(next_idx, width).to_point2();
            if points
                .last()
                .is_some_and(|last| point.distance_to(*last) > EPSILON)
            {
                points.push(point);
            }
            current_idx = next_idx;
        }
        if points
            .last()
            .is_some_and(|last| start.distance_to(*last) > EPSILON)
        {
            points.push(start);
        }
        points.reverse();

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

fn set_vertex(
    grid: &Grid,
    current_v: Vertex,
    current_idx: usize,
    width: usize,
    g_costs: &mut [f64],
    parents: &mut [Option<usize>],
    closed: &[bool],
) {
    let Some(parent_idx) = parents[current_idx] else {
        return;
    };
    if parent_idx == current_idx {
        return;
    }

    let parent_v = vertex_from_index(parent_idx, width);
    if has_line_of_sight(grid, parent_v.to_point2(), current_v.to_point2()) {
        return;
    }

    let mut best_parent = None;
    let mut best_g = f64::INFINITY;
    for neighbor_v in neighbors(grid, current_v) {
        let neighbor_idx = vertex_index(neighbor_v, width);
        if !closed[neighbor_idx] {
            continue;
        }

        let candidate_g =
            g_costs[neighbor_idx] + neighbor_v.to_point2().distance_to(current_v.to_point2());
        if candidate_g < best_g {
            best_g = candidate_g;
            best_parent = Some(neighbor_idx);
        }
    }

    if let Some(best_parent_idx) = best_parent {
        g_costs[current_idx] = best_g;
        parents[current_idx] = Some(best_parent_idx);
    } else {
        g_costs[current_idx] = f64::INFINITY;
        parents[current_idx] = None;
    }
}

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

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

        Some(Self {
            x: point.x.round() as usize,
            y: point.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() <= EPSILON
}

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

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

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

fn neighbors(grid: &Grid, vertex: Vertex) -> Vec<Vertex> {
    let mut neighbors = Vec::with_capacity(8);
    let x = vertex.x as i64;
    let y = vertex.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 neighbor = Vertex {
                x: nx as usize,
                y: ny as usize,
            };

            if is_move_legal(grid, vertex, neighbor) {
                neighbors.push(neighbor);
            }
        }
    }

    neighbors
}

fn is_move_legal(grid: &Grid, from: Vertex, to: Vertex) -> bool {
    let x_min = from.x.min(to.x);
    let x_max = from.x.max(to.x);
    let y_min = from.y.min(to.y);
    let y_max = from.y.max(to.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 cell_x = if to.x > from.x { from.x } else { from.x - 1 };
        let cell_y = if to.y > from.y { from.y } else { from.y - 1 };
        grid.is_walkable(Point::new(cell_x, cell_y))
    }
}

#[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))
    }
}