rustsim_pathfinding/

walkability.rs

//! Walkability query utilities for pathfinding.
//!
//! Convenience functions that combine spatial neighbor queries with
//! walkability filtering. Mirrors Agents.jl `Pathfinding.nearby_walkable`
//! and `Pathfinding.random_walkable`.
//!
//! These functions operate on raw grid data (walkability arrays and
//! dimensions) rather than depending on specific space types, keeping
//! the pathfinding crate independent.

use rand::Rng;

/// Returns all walkable grid cells within Chebyshev radius `r` of `pos`.
///
/// Excludes `pos` itself. Supports periodic boundaries.
///
/// # Example
///
/// ```
/// use rustsim_pathfinding::walkability::nearby_walkable;
///
/// let walkmap = vec![true; 25]; // 5x5, all walkable
/// let neighbors = nearby_walkable((2, 2), 1, &walkmap, 5, 5, false);
/// assert_eq!(neighbors.len(), 8); // 8 neighbors of center cell
/// ```
pub fn nearby_walkable(
    pos: (usize, usize),
    radius: usize,
    walkmap: &[bool],
    width: usize,
    height: usize,
    periodic: bool,
) -> Vec<(usize, usize)> {
    let r = radius as i32;
    let (cx, cy) = (pos.0 as i32, pos.1 as i32);
    let mut result = Vec::new();

    for dy in -r..=r {
        for dx in -r..=r {
            if dx == 0 && dy == 0 {
                continue;
            }
            let nx = cx + dx;
            let ny = cy + dy;

            let cell = if periodic {
                let px = ((nx % width as i32) + width as i32) % width as i32;
                let py = ((ny % height as i32) + height as i32) % height as i32;
                Some((px as usize, py as usize))
            } else if nx >= 0 && ny >= 0 && (nx as usize) < width && (ny as usize) < height {
                Some((nx as usize, ny as usize))
            } else {
                None
            };

            if let Some((x, y)) = cell {
                if walkmap[y * width + x] {
                    result.push((x, y));
                }
            }
        }
    }

    result
}

/// Returns a random walkable grid cell, or `None` if none exist.
///
/// Uniformly samples from all walkable cells.
///
/// # Example
///
/// ```
/// use rustsim_pathfinding::walkability::random_walkable;
/// use rand::rngs::StdRng;
/// use rand::SeedableRng;
///
/// let walkmap = vec![true; 25];
/// let mut rng = StdRng::seed_from_u64(42);
/// let pos = random_walkable(&walkmap, 5, 5, &mut rng);
/// assert!(pos.is_some());
/// ```
pub fn random_walkable<R: Rng>(
    walkmap: &[bool],
    width: usize,
    _height: usize,
    rng: &mut R,
) -> Option<(usize, usize)> {
    let walkable_cells: Vec<usize> = walkmap
        .iter()
        .enumerate()
        .filter(|(_, &w)| w)
        .map(|(i, _)| i)
        .collect();

    if walkable_cells.is_empty() {
        return None;
    }

    let idx = walkable_cells[rng.gen_range(0..walkable_cells.len())];
    let x = idx % width;
    let y = idx / width;
    Some((x, y))
}

/// Returns a random walkable grid cell within Chebyshev radius `r` of `pos`.
///
/// Returns `None` if no walkable cells exist in the neighborhood.
///
/// # Example
///
/// ```
/// use rustsim_pathfinding::walkability::random_walkable_nearby;
/// use rand::rngs::StdRng;
/// use rand::SeedableRng;
///
/// let walkmap = vec![true; 25];
/// let mut rng = StdRng::seed_from_u64(42);
/// let pos = random_walkable_nearby((2, 2), 1, &walkmap, 5, 5, false, &mut rng);
/// assert!(pos.is_some());
/// ```
pub fn random_walkable_nearby<R: Rng>(
    pos: (usize, usize),
    radius: usize,
    walkmap: &[bool],
    width: usize,
    height: usize,
    periodic: bool,
    rng: &mut R,
) -> Option<(usize, usize)> {
    let candidates = nearby_walkable(pos, radius, walkmap, width, height, periodic);
    if candidates.is_empty() {
        return None;
    }
    Some(candidates[rng.gen_range(0..candidates.len())])
}

/// Returns a random walkable continuous position within the space.
///
/// Picks a random walkable grid cell, then returns a random position
/// within that cell's continuous extent.
///
/// Returns `None` if no walkable cells exist.
pub fn random_walkable_continuous<R: Rng>(
    walkmap: &[bool],
    grid_w: usize,
    grid_h: usize,
    extent_x: f64,
    extent_y: f64,
    rng: &mut R,
) -> Option<(f64, f64)> {
    let cell = random_walkable(walkmap, grid_w, grid_h, rng)?;
    let cell_w = extent_x / grid_w as f64;
    let cell_h = extent_y / grid_h as f64;
    let x = cell.0 as f64 * cell_w + rng.gen_range(0.0..cell_w);
    let y = cell.1 as f64 * cell_h + rng.gen_range(0.0..cell_h);
    Some((x, y))
}

/// Returns a random walkable continuous position within Euclidean radius
/// `r` of `pos`.
///
/// Finds walkable grid cells near `pos`, picks one randomly, then returns
/// a random continuous position within that cell that is within `r` of `pos`.
///
/// Returns `None` if no walkable cells exist nearby.
#[allow(clippy::too_many_arguments)]
pub fn random_walkable_continuous_nearby<R: Rng>(
    pos: (f64, f64),
    radius: f64,
    walkmap: &[bool],
    grid_w: usize,
    grid_h: usize,
    extent_x: f64,
    extent_y: f64,
    periodic: bool,
    rng: &mut R,
) -> Option<(f64, f64)> {
    let cell_w = extent_x / grid_w as f64;
    let cell_h = extent_y / grid_h as f64;

    // Convert continuous position to grid cell
    let cx = ((pos.0 / extent_x * grid_w as f64).floor() as usize).min(grid_w - 1);
    let cy = ((pos.1 / extent_y * grid_h as f64).floor() as usize).min(grid_h - 1);

    // Grid radius that covers the continuous radius
    let grid_r = ((radius / cell_w.min(cell_h)).ceil() as usize).max(1);

    let candidates = nearby_walkable((cx, cy), grid_r, walkmap, grid_w, grid_h, periodic);

    if candidates.is_empty() {
        return None;
    }

    // Try up to 100 times to find a position within Euclidean radius
    for _ in 0..100 {
        let cell = candidates[rng.gen_range(0..candidates.len())];
        let x = cell.0 as f64 * cell_w + rng.gen_range(0.0..cell_w);
        let y = cell.1 as f64 * cell_h + rng.gen_range(0.0..cell_h);

        let mut dx = (x - pos.0).abs();
        let mut dy = (y - pos.1).abs();
        if periodic {
            if dx > extent_x * 0.5 {
                dx = extent_x - dx;
            }
            if dy > extent_y * 0.5 {
                dy = extent_y - dy;
            }
        }
        let dist = (dx * dx + dy * dy).sqrt();
        if dist <= radius {
            return Some((x, y));
        }
    }

    None
}

#[cfg(test)]
mod tests {
    use super::*;
    use rand::rngs::StdRng;
    use rand::SeedableRng;

    #[test]
    fn nearby_walkable_all_open() {
        let walkmap = vec![true; 25]; // 5x5
        let result = nearby_walkable((2, 2), 1, &walkmap, 5, 5, false);
        assert_eq!(result.len(), 8); // 8 neighbors
    }

    #[test]
    fn nearby_walkable_with_walls() {
        let mut walkmap = vec![true; 25];
        walkmap[5 + 2] = false; // (2,1) blocked
        walkmap[3 * 5 + 2] = false; // (2,3) blocked
        let result = nearby_walkable((2, 2), 1, &walkmap, 5, 5, false);
        assert_eq!(result.len(), 6);
    }

    #[test]
    fn nearby_walkable_corner_periodic() {
        let walkmap = vec![true; 25]; // 5x5
        let result = nearby_walkable((0, 0), 1, &walkmap, 5, 5, true);
        assert_eq!(result.len(), 8); // wraps around
    }

    #[test]
    fn nearby_walkable_corner_bounded() {
        let walkmap = vec![true; 25]; // 5x5
        let result = nearby_walkable((0, 0), 1, &walkmap, 5, 5, false);
        assert_eq!(result.len(), 3); // only (1,0), (0,1), (1,1)
    }

    #[test]
    fn random_walkable_basic() {
        let walkmap = vec![true; 25];
        let mut rng = StdRng::seed_from_u64(42);
        let pos = random_walkable(&walkmap, 5, 5, &mut rng);
        assert!(pos.is_some());
        let (x, y) = pos.unwrap();
        assert!(x < 5 && y < 5);
    }

    #[test]
    fn random_walkable_none() {
        let walkmap = vec![false; 25];
        let mut rng = StdRng::seed_from_u64(42);
        let pos = random_walkable(&walkmap, 5, 5, &mut rng);
        assert!(pos.is_none());
    }

    #[test]
    fn random_walkable_continuous_basic() {
        let walkmap = vec![true; 100];
        let mut rng = StdRng::seed_from_u64(42);
        let pos = random_walkable_continuous(&walkmap, 10, 10, 100.0, 100.0, &mut rng);
        assert!(pos.is_some());
        let (x, y) = pos.unwrap();
        assert!((0.0..100.0).contains(&x));
        assert!((0.0..100.0).contains(&y));
    }
}