rustsim-broadphase 0.0.1

//! 3-D uniform hash-grid broadphase.

use hashbrown::HashMap;
use rustsim_geometry::vec3::{self, Vec3};

/// Uniform hash-grid indexing points in 3-D by cell coordinate.
#[derive(Debug, Clone)]
pub struct UniformGrid3 {
    /// Cell edge length (metres).
    pub cell_size: f64,
    inv_cell: f64,
    cells: HashMap<(i32, i32, i32), Vec<(u64, Vec3)>>,
}

impl UniformGrid3 {
    /// Construct a grid with the given cell size (metres).
    ///
    /// # Panics
    /// Panics if `cell_size` is not strictly positive.
    pub fn new(cell_size: f64) -> Self {
        assert!(cell_size > 0.0, "cell_size must be positive");
        Self {
            cell_size,
            inv_cell: 1.0 / cell_size,
            cells: HashMap::new(),
        }
    }

    /// Remove every entry.
    pub fn clear(&mut self) {
        self.cells.clear();
    }

    /// Insert `key` at `pos`.
    pub fn insert(&mut self, key: u64, pos: Vec3) {
        let cell = self.cell_of(pos);
        self.cells.entry(cell).or_default().push((key, pos));
    }

    /// Insert many `(key, pos)` pairs.
    pub fn insert_all<I: IntoIterator<Item = (u64, Vec3)>>(&mut self, iter: I) {
        for (k, p) in iter {
            self.insert(k, p);
        }
    }

    /// Return all keys within Euclidean distance `radius` of `center`.
    pub fn query_radius(&self, center: Vec3, radius: f64) -> Vec<u64> {
        let r2 = radius * radius;
        let mut out = Vec::new();
        for cell in self.cells_within(center, radius) {
            if let Some(bucket) = self.cells.get(&cell) {
                for (k, p) in bucket {
                    if vec3::norm_squared(vec3::sub(*p, center)) <= r2 {
                        out.push(*k);
                    }
                }
            }
        }
        out
    }

    /// Invoke `f(key, pos)` for every key within `radius` of `center`.
    pub fn for_each_within<F: FnMut(u64, Vec3)>(&self, center: Vec3, radius: f64, mut f: F) {
        let r2 = radius * radius;
        for cell in self.cells_within(center, radius) {
            if let Some(bucket) = self.cells.get(&cell) {
                for (k, p) in bucket {
                    if vec3::norm_squared(vec3::sub(*p, center)) <= r2 {
                        f(*k, *p);
                    }
                }
            }
        }
    }

    /// Number of entries currently stored.
    pub fn len(&self) -> usize {
        self.cells.values().map(|v| v.len()).sum()
    }

    /// True if the grid is empty.
    pub fn is_empty(&self) -> bool {
        self.cells.values().all(|v| v.is_empty())
    }

    #[inline]
    fn cell_of(&self, p: Vec3) -> (i32, i32, i32) {
        (
            (p[0] * self.inv_cell).floor() as i32,
            (p[1] * self.inv_cell).floor() as i32,
            (p[2] * self.inv_cell).floor() as i32,
        )
    }

    fn cells_within(&self, center: Vec3, radius: f64) -> Vec<(i32, i32, i32)> {
        let (cx, cy, cz) = self.cell_of(center);
        let r = (radius * self.inv_cell).ceil() as i32;
        let side = 2 * r + 1;
        let mut out = Vec::with_capacity((side * side * side) as usize);
        for dx in -r..=r {
            for dy in -r..=r {
                for dz in -r..=r {
                    out.push((cx + dx, cy + dy, cz + dz));
                }
            }
        }
        out
    }
}

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

    #[test]
    fn query_radius_respects_z_separation() {
        let mut g = UniformGrid3::new(1.0);
        g.insert(1, [0.0, 0.0, 0.0]);
        g.insert(2, [0.0, 0.0, 0.4]); // same floor
        g.insert(3, [0.0, 0.0, 5.0]); // upper floor, outside radius
        let mut hits = g.query_radius([0.0, 0.0, 0.0], 1.0);
        hits.sort();
        assert_eq!(hits, vec![1, 2]);
    }

    #[test]
    fn len_and_clear() {
        let mut g = UniformGrid3::new(1.0);
        g.insert_all([(1, [0.0, 0.0, 0.0]), (2, [1.0, 1.0, 1.0])]);
        assert_eq!(g.len(), 2);
        g.clear();
        assert!(g.is_empty());
    }
}