condor_grid/algorithms/

jps_plus.rs

//! Prepared-grid [`PreparedGridSearch`]: cardinal JPS+.
//!
//! [`JpsPlusBuilder`] builds a durable per-direction jump table; [`PreparedJpsPlus`]
//! answers repeated requests with the standard invalid/found/no-path outcome without
//! rescanning rays. Jump-edge cost sums per-cell [`traversal_cost`](crate::Grid::traversal_cost)
//! with a Manhattan heuristic. Prefer it for a static repeated-query map; use
//! [`super::jump_point_search::JumpPointSearch`] for one-shot or changing maps.

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

use crate::{
    Grid, Path, Point,
    algorithms::jps_cardinal::{
        Direction, PointKind, classify_point_kind, manhattan_distance, reconstruct_jump_path, step,
    },
    preprocessed_grid::{
        PreparedGridSearch, PreprocessedGridBuildError, PreprocessedGridBuilder,
        PreprocessedGridMetadata, metadata_for_grid,
    },
    search::{SearchRequest, SearchResult},
};

/// [`PreprocessedGridBuilder`] for cardinal JPS+.
///
/// Builds a durable jump table so queries avoid rescanning rays. Weighted cost model
/// matches online [`super::jump_point_search::JumpPointSearch`]. Prefer for multi-query
/// static grids; online JPS for one-shot or frequently edited maps.
#[derive(Debug, Clone, Copy, Default)]
pub struct JpsPlusBuilder;

impl JpsPlusBuilder {
    /// Creates a JPS+ builder (no configuration knobs in v0).
    #[must_use]
    pub const fn new() -> Self {
        Self
    }
}

impl PreprocessedGridBuilder for JpsPlusBuilder {
    type Map = PreparedJpsPlus;

    fn name(&self) -> &'static str {
        "jps-plus"
    }

    fn preprocess(&self, grid: &Grid) -> Result<Self::Map, PreprocessedGridBuildError> {
        let cell_count = grid.cell_count();
        let mut jumps = vec![None; cell_count * 4];

        for index in 0..cell_count {
            let point = grid.point_from_index(index);
            if !grid.is_walkable(point) {
                continue;
            }
            for (dir_i, direction) in Direction::ALL.iter().enumerate() {
                jumps[index * 4 + dir_i] = precompute_jump(grid, point, index, *direction);
            }
        }

        Ok(PreparedJpsPlus {
            grid: grid.clone(),
            jumps,
            metadata: metadata_for_grid(grid, "jps-plus", "jps-plus"),
        })
    }
}

/// Prepared map implementing [`PreparedGridSearch`] for JPS+.
///
/// Immutable after preprocess. Path costs match online
/// [`JumpPointSearch`](crate::JumpPointSearch) on 4-way weighted packs.
#[derive(Debug, Clone)]
pub struct PreparedJpsPlus {
    grid: Grid,
    /// `jumps[index * 4 + dir]` = jump stop from this cell in that direction.
    jumps: Vec<Option<JumpEdge>>,
    metadata: PreprocessedGridMetadata,
}

impl PreparedJpsPlus {
    /// Returns a [`JpsPlusBuilder`] for preprocess entry.
    #[must_use]
    pub fn builder() -> JpsPlusBuilder {
        JpsPlusBuilder::new()
    }
}

impl PreparedGridSearch for PreparedJpsPlus {
    fn name(&self) -> &'static str {
        "jps-plus"
    }

    fn grid(&self) -> &Grid {
        &self.grid
    }

    fn metadata(&self) -> &PreprocessedGridMetadata {
        &self.metadata
    }

    fn search(&self, request: SearchRequest) -> SearchResult {
        crate::search::validate_request(&self.grid, request)?;
        search_with_tables(&self.grid, &self.jumps, request)
    }
}

#[derive(Debug, Clone, Copy)]
struct JumpEdge {
    target_index: usize,
    edge_cost: usize,
    edge_weight: usize,
}

fn precompute_jump(
    grid: &Grid,
    start: Point,
    start_index: usize,
    direction: Direction,
) -> Option<JumpEdge> {
    let mut point = start;
    let mut index = start_index;
    let mut edge_cost = 0usize;
    let mut edge_weight = 0usize;

    loop {
        let (next_point, next_index) = step(grid, point, index, direction)?;
        point = next_point;
        index = next_index;
        edge_cost = edge_cost.checked_add(1)?;
        edge_weight = edge_weight.checked_add(grid.traversal_cost(point).unwrap_or(1))?;

        let point_kind = classify_point_kind(grid, point);
        if point_kind != PointKind::StraightCorridor {
            return Some(JumpEdge {
                target_index: index,
                edge_cost,
                edge_weight,
            });
        }
    }
}

fn search_with_tables(
    grid: &Grid,
    jumps: &[Option<JumpEdge>],
    request: SearchRequest,
) -> SearchResult {
    let Some(start_index) = grid.index_of(request.start) else {
        return crate::search::not_found(0);
    };
    let Some(goal_index) = grid.index_of(request.goal) else {
        return crate::search::not_found(0);
    };
    if !grid.is_walkable(request.start) || !grid.is_walkable(request.goal) {
        return crate::search::not_found(0);
    }
    if !grid.is_reachable(request.start, request.goal) {
        return crate::search::not_found(0);
    }
    if request.start == request.goal {
        return crate::search::found(
            Path::from_steps(vec![request.start]).expect("path contains at least one point"),
            1,
        );
    }

    let initial_heuristic = manhattan_distance(request.start, request.goal);
    let mut frontier = BinaryHeap::from([FrontierEntry {
        estimated_total_cost: initial_heuristic,
        heuristic_cost: initial_heuristic,
        cost_so_far: 0,
        index: start_index,
    }]);
    let mut best_costs = vec![None; grid.cell_count()];
    let mut parents = vec![None; grid.cell_count()];
    let mut visited_nodes = 0usize;
    let watch = crate::search::BudgetWatch::start(request.budget);
    best_costs[start_index] = Some(0);

    while let Some(entry) = frontier.pop() {
        if best_costs[entry.index] != Some(entry.cost_so_far) {
            continue;
        }
        visited_nodes += 1;
        if entry.index == goal_index {
            break;
        }

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

        let current = grid.point_from_index(entry.index);
        for (dir_i, direction) in Direction::ALL.iter().enumerate() {
            let Some(edge) = resolve_jump(
                grid,
                jumps,
                current,
                entry.index,
                *direction,
                dir_i,
                goal_index,
            ) else {
                continue;
            };
            let Some(next_cost) = entry.cost_so_far.checked_add(edge.edge_weight) else {
                continue;
            };
            if best_costs[edge.target_index].is_some_and(|best| next_cost >= best) {
                continue;
            }
            best_costs[edge.target_index] = Some(next_cost);
            parents[edge.target_index] = Some(entry.index);
            let target = grid.point_from_index(edge.target_index);
            let heuristic_cost = manhattan_distance(target, request.goal);
            frontier.push(FrontierEntry {
                estimated_total_cost: next_cost.saturating_add(heuristic_cost),
                heuristic_cost,
                cost_so_far: next_cost,
                index: edge.target_index,
            });
        }
    }

    if let Some(goal_cost) = best_costs[goal_index] {
        crate::search::found(
            reconstruct_jump_path(grid, &parents, start_index, goal_index, goal_cost),
            visited_nodes,
        )
    } else {
        crate::search::not_found(visited_nodes)
    }
}

/// Use precomputed jump, but stop at the goal if it lies on the ray first.
fn resolve_jump(
    grid: &Grid,
    jumps: &[Option<JumpEdge>],
    start: Point,
    start_index: usize,
    direction: Direction,
    dir_i: usize,
    goal_index: usize,
) -> Option<JumpEdge> {
    // Prefer a shorter on-ray goal jump over the precomputed primary stop.
    if let Some(to_goal) = jump_toward_goal(grid, start, start_index, direction, goal_index) {
        if let Some(pre) = jumps[start_index * 4 + dir_i] {
            if to_goal.edge_cost <= pre.edge_cost {
                return Some(to_goal);
            }
            return Some(pre);
        }
        return Some(to_goal);
    }
    jumps[start_index * 4 + dir_i]
}

fn jump_toward_goal(
    grid: &Grid,
    start: Point,
    start_index: usize,
    direction: Direction,
    goal_index: usize,
) -> Option<JumpEdge> {
    let goal = grid.point_from_index(goal_index);
    let aligned = match direction {
        Direction::Left => start.y == goal.y && goal.x < start.x,
        Direction::Right => start.y == goal.y && goal.x > start.x,
        Direction::Up => start.x == goal.x && goal.y < start.y,
        Direction::Down => start.x == goal.x && goal.y > start.y,
    };
    if !aligned {
        return None;
    }

    let mut point = start;
    let mut index = start_index;
    let mut edge_cost = 0usize;
    let mut edge_weight = 0usize;
    loop {
        let (next_point, next_index) = step(grid, point, index, direction)?;
        point = next_point;
        index = next_index;
        edge_cost = edge_cost.checked_add(1)?;
        edge_weight = edge_weight.checked_add(grid.traversal_cost(point).unwrap_or(1))?;
        if index == goal_index {
            return Some(JumpEdge {
                target_index: index,
                edge_cost,
                edge_weight,
            });
        }
    }
}

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
struct FrontierEntry {
    estimated_total_cost: usize,
    heuristic_cost: usize,
    cost_so_far: usize,
    index: usize,
}

impl Ord for FrontierEntry {
    fn cmp(&self, other: &Self) -> Ordering {
        other
            .estimated_total_cost
            .cmp(&self.estimated_total_cost)
            .then_with(|| other.heuristic_cost.cmp(&self.heuristic_cost))
            .then_with(|| self.cost_so_far.cmp(&other.cost_so_far))
            .then_with(|| other.index.cmp(&self.index))
    }
}

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

#[cfg(test)]
mod tests {
    use super::*;
    use crate::{AStar, JumpPointSearch, Pathfinder, SearchRequest};

    #[test]
    fn jps_plus_matches_astar_and_online_jps_on_open_field() {
        let grid = Grid::new(32, 32).unwrap();
        let prepared = JpsPlusBuilder::new().preprocess(&grid).unwrap();
        let request = SearchRequest::new(Point::new(0, 0), Point::new(31, 31));
        let plus = prepared.search(request);
        let jps = JumpPointSearch.search(&grid, request);
        let astar = AStar.search(&grid, request);
        assert!(
            plus.as_ref().expect("valid search request").is_found()
                && jps.as_ref().expect("valid search request").is_found()
                && astar.as_ref().expect("valid search request").is_found()
        );
        assert_eq!(
            plus.as_ref().expect("valid search request").cost(),
            astar.as_ref().expect("valid search request").cost()
        );
        assert_eq!(
            plus.as_ref().expect("valid search request").cost(),
            jps.as_ref().expect("valid search request").cost()
        );
        assert_eq!(prepared.metadata().builder_name, "jps-plus");
        assert_eq!(PreparedJpsPlus::builder().name(), "jps-plus");
    }

    #[test]
    fn jps_plus_matches_astar_and_online_jps_on_weighted_corridor() {
        use crate::Cell;

        let mut grid = Grid::new(8, 3).expect("grid dimensions are valid");
        for x in 0..8 {
            grid.set_cell(Point::new(x, 0), Cell::Blocked)
                .expect("top corridor wall should be valid");
            grid.set_cell(Point::new(x, 2), Cell::Blocked)
                .expect("bottom corridor wall should be valid");
        }
        grid.set_traversal_cost(Point::new(3, 1), 5)
            .expect("weighted corridor cost should be valid");

        let prepared = JpsPlusBuilder::new()
            .preprocess(&grid)
            .expect("weighted grid should preprocess");
        let request = SearchRequest::new(Point::new(0, 1), Point::new(7, 1));
        let plus = prepared.search(request).expect("valid search request");
        let astar = AStar.search(&grid, request).expect("valid search request");
        let jps = JumpPointSearch
            .search(&grid, request)
            .expect("valid search request");

        assert!(plus.is_found());
        assert_eq!(
            plus.path().map(|path| path.cost()),
            astar.path().map(|path| path.cost())
        );
        assert_eq!(
            plus.path().map(|path| path.cost()),
            jps.path().map(|path| path.cost())
        );
    }
}