condor_grid/algorithms/anya/

geometry.rs

//! Interval geometry for the row-interval Anya candidate: keys, cones, flat spans.
//!
//! Independent of public Theta* LOS helpers except where sampling legality is
//! intentionally shared. Used by [`super::successors`] and inspect-mode search.

use std::cmp::Ordering;

use crate::{
    Grid,
    any_angle::geometry::{
        approximately_equal, canonicalize_grid_vertex, is_endpoint_valid, retain_as_corner,
        sampling_segment_is_legal, validate_sampling_path,
    },
};
use condor_core::Point2;

/// Whether an interval state projects a flat span or a root cone.
///
/// Flat spans cover open runs with a root on the same row; cones project from a
/// root on an adjacent row through turning geometry.
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
pub enum IntervalKind {
    /// Open horizontal span visible from a co-row root (no cone projection).
    Flat,
    /// Visibility cone from an off-row root through an interval on another row.
    Cone,
}

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

impl Ord for IntervalKind {
    fn cmp(&self, other: &Self) -> Ordering {
        match (self, other) {
            (Self::Flat, Self::Flat) | (Self::Cone, Self::Cone) => Ordering::Equal,
            (Self::Flat, Self::Cone) => Ordering::Less,
            (Self::Cone, Self::Flat) => Ordering::Greater,
        }
    }
}

/// Total order for `f64` keys; non-finite values sort after all finite keys.
#[must_use]
pub fn compare_f64_total(left: f64, right: f64) -> Ordering {
    match (left.is_finite(), right.is_finite()) {
        (false, false) => Ordering::Equal,
        (false, true) => Ordering::Greater,
        (true, false) => Ordering::Less,
        (true, true) => {
            if left < right {
                Ordering::Less
            } else if left > right {
                Ordering::Greater
            } else {
                Ordering::Equal
            }
        }
    }
}

/// Admissible lower bound for an interval state.
#[must_use]
pub fn interval_state_key(
    grid: &Grid,
    root: Point2,
    root_g: f64,
    row: f64,
    left: f64,
    right: f64,
    goal: Point2,
) -> f64 {
    if !root_g.is_finite() {
        return f64::INFINITY;
    }
    let Some(best_p) = minimizing_legal_point_on_interval(grid, root, row, left, right, goal)
    else {
        return f64::INFINITY;
    };
    root_g + root.distance_to(best_p) + best_p.distance_to(goal)
}

/// Best point on `[left,right]` at `row` for a final leg to `goal` after reaching `root`.
#[must_use]
pub fn best_interval_goal_candidate(
    grid: &Grid,
    root: Point2,
    root_g: f64,
    row: f64,
    left: f64,
    right: f64,
    goal: Point2,
) -> Option<(Point2, f64)> {
    let best_p = minimizing_legal_point_on_interval(grid, root, row, left, right, goal)?;
    let cost = root_g + root.distance_to(best_p) + best_p.distance_to(goal);
    Some((best_p, cost))
}

/// Maximal open horizontal span from `corner`, clipped to `[clip_left, clip_right]`.
#[must_use]
pub fn visible_flat_span_from_corner(
    grid: &Grid,
    corner: Point2,
    clip_left: f64,
    clip_right: f64,
) -> Option<(f64, f64)> {
    let row_y = corner.y;
    let cx = corner.x.round() as i32;
    let left_bound = clip_left.round() as i32;
    let right_bound = clip_right.round() as i32;

    let mut lo = cx;
    while lo > left_bound {
        let prev = Point2::new((lo - 1) as f64, row_y);
        if !segment_legal(grid, corner, prev) {
            break;
        }
        lo -= 1;
    }

    let mut hi = cx;
    while hi < right_bound {
        let next = Point2::new((hi + 1) as f64, row_y);
        if !segment_legal(grid, corner, next) {
            break;
        }
        hi += 1;
    }

    let left = (lo as f64).max(clip_left);
    let right = (hi as f64).min(clip_right);
    if right + 1e-12 < left {
        None
    } else {
        Some((left, right))
    }
}

fn minimizing_legal_point_on_interval(
    grid: &Grid,
    root: Point2,
    row: f64,
    left: f64,
    right: f64,
    goal: Point2,
) -> Option<Point2> {
    let mut best: Option<Point2> = None;
    let mut best_cost = f64::INFINITY;

    let mut consider = |candidate: Point2| {
        if candidate.x + 1e-12 < left || candidate.x > right + 1e-12 {
            return;
        }
        if !segment_legal(grid, root, candidate) {
            return;
        }
        let cost = point_goal_cost(root, candidate, goal);
        if cost < best_cost {
            best = Some(candidate);
            best_cost = cost;
        }
    };

    consider(Point2::new(left, row));
    consider(Point2::new(right, row));

    // The reflected construction degenerates when either endpoint lies on the
    // interval row.  Retain the projected endpoint positions explicitly: in
    // particular, a state whose root and goal share a row may reach the goal
    // directly, while the interval endpoints alone can be arbitrarily worse.
    if approximately_equal(root.y, row) {
        consider(Point2::new(root.x.clamp(left, right), row));
    }
    if approximately_equal(goal.y, row) {
        consider(Point2::new(goal.x.clamp(left, right), row));
    }

    // Unfold the final leg across this horizontal row.  The stationary point of
    // |root-p| + |p-goal| is where `root -> reflect(goal, row)` crosses the
    // row, not where the unreflected root-goal segment happens to cross it.
    // The latter can overestimate the interval key and invalidate A*'s early
    // termination condition.
    let reflected_goal_y = 2.0 * row - goal.y;
    let dy = reflected_goal_y - root.y;
    if dy.abs() > 1e-15 {
        let t = (row - root.y) / dy;
        let x = root.x + t * (goal.x - root.x);
        if x.is_finite() {
            consider(Point2::new(x.clamp(left, right), row));
        }
    }

    best
}

fn point_goal_cost(root: Point2, point: Point2, goal: Point2) -> f64 {
    root.distance_to(point) + point.distance_to(goal)
}

/// Projects the visibility cone from `root` through `[left,right]` on `row` onto `target_row`.
///
/// When `root` lies on `row`, interval endpoint rays can be horizontal (degenerate for
/// vertical projection). Unconstrained sides use infinities and are clipped later against
/// open runs.
#[must_use]
pub fn project_cone_to_row(
    root: Point2,
    row: f64,
    left: f64,
    right: f64,
    target_row: f64,
) -> Option<(f64, f64)> {
    if approximately_equal(row, target_row) {
        return Some((left, right));
    }

    let x0 = cone_bound_at_row(root, row, left, target_row);
    let x1 = cone_bound_at_row(root, row, right, target_row);
    let lo = x0.min(x1);
    let hi = x0.max(x1);
    if hi + 1e-12 >= lo {
        Some((lo, hi))
    } else {
        None
    }
}

fn cone_bound_at_row(root: Point2, interval_row: f64, endpoint_x: f64, target_row: f64) -> f64 {
    if approximately_equal(interval_row, root.y) && approximately_equal(endpoint_x, root.x) {
        return root.x;
    }

    let endpoint = Point2::new(endpoint_x, interval_row);
    if let Some(x) = ray_intersect_row(root, endpoint, target_row) {
        return x;
    }

    // Endpoint shares the root row but differs in x: horizontal ray does not bound vertical reach.
    if approximately_equal(interval_row, root.y) {
        if target_row > root.y {
            if endpoint_x + 1e-12 < root.x {
                return f64::NEG_INFINITY;
            }
            if endpoint_x > root.x + 1e-12 {
                return f64::INFINITY;
            }
        } else if target_row < root.y {
            if endpoint_x + 1e-12 < root.x {
                return f64::INFINITY;
            }
            if endpoint_x > root.x + 1e-12 {
                return f64::NEG_INFINITY;
            }
        }
    }

    root.x
}

fn ray_intersect_row(origin: Point2, through: Point2, row_y: f64) -> Option<f64> {
    let dy = through.y - origin.y;
    if dy.abs() <= 1e-15 {
        return None;
    }
    let t = (row_y - origin.y) / dy;
    if t < -1e-12 {
        return None;
    }
    Some(origin.x + t * (through.x - origin.x))
}

/// Whether the open segment from `start` to `end` is legal under Anya sampling LOS.
#[must_use]
pub fn segment_legal(grid: &Grid, start: Point2, end: Point2) -> bool {
    sampling_segment_is_legal(grid, start, end)
}

/// Whether every consecutive polyline segment is legal under Anya sampling LOS.
#[must_use]
pub fn validate_path(grid: &Grid, points: &[Point2]) -> bool {
    validate_sampling_path(grid, points)
}

/// Canonicalize and validate request endpoints.
pub fn parse_request(
    grid: &Grid,
    start: Point2,
    goal: Point2,
) -> Result<(Point2, Point2), crate::AnyAngleSearchError> {
    let Some(start) = canonicalize_grid_vertex(start) else {
        return Err(crate::AnyAngleSearchError::InvalidStart { point: start });
    };
    let Some(goal) = canonicalize_grid_vertex(goal) else {
        return Err(crate::AnyAngleSearchError::InvalidGoal { point: goal });
    };
    if !is_endpoint_valid(grid, start) {
        return Err(crate::AnyAngleSearchError::InvalidStart { point: start });
    }
    if !is_endpoint_valid(grid, goal) {
        return Err(crate::AnyAngleSearchError::InvalidGoal { point: goal });
    }
    Ok((start, goal))
}

/// Whether a grid vertex can anchor a turning successor.
///
/// Retains every mixed-mask vertex (including pinch-adjacent corners). Legality
/// is enforced separately via segment checks.
#[must_use]
pub fn is_turn_candidate(grid: &Grid, vx: i32, vy: i32) -> bool {
    retain_as_corner(grid, vx, vy)
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::{Grid, grid::Cell, point::Point};
    use condor_core::Point2;

    #[test]
    fn project_cone_from_root_on_row_reaches_adjacent_rows() {
        let root = Point2::new(0.0, 0.0);
        let projected = project_cone_to_row(root, 0.0, 0.0, 63.0, 1.0).expect("projection");
        assert!(projected.0 <= 0.0 + 1e-9);
        assert!(projected.1 >= 63.0 - 1e-9);
    }

    #[test]
    fn checkerboard_direct_diagonal_is_illegal() {
        let mut grid = Grid::new(5, 5).expect("grid");
        for (x, y) in [
            (1, 0),
            (3, 0),
            (0, 1),
            (2, 1),
            (4, 1),
            (1, 2),
            (3, 2),
            (0, 3),
            (2, 3),
            (4, 3),
            (1, 4),
            (3, 4),
        ] {
            grid.set_cell(Point::new(x, y), Cell::Blocked)
                .expect("block");
        }
        assert!(!segment_legal(
            &grid,
            Point2::new(0.0, 0.0),
            Point2::new(4.0, 4.0)
        ));
    }

    #[test]
    fn staircase_concave_witness_segments_are_legal() {
        let mut grid = Grid::new(6, 4).expect("grid");
        for (x, y) in [
            (1, 0),
            (2, 0),
            (2, 1),
            (3, 1),
            (4, 1),
            (4, 2),
            (5, 2),
            (5, 3),
        ] {
            grid.set_cell(Point::new(x, y), Cell::Blocked)
                .expect("block");
        }
        let pts = [
            Point2::new(0.0, 3.0),
            Point2::new(2.0, 1.0),
            Point2::new(3.0, 1.0),
            Point2::new(5.0, 0.0),
        ];
        for pair in pts.windows(2) {
            assert!(
                segment_legal(&grid, pair[0], pair[1]),
                "{:?} -> {:?}",
                pair[0],
                pair[1]
            );
        }
        assert!(is_turn_candidate(&grid, 2, 1));
        assert!(is_turn_candidate(&grid, 3, 1));
        assert!(segment_legal(
            &grid,
            Point2::new(0.0, 3.0),
            Point2::new(2.0, 1.0)
        ));
    }

    #[test]
    fn forbidden_pinch_corner_geometry() {
        let mut grid = Grid::new(4, 4).expect("grid");
        grid.set_cell(Point::new(1, 1), Cell::Blocked)
            .expect("block");
        grid.set_cell(Point::new(2, 2), Cell::Blocked)
            .expect("block");

        assert!(segment_legal(
            &grid,
            Point2::new(0.0, 0.0),
            Point2::new(1.0, 2.0)
        ));
        assert!(segment_legal(
            &grid,
            Point2::new(1.0, 2.0),
            Point2::new(2.0, 3.0)
        ));
        assert!(is_turn_candidate(&grid, 1, 2));

        let key = interval_state_key(
            &grid,
            Point2::new(1.0, 2.0),
            2.0_f64.sqrt(),
            2.0,
            1.0,
            3.0,
            Point2::new(3.0, 3.0),
        );
        assert!(
            key < 5.0,
            "turn key should beat suboptimal detour, got {key}"
        );
    }
}