rustsim_crowd/

broadphase.rs

//! Broadphase acceleration for crowd models.
//!
//! Every force-based or velocity-based model in this crate is
//! fundamentally a pair-wise interaction: pedestrian *i* only needs to
//! consider pedestrians *j* within a small cutoff radius where the
//! interaction term is non-negligible. The naïve double loop is
//! `O(n^2)`; wrapping `rustsim-broadphase::UniformGrid2` around it
//! turns every `step` into `O(n + n * k)` where `k` is the average
//! per-agent neighbour count, which is typically 5–20 for realistic
//! crowd densities.
//!
//! Each model picks its own cutoff through the function below:
//!
//! - [`NeighborGrid::rebuild`] drops the previous frame and re-inserts
//!   every pedestrian by its current position. Callers own the grid,
//!   so one allocation per *simulation*, not per *tick*.
//! - [`NeighborGrid::for_each_neighbor`] hands the caller `(j, pos_j,
//!   ped_j)` for every pedestrian inside the cutoff radius of a source
//!   position, skipping the source index itself.
//!
//! The grid keys are `u64` agent indices cast from `usize`. Pedestrian
//! slices must fit in `u64::MAX` — trivially satisfied in practice.

use rustsim_broadphase::UniformGrid2;

use crate::common::{Pedestrian, Vec2};

/// Per-tick neighbour grid for the crowd models.
///
/// Typical usage:
///
/// ```ignore
/// let mut grid = NeighborGrid::new(1.0);
/// for _step in 0..num_steps {
///     grid.rebuild(&peds);
///     social_force::step_with_grid(&mut peds, &walls, &params, dt, &grid);
/// }
/// ```
#[derive(Debug, Clone)]
pub struct NeighborGrid {
    grid: UniformGrid2,
}

impl NeighborGrid {
    /// Construct a grid with the given cell size (metres).
    ///
    /// Choose `cell_size` close to the dominant model cutoff — e.g.
    /// `1.0` m for Social Force with default `b_ped`, `3.0` m for
    /// Optimal Steps with default `ped_range`, or the output of
    /// [`recommended_cell_size`].
    ///
    /// # Panics
    /// Panics if `cell_size` is not strictly positive.
    pub fn new(cell_size: f64) -> Self {
        Self {
            grid: UniformGrid2::new(cell_size),
        }
    }

    /// Drop every entry.
    #[inline]
    pub fn clear(&mut self) {
        self.grid.clear();
    }

    /// Clear the grid and re-insert every pedestrian keyed by its
    /// slice index cast to `u64`.
    pub fn rebuild(&mut self, peds: &[Pedestrian]) {
        self.grid.clear();
        for (i, p) in peds.iter().enumerate() {
            self.grid.insert(i as u64, p.pos);
        }
    }

    /// Invoke `f(j, pos_j, ped_j)` for every pedestrian within
    /// `radius` of `pos_i`, skipping index `i`.
    ///
    /// `peds` must be the same slice that was passed to
    /// [`rebuild`](Self::rebuild); the grid stores only indices and
    /// positions, so the caller provides the slice for the actual
    /// pedestrian record lookup.
    #[inline]
    pub fn for_each_neighbor<F>(&self, i: usize, radius: f64, peds: &[Pedestrian], mut f: F)
    where
        F: FnMut(usize, &Pedestrian),
    {
        let pos_i = peds[i].pos;
        self.grid.for_each_within(pos_i, radius, |k, _pos_j| {
            let j = k as usize;
            if j == i {
                return;
            }
            if j >= peds.len() {
                // Stale entry from a previous rebuild that spanned a
                // larger slice — defensive guard, never triggered in
                // the intended `rebuild → step` sequence.
                return;
            }
            f(j, &peds[j]);
        });
    }

    /// Underlying grid, for diagnostics or custom queries.
    #[inline]
    pub fn inner(&self) -> &UniformGrid2 {
        &self.grid
    }
}

/// Suggested cell size for a given interaction cutoff radius.
///
/// Uniform hash-grids prune best when the cell edge matches the query
/// radius; anything smaller wastes cells, anything larger wastes
/// comparisons. Returns `radius` clamped to at least `0.1` m so
/// pathological inputs never produce zero-sized cells.
#[inline]
pub fn recommended_cell_size(radius: f64) -> f64 {
    radius.max(0.1)
}

/// Reusable scratch buffers for the crowd models.
///
/// Every `step_*` path in this crate needs exactly one `Vec<Vec2>` of
/// length `peds.len()` (for next-tick accelerations, velocities, or
/// positions) and one [`NeighborGrid`] rebuilt against the current
/// pedestrian slice. `Scratch` bundles both so a caller can allocate
/// **once per simulation** instead of twice per tick.
///
/// ```ignore
/// let mut scratch = Scratch::with_capacity(peds.len(), 1.0);
/// for _ in 0..num_steps {
///     social_force::step_scratch(&mut peds, &walls, &params, dt, &mut scratch);
/// }
/// ```
///
/// `Scratch` is not tied to a specific model: the same instance can
/// drive `social_force`, `collision_free_speed`, `anticipation_velocity`,
/// `generalized_centrifugal_force`, or `optimal_steps` step-by-step,
/// provided the grid's `cell_size` is a sensible cap on the largest
/// [`neighbor_cutoff`](crate::social_force::neighbor_cutoff) in play.
#[derive(Debug, Clone)]
pub struct Scratch {
    /// Per-agent working buffer (re-used as accelerations, velocities,
    /// or next-step positions depending on the caller).
    pub(crate) buf: Vec<Vec2>,
    /// Neighbour grid rebuilt against the pedestrian slice each tick.
    pub grid: NeighborGrid,
}

impl Scratch {
    /// Empty scratch with a grid of the given cell size (metres).
    ///
    /// # Panics
    /// Panics if `cell_size` is not strictly positive.
    #[inline]
    pub fn new(cell_size: f64) -> Self {
        Self {
            buf: Vec::new(),
            grid: NeighborGrid::new(cell_size),
        }
    }

    /// Scratch pre-sized for `n` pedestrians and a grid of `cell_size`.
    #[inline]
    pub fn with_capacity(n: usize, cell_size: f64) -> Self {
        Self {
            buf: Vec::with_capacity(n),
            grid: NeighborGrid::new(cell_size),
        }
    }

    /// Clear the buffers without releasing their capacity.
    #[inline]
    pub fn clear(&mut self) {
        self.buf.clear();
        self.grid.clear();
    }

    /// Resize `buf` to length `n` (zero-filled) without freeing capacity
    /// and rebuild `grid` against `peds`. Called by every `step_scratch`.
    #[inline]
    pub(crate) fn prepare(&mut self, peds: &[Pedestrian]) {
        let n = peds.len();
        self.buf.clear();
        self.buf.resize(n, [0.0, 0.0]);
        self.grid.rebuild(peds);
    }

    /// Split-borrow the scratch buffer and the read-only grid.
    #[inline]
    pub(crate) fn split(&mut self) -> (&mut [Vec2], &NeighborGrid) {
        (&mut self.buf, &self.grid)
    }
}

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

    fn ped_at(x: f64, y: f64) -> Pedestrian {
        Pedestrian {
            pos: [x, y],
            vel: [0.0, 0.0],
            radius: 0.25,
            desired_speed: 1.34,
            destination: [x + 10.0, y],
        }
    }

    #[test]
    fn rebuild_and_query_returns_only_nearby() {
        let peds = vec![
            ped_at(0.0, 0.0),
            ped_at(0.3, 0.0),
            ped_at(5.0, 0.0),
            ped_at(0.0, 0.5),
        ];
        let mut grid = NeighborGrid::new(1.0);
        grid.rebuild(&peds);
        let mut hits = Vec::new();
        grid.for_each_neighbor(0, 1.0, &peds, |j, _| hits.push(j));
        hits.sort();
        assert_eq!(hits, vec![1, 3]);
    }

    #[test]
    fn skips_self_index() {
        let peds = vec![ped_at(0.0, 0.0), ped_at(0.1, 0.0)];
        let mut grid = NeighborGrid::new(1.0);
        grid.rebuild(&peds);
        let mut hits = Vec::new();
        grid.for_each_neighbor(0, 5.0, &peds, |j, _| hits.push(j));
        assert_eq!(hits, vec![1]);
    }

    #[test]
    fn empty_slice_is_safe() {
        let peds: Vec<Pedestrian> = Vec::new();
        let mut grid = NeighborGrid::new(1.0);
        grid.rebuild(&peds);
        // No assertion needed — just must not panic.
    }

    #[test]
    fn scratch_reuses_capacity_across_ticks() {
        let peds = vec![ped_at(0.0, 0.0), ped_at(0.5, 0.0), ped_at(1.0, 0.0)];
        let mut scratch = Scratch::with_capacity(3, 1.0);
        scratch.prepare(&peds);
        assert_eq!(scratch.buf.len(), 3);
        let cap_after_first = scratch.buf.capacity();
        // Second tick must not re-allocate.
        scratch.prepare(&peds);
        assert_eq!(scratch.buf.len(), 3);
        assert_eq!(scratch.buf.capacity(), cap_after_first);
    }
}