collision_detection/

lib.rs

#![deny(missing_docs)]

/*!
This crate defines an infrastructure to handle collision detection.

It's based on the `collide` crate, so your colliders need to implement the `Collider` trait.

The `CollisionManager` can only contain one type of collider.
When you want to use multiple colliders, you currently need to use trait objects or enums.

Then you can add colliders to the manager. You should store the returned [`ColliderId`]s, which will be used when you modify or remove them.
When you're done, you can compute the collisions, which you can use to update your objects.
You can either compute the collisions between all objects internally or between this object with a different layer.
Before computing the collisions again, you should update your colliders, for example if the position of the object changed.
**/

use std::{
    collections::{HashMap, HashSet},
    hash::Hash,
    ops::Neg,
};

use collide::{Bounded, BoundingVolume, Collider, CollisionInfo, SpatialPartition};
use slab::Slab;

/// The collision info with the index of the other collider.
pub struct IndexedCollisionInfo<V, I> {
    /// Index of the object.
    pub index: I,
    /// The actual collision info.
    pub info: CollisionInfo<V>,
}

/// Opaque handle to a collider stored in a [`CollisionManager`].
///
/// Returned by [`CollisionManager::insert_collider`] and used to address that
/// specific collider later (replace, remove, query). Distinct from the object
/// index `I`: a `ColliderId` identifies one stored collider uniquely, whereas
/// several colliders may share the same object index `I`.
#[derive(Clone, Copy, PartialEq, Eq, PartialOrd, Ord, Hash, Debug)]
pub struct ColliderId(usize);

struct Indexed<C: Collider, I> {
    index: I,
    collider: C,
}

/// The collision manager, which manages collisions.
/// Can contain multiple colliders.
pub struct CollisionManager<C: Collider, I> {
    colliders: Slab<Indexed<C, I>>,
}

impl<C: Collider, I: Copy> Default for CollisionManager<C, I> {
    fn default() -> Self {
        Self::new()
    }
}

impl<C: Collider, I: Copy> CollisionManager<C, I> {
    /// Creates a new collision manager.
    pub fn new() -> Self {
        Self {
            colliders: Slab::new(),
        }
    }

    /// Creates a new collision manager with the capacity of expected colliders specified.
    pub fn with_capacity(capacity: usize) -> Self {
        Self {
            colliders: Slab::with_capacity(capacity),
        }
    }

    /// Inserts a new collider and returns its [`ColliderId`].
    /// An object index needs to be specified.
    pub fn insert_collider(&mut self, collider: C, index: I) -> ColliderId {
        ColliderId(self.colliders.insert(Indexed { index, collider }))
    }

    /// Replaces the existing collider at the specified id by a new collider.
    pub fn replace_collider(&mut self, id: ColliderId, collider: C) {
        self.colliders[id.0].collider = collider;
    }

    /// Removes an existing collider, so the id is usable for a new collider again.
    pub fn remove_collider(&mut self, id: ColliderId) {
        self.colliders.remove(id.0);
    }

    /// Get the collider at the specified id.
    pub fn collider(&self, id: ColliderId) -> &C {
        &self.colliders[id.0].collider
    }

    /// Get the collider at the specified id as mutable.
    pub fn collider_mut(&mut self, id: ColliderId) -> &mut C {
        &mut self.colliders[id.0].collider
    }

    /// Checks if there is a collision at a specific position.
    pub fn check_collision(&self, check_collider: &C) -> bool {
        self.colliders
            .iter()
            .any(|(_, entry)| check_collider.check_collision(&entry.collider))
    }

    /// Checks for a collision with collider and returns some index if found.
    pub fn find_collision(&self, check_collider: &C) -> Option<I> {
        self.colliders.iter().find_map(|(_, entry)| {
            check_collider
                .check_collision(&entry.collider)
                .then_some(entry.index)
        })
    }

    /// Finds all collisions with colliders and returns their indices.
    pub fn find_collisions<'a>(
        &'a self,
        check_collider: &'a C,
    ) -> impl DoubleEndedIterator<Item = I> + 'a {
        self.colliders.iter().filter_map(move |(_, entry)| {
            check_collider
                .check_collision(&entry.collider)
                .then_some(entry.index)
        })
    }

    /// Computes the internal collisions between all colliders.
    /// Returns a map for each collider index to its collision infos.
    pub fn compute_inner_collisions(
        &self,
    ) -> HashMap<ColliderId, Vec<IndexedCollisionInfo<C::Vector, I>>>
    where
        C::Vector: Copy + Neg<Output = C::Vector>,
    {
        let mut result: HashMap<ColliderId, Vec<_>> = self
            .colliders
            .iter()
            .map(|(key, _)| (ColliderId(key), Vec::new()))
            .collect();

        let entries: Vec<_> = self.colliders.iter().collect();
        for (i, &(key, entry)) in entries.iter().enumerate() {
            for &(other_key, other_entry) in &entries[i + 1..] {
                if !entry.collider.check_collision(&other_entry.collider) {
                    continue;
                }
                let Some(info) = entry.collider.collision_info(&other_entry.collider) else {
                    continue;
                };

                result
                    .entry(ColliderId(other_key))
                    .or_default()
                    .push(IndexedCollisionInfo {
                        index: entry.index,
                        info: -info,
                    });
                result
                    .entry(ColliderId(key))
                    .or_default()
                    .push(IndexedCollisionInfo {
                        index: other_entry.index,
                        info,
                    });
            }
        }

        result
    }

    /// Computes the collisions between all colliders with the colliders of another collision manager.
    /// Returns a map for each collider index to its collision infos.
    pub fn compute_collisions_with(
        &self,
        other: &Self,
    ) -> HashMap<ColliderId, Vec<IndexedCollisionInfo<C::Vector, I>>> {
        self.colliders
            .iter()
            .map(|(key, entry)| {
                let infos = other
                    .colliders
                    .iter()
                    .filter(|(_, other_entry)| {
                        entry.collider.check_collision(&other_entry.collider)
                    })
                    .filter_map(|(_, other_entry)| {
                        entry
                            .collider
                            .collision_info(&other_entry.collider)
                            .map(|info| IndexedCollisionInfo {
                                index: other_entry.index,
                                info,
                            })
                    })
                    .collect();
                (ColliderId(key), infos)
            })
            .collect()
    }
}

impl<C: Collider + SpatialPartition, I: Copy> CollisionManager<C, I> {
    fn cell_map(&self) -> HashMap<C::Cell, Vec<ColliderId>> {
        let mut map: HashMap<C::Cell, Vec<ColliderId>> = HashMap::new();
        for (key, entry) in &self.colliders {
            for cell in entry.collider.cells() {
                map.entry(cell).or_default().push(ColliderId(key));
            }
        }
        map
    }

    /// Computes internal collisions using spatial partitioning.
    /// Only checks pairs that share at least one cell, reducing O(n²) to O(n×k).
    ///
    /// The cell map is rebuilt from scratch on every call, so this is best
    /// suited to layers whose colliders move between calls. For a static layer
    /// queried repeatedly, building the acceleration structure once would be
    /// cheaper, but that is not yet provided.
    pub fn compute_inner_collisions_spatial(
        &self,
    ) -> HashMap<ColliderId, Vec<IndexedCollisionInfo<C::Vector, I>>>
    where
        C::Vector: Copy + Neg<Output = C::Vector>,
        C::Cell: Hash + Eq,
    {
        let cell_map = self.cell_map();

        let mut result: HashMap<ColliderId, Vec<_>> = self
            .colliders
            .iter()
            .map(|(key, _)| (ColliderId(key), Vec::new()))
            .collect();

        let mut checked = HashSet::new();
        for keys_in_cell in cell_map.values() {
            for (i, &key) in keys_in_cell.iter().enumerate() {
                for &other_key in &keys_in_cell[i + 1..] {
                    let pair = if key < other_key {
                        [key, other_key]
                    } else {
                        [other_key, key]
                    };
                    if !checked.insert(pair) {
                        continue;
                    }

                    let entry = &self.colliders[key.0];
                    let other_entry = &self.colliders[other_key.0];
                    if !entry.collider.check_collision(&other_entry.collider) {
                        continue;
                    }
                    let Some(info) = entry.collider.collision_info(&other_entry.collider) else {
                        continue;
                    };

                    result
                        .entry(other_key)
                        .or_default()
                        .push(IndexedCollisionInfo {
                            index: entry.index,
                            info: -info,
                        });
                    result.entry(key).or_default().push(IndexedCollisionInfo {
                        index: other_entry.index,
                        info,
                    });
                }
            }
        }

        result
    }

    /// Computes collisions with another manager using spatial partitioning.
    /// Only checks pairs that share at least one cell, reducing O(n²) to O(n×k).
    ///
    /// The other manager's cell map is rebuilt on every call. When the same
    /// static layer is queried repeatedly, building its acceleration structure
    /// once would be cheaper, but that is not yet provided.
    pub fn compute_collisions_with_spatial(
        &self,
        other: &Self,
    ) -> HashMap<ColliderId, Vec<IndexedCollisionInfo<C::Vector, I>>> {
        let other_cell_map = other.cell_map();

        let mut result: HashMap<ColliderId, Vec<_>> = HashMap::new();
        let mut checked: HashSet<[ColliderId; 2]> = HashSet::new();

        for (key, entry) in &self.colliders {
            let key = ColliderId(key);
            result.entry(key).or_default();
            for cell in entry.collider.cells() {
                let Some(other_keys) = other_cell_map.get(&cell) else {
                    continue;
                };
                for &other_key in other_keys {
                    if !checked.insert([key, other_key]) {
                        continue;
                    }
                    let other_entry = &other.colliders[other_key.0];
                    if !entry.collider.check_collision(&other_entry.collider) {
                        continue;
                    }
                    if let Some(info) = entry.collider.collision_info(&other_entry.collider) {
                        result.entry(key).or_default().push(IndexedCollisionInfo {
                            index: other_entry.index,
                            info,
                        });
                    }
                }
            }
        }

        result
    }
}

enum BvhNode<V: BoundingVolume> {
    Leaf {
        key: ColliderId,
        volume: V,
    },
    Branch {
        volume: V,
        left: Box<Self>,
        right: Box<Self>,
    },
}

impl<V: BoundingVolume> BvhNode<V> {
    fn volume(&self) -> &V {
        match self {
            Self::Leaf { volume, .. } | Self::Branch { volume, .. } => volume,
        }
    }

    fn build(items: &mut [(ColliderId, V)]) -> Self {
        match items {
            [] => unreachable!(),
            [(key, volume)] => Self::Leaf {
                key: *key,
                volume: volume.clone(),
            },
            _ => {
                let mid = items.len() / 2;
                let left = Box::new(Self::build(&mut items[..mid]));
                let right = Box::new(Self::build(&mut items[mid..]));
                let volume = left.volume().merged(right.volume());
                Self::Branch {
                    volume,
                    left,
                    right,
                }
            }
        }
    }

    fn self_collisions<C: Collider, I: Copy>(
        &self,
        colliders: &Slab<Indexed<C, I>>,
        result: &mut HashMap<ColliderId, Vec<IndexedCollisionInfo<C::Vector, I>>>,
    ) where
        C::Vector: Copy + Neg<Output = C::Vector>,
    {
        match self {
            Self::Leaf { .. } => {}
            Self::Branch { left, right, .. } => {
                left.self_collisions(colliders, result);
                right.self_collisions(colliders, result);
                Self::cross_collisions_inner(left, right, colliders, result);
            }
        }
    }

    fn cross_collisions_inner<C: Collider, I: Copy>(
        a: &Self,
        b: &Self,
        colliders: &Slab<Indexed<C, I>>,
        result: &mut HashMap<ColliderId, Vec<IndexedCollisionInfo<C::Vector, I>>>,
    ) where
        C::Vector: Copy + Neg<Output = C::Vector>,
    {
        if !a.volume().overlaps(b.volume()) {
            return;
        }

        match (a, b) {
            (Self::Leaf { key, .. }, Self::Leaf { key: other_key, .. }) => {
                let entry = &colliders[key.0];
                let other_entry = &colliders[other_key.0];
                if !entry.collider.check_collision(&other_entry.collider) {
                    return;
                }
                let Some(info) = entry.collider.collision_info(&other_entry.collider) else {
                    return;
                };
                result
                    .entry(*other_key)
                    .or_default()
                    .push(IndexedCollisionInfo {
                        index: entry.index,
                        info: -info,
                    });
                result.entry(*key).or_default().push(IndexedCollisionInfo {
                    index: other_entry.index,
                    info,
                });
            }
            (Self::Leaf { .. }, Self::Branch { left, right, .. }) => {
                Self::cross_collisions_inner(a, left, colliders, result);
                Self::cross_collisions_inner(a, right, colliders, result);
            }
            (Self::Branch { left, right, .. }, Self::Leaf { .. }) => {
                Self::cross_collisions_inner(left, b, colliders, result);
                Self::cross_collisions_inner(right, b, colliders, result);
            }
            (
                Self::Branch {
                    left: a_left,
                    right: a_right,
                    ..
                },
                Self::Branch {
                    left: b_left,
                    right: b_right,
                    ..
                },
            ) => {
                Self::cross_collisions_inner(a_left, b_left, colliders, result);
                Self::cross_collisions_inner(a_left, b_right, colliders, result);
                Self::cross_collisions_inner(a_right, b_left, colliders, result);
                Self::cross_collisions_inner(a_right, b_right, colliders, result);
            }
        }
    }

    fn cross_collisions_between<C: Collider, I: Copy>(
        a: &Self,
        b: &Self,
        a_colliders: &Slab<Indexed<C, I>>,
        b_colliders: &Slab<Indexed<C, I>>,
        result: &mut HashMap<ColliderId, Vec<IndexedCollisionInfo<C::Vector, I>>>,
    ) {
        if !a.volume().overlaps(b.volume()) {
            return;
        }

        match (a, b) {
            (Self::Leaf { key, .. }, Self::Leaf { key: other_key, .. }) => {
                let entry = &a_colliders[key.0];
                let other_entry = &b_colliders[other_key.0];
                if !entry.collider.check_collision(&other_entry.collider) {
                    return;
                }
                if let Some(info) = entry.collider.collision_info(&other_entry.collider) {
                    result.entry(*key).or_default().push(IndexedCollisionInfo {
                        index: other_entry.index,
                        info,
                    });
                }
            }
            (Self::Leaf { .. }, Self::Branch { left, right, .. }) => {
                Self::cross_collisions_between(a, left, a_colliders, b_colliders, result);
                Self::cross_collisions_between(a, right, a_colliders, b_colliders, result);
            }
            (Self::Branch { left, right, .. }, _) => {
                Self::cross_collisions_between(left, b, a_colliders, b_colliders, result);
                Self::cross_collisions_between(right, b, a_colliders, b_colliders, result);
            }
        }
    }
}

impl<C: Collider, I: Copy> CollisionManager<C, I> {
    fn build_bvh<V: BoundingVolume>(&self) -> Option<BvhNode<V>>
    where
        C: Bounded<V>,
    {
        let mut items: Vec<_> = self
            .colliders
            .iter()
            .map(|(key, entry)| (ColliderId(key), entry.collider.bounding_volume()))
            .collect();
        if items.is_empty() {
            return None;
        }
        Some(BvhNode::build(&mut items))
    }

    /// Computes internal collisions using a bounding volume hierarchy.
    /// Skips pairs whose bounding volumes don't overlap, reducing O(n²) to O(n log n).
    /// The type parameter `V` selects which bounding volume to use.
    ///
    /// The BVH is rebuilt on every call, so this fits layers whose colliders
    /// move between calls. For a static layer queried repeatedly, building the
    /// BVH once would be cheaper, but that is not yet provided.
    pub fn compute_inner_collisions_bvh<V: BoundingVolume>(
        &self,
    ) -> HashMap<ColliderId, Vec<IndexedCollisionInfo<C::Vector, I>>>
    where
        C: Bounded<V>,
        C::Vector: Copy + Neg<Output = C::Vector>,
    {
        let mut result: HashMap<ColliderId, Vec<_>> = self
            .colliders
            .iter()
            .map(|(key, _)| (ColliderId(key), Vec::new()))
            .collect();

        if let Some(root) = self.build_bvh::<V>() {
            root.self_collisions(&self.colliders, &mut result);
        }

        result
    }

    /// Computes collisions with another manager using bounding volume hierarchies.
    /// Builds a BVH for each manager and traverses them together,
    /// skipping subtree pairs whose volumes don't overlap.
    /// The type parameter `V` selects which bounding volume to use.
    ///
    /// Both BVHs are rebuilt on every call. When the same static layer is
    /// queried repeatedly, building its BVH once would be cheaper, but that is
    /// not yet provided.
    pub fn compute_collisions_with_bvh<V: BoundingVolume>(
        &self,
        other: &Self,
    ) -> HashMap<ColliderId, Vec<IndexedCollisionInfo<C::Vector, I>>>
    where
        C: Bounded<V>,
    {
        let mut result: HashMap<ColliderId, Vec<_>> = self
            .colliders
            .iter()
            .map(|(key, _)| (ColliderId(key), Vec::new()))
            .collect();

        if let Some(self_root) = self.build_bvh::<V>()
            && let Some(other_root) = other.build_bvh::<V>()
        {
            BvhNode::cross_collisions_between(
                &self_root,
                &other_root,
                &self.colliders,
                &other.colliders,
                &mut result,
            );
        }

        result
    }
}

#[cfg(test)]
#[allow(clippy::unwrap_used, clippy::expect_used)]
mod tests {
    use super::*;
    use collide_sphere::Sphere;
    use inner_space::InnerSpace;
    use simple_vectors::Vector;

    type Vec3 = Vector<f32, 3>;

    #[derive(Clone, Debug)]
    struct TestSphere(Sphere<Vec3>);

    impl Collider for TestSphere {
        type Vector = Vec3;

        fn check_collision(&self, other: &Self) -> bool {
            self.0.check_collision(&other.0)
        }

        fn collision_info(&self, other: &Self) -> Option<CollisionInfo<Vec3>> {
            self.0.collision_info(&other.0)
        }
    }

    impl BoundingVolume for TestSphere {
        fn overlaps(&self, other: &Self) -> bool {
            self.0.check_collision(&other.0)
        }

        fn merged(&self, other: &Self) -> Self {
            Self(collide::BoundingVolume::merged(&self.0, &other.0))
        }
    }

    impl Bounded<Self> for TestSphere {
        fn bounding_volume(&self) -> Self {
            self.clone()
        }
    }

    impl SpatialPartition for TestSphere {
        type Cell = [i32; 3];

        fn cells(&self) -> impl Iterator<Item = [i32; 3]> {
            let min_x = (self.0.center[0] - self.0.radius).floor() as i32;
            let max_x = (self.0.center[0] + self.0.radius).floor() as i32;
            let min_y = (self.0.center[1] - self.0.radius).floor() as i32;
            let max_y = (self.0.center[1] + self.0.radius).floor() as i32;
            let min_z = (self.0.center[2] - self.0.radius).floor() as i32;
            let max_z = (self.0.center[2] + self.0.radius).floor() as i32;

            let mut result = Vec::new();
            for x in min_x..=max_x {
                for y in min_y..=max_y {
                    for z in min_z..=max_z {
                        result.push([x, y, z]);
                    }
                }
            }
            result.into_iter()
        }
    }

    fn sphere(x: f32, y: f32, z: f32, radius: f32) -> TestSphere {
        TestSphere(Sphere::new(Vec3::from([x, y, z]), radius))
    }

    fn setup_manager() -> CollisionManager<TestSphere, u32> {
        let mut manager = CollisionManager::new();
        manager.insert_collider(sphere(0.0, 0.0, 0.0, 1.0), 0);
        manager.insert_collider(sphere(1.5, 0.0, 0.0, 1.0), 1);
        manager.insert_collider(sphere(10.0, 0.0, 0.0, 1.0), 2);
        manager
    }

    #[test]
    fn check_collision_finds_overlap() {
        let manager = setup_manager();
        assert!(manager.check_collision(&sphere(0.5, 0.0, 0.0, 0.1)));
    }

    #[test]
    fn check_collision_misses() {
        let manager = setup_manager();
        assert!(!manager.check_collision(&sphere(5.0, 0.0, 0.0, 0.1)));
    }

    #[test]
    fn find_collision_returns_index() {
        let manager = setup_manager();
        let found = manager.find_collision(&sphere(0.5, 0.0, 0.0, 0.1));
        assert!(found == Some(0) || found == Some(1));
    }

    #[test]
    fn find_collision_returns_none() {
        let manager = setup_manager();
        assert_eq!(manager.find_collision(&sphere(5.0, 0.0, 0.0, 0.1)), None);
    }

    #[test]
    fn find_collisions_returns_all() {
        let manager = setup_manager();
        let found: Vec<_> = manager
            .find_collisions(&sphere(0.75, 0.0, 0.0, 1.0))
            .collect();
        assert!(found.contains(&0));
        assert!(found.contains(&1));
        assert!(!found.contains(&2));
    }

    #[test]
    fn inner_collisions_detects_pair() {
        let manager = setup_manager();
        let collisions = manager.compute_inner_collisions();

        let hits_0: Vec<_> = collisions[&ColliderId(0)].iter().map(|c| c.index).collect();
        let hits_2: Vec<_> = collisions[&ColliderId(2)].iter().map(|c| c.index).collect();

        assert!(hits_0.contains(&1));
        assert!(hits_2.is_empty());
    }

    #[test]
    fn inner_collisions_bidirectional() {
        let manager = setup_manager();
        let collisions = manager.compute_inner_collisions();

        let zero_hits_one = collisions[&ColliderId(0)].iter().any(|c| c.index == 1);
        let one_hits_zero = collisions[&ColliderId(1)].iter().any(|c| c.index == 0);

        assert!(zero_hits_one);
        assert!(one_hits_zero);
    }

    #[test]
    fn inner_collisions_vectors_opposite() {
        let manager = setup_manager();
        let collisions = manager.compute_inner_collisions();

        let vec_0_to_1 = collisions[&ColliderId(0)]
            .iter()
            .find(|c| c.index == 1)
            .unwrap()
            .info
            .vector;
        let vec_1_to_0 = collisions[&ColliderId(1)]
            .iter()
            .find(|c| c.index == 0)
            .unwrap()
            .info
            .vector;

        let sum = vec_0_to_1 + vec_1_to_0;
        assert!(sum.magnitude2() < 0.001);
    }

    #[test]
    fn collisions_with_cross_manager() {
        let mut manager_a = CollisionManager::new();
        manager_a.insert_collider(sphere(0.0, 0.0, 0.0, 1.0), 10);

        let mut manager_b = CollisionManager::new();
        manager_b.insert_collider(sphere(1.5, 0.0, 0.0, 1.0), 20);
        manager_b.insert_collider(sphere(10.0, 0.0, 0.0, 1.0), 30);

        let collisions = manager_a.compute_collisions_with(&manager_b);
        let hits: Vec<_> = collisions
            .values()
            .flat_map(|v| v.iter().map(|c| c.index))
            .collect();

        assert!(hits.contains(&20));
        assert!(!hits.contains(&30));
    }

    #[test]
    fn insert_and_remove() {
        let mut manager = CollisionManager::<TestSphere, u32>::new();
        let idx = manager.insert_collider(sphere(0.0, 0.0, 0.0, 1.0), 0);
        manager.remove_collider(idx);

        assert!(!manager.check_collision(&sphere(0.0, 0.0, 0.0, 0.1)));
    }

    #[test]
    fn replace_collider_updates_position() {
        let mut manager = CollisionManager::new();
        let idx = manager.insert_collider(sphere(0.0, 0.0, 0.0, 1.0), 0);
        manager.insert_collider(sphere(1.5, 0.0, 0.0, 1.0), 1);

        let collisions_before = manager.compute_inner_collisions();
        assert!(!collisions_before[&idx].is_empty());

        manager.replace_collider(idx, sphere(100.0, 0.0, 0.0, 1.0));

        let collisions_after = manager.compute_inner_collisions();
        assert!(collisions_after[&idx].is_empty());
    }

    #[test]
    fn empty_manager() {
        let manager = CollisionManager::<Sphere<Vec3>, u32>::new();
        let collisions = manager.compute_inner_collisions();
        assert!(collisions.is_empty());
    }

    #[test]
    fn bvh_matches_brute_force() {
        let manager = setup_manager();
        let brute = manager.compute_inner_collisions();
        let bvh = manager.compute_inner_collisions_bvh::<TestSphere>();

        for (key, brute_hits) in &brute {
            let bvh_hits = &bvh[key];
            let mut brute_indices: Vec<_> = brute_hits.iter().map(|c| c.index).collect();
            let mut bvh_indices: Vec<_> = bvh_hits.iter().map(|c| c.index).collect();
            brute_indices.sort_unstable();
            bvh_indices.sort_unstable();
            assert_eq!(brute_indices, bvh_indices);
        }
    }

    #[test]
    fn bvh_cross_matches_brute_force() {
        let mut manager_a = CollisionManager::new();
        manager_a.insert_collider(sphere(0.0, 0.0, 0.0, 1.0), 10);
        manager_a.insert_collider(sphere(5.0, 0.0, 0.0, 1.0), 11);

        let mut manager_b = CollisionManager::new();
        manager_b.insert_collider(sphere(1.5, 0.0, 0.0, 1.0), 20);
        manager_b.insert_collider(sphere(5.5, 0.0, 0.0, 1.0), 21);
        manager_b.insert_collider(sphere(100.0, 0.0, 0.0, 1.0), 22);

        let brute = manager_a.compute_collisions_with(&manager_b);
        let bvh = manager_a.compute_collisions_with_bvh::<TestSphere>(&manager_b);

        for (key, brute_hits) in &brute {
            let bvh_hits = &bvh[key];
            let mut brute_indices: Vec<_> = brute_hits.iter().map(|c| c.index).collect();
            let mut bvh_indices: Vec<_> = bvh_hits.iter().map(|c| c.index).collect();
            brute_indices.sort_unstable();
            bvh_indices.sort_unstable();
            assert_eq!(brute_indices, bvh_indices);
        }
    }

    #[test]
    fn bvh_many_objects() {
        let mut manager = CollisionManager::new();
        for i in 0..50u32 {
            manager.insert_collider(sphere(i as f32 * 1.5, 0.0, 0.0, 1.0), i);
        }

        let brute = manager.compute_inner_collisions();
        let bvh = manager.compute_inner_collisions_bvh::<TestSphere>();

        let brute_count: usize = brute.values().map(std::vec::Vec::len).sum();
        let bvh_count: usize = bvh.values().map(std::vec::Vec::len).sum();
        assert_eq!(brute_count, bvh_count);
    }

    #[test]
    fn bvh_no_collisions() {
        let mut manager = CollisionManager::new();
        for i in 0..10u32 {
            manager.insert_collider(sphere(i as f32 * 10.0, 0.0, 0.0, 1.0), i);
        }

        let collisions = manager.compute_inner_collisions_bvh::<TestSphere>();
        let total: usize = collisions.values().map(std::vec::Vec::len).sum();
        assert_eq!(total, 0);
    }

    #[test]
    fn bvh_all_overlapping() {
        let mut manager = CollisionManager::new();
        for i in 0..5u32 {
            manager.insert_collider(sphere(0.0, 0.0, 0.0, 1.0), i);
        }

        let brute = manager.compute_inner_collisions();
        let bvh = manager.compute_inner_collisions_bvh::<TestSphere>();

        let brute_count: usize = brute.values().map(std::vec::Vec::len).sum();
        let bvh_count: usize = bvh.values().map(std::vec::Vec::len).sum();
        assert_eq!(brute_count, bvh_count);
        assert_eq!(brute_count, 5 * 4);
    }

    #[test]
    fn spatial_matches_brute_force() {
        let manager = setup_manager();
        let brute = manager.compute_inner_collisions();
        let spatial = manager.compute_inner_collisions_spatial();

        for (key, brute_hits) in &brute {
            let spatial_hits = &spatial[key];
            let mut brute_indices: Vec<_> = brute_hits.iter().map(|c| c.index).collect();
            let mut spatial_indices: Vec<_> = spatial_hits.iter().map(|c| c.index).collect();
            brute_indices.sort_unstable();
            spatial_indices.sort_unstable();
            assert_eq!(brute_indices, spatial_indices);
        }
    }

    #[test]
    fn spatial_cross_matches_brute_force() {
        let mut manager_a = CollisionManager::new();
        manager_a.insert_collider(sphere(0.0, 0.0, 0.0, 1.0), 10);
        manager_a.insert_collider(sphere(5.0, 0.0, 0.0, 1.0), 11);

        let mut manager_b = CollisionManager::new();
        manager_b.insert_collider(sphere(1.5, 0.0, 0.0, 1.0), 20);
        manager_b.insert_collider(sphere(100.0, 0.0, 0.0, 1.0), 30);

        let brute = manager_a.compute_collisions_with(&manager_b);
        let spatial = manager_a.compute_collisions_with_spatial(&manager_b);

        for (key, brute_hits) in &brute {
            let spatial_hits = &spatial[key];
            let mut brute_indices: Vec<_> = brute_hits.iter().map(|c| c.index).collect();
            let mut spatial_indices: Vec<_> = spatial_hits.iter().map(|c| c.index).collect();
            brute_indices.sort_unstable();
            spatial_indices.sort_unstable();
            assert_eq!(brute_indices, spatial_indices);
        }
    }

    #[test]
    fn spatial_many_objects() {
        let mut manager = CollisionManager::new();
        for i in 0..50u32 {
            manager.insert_collider(sphere(i as f32 * 1.5, 0.0, 0.0, 1.0), i);
        }

        let brute = manager.compute_inner_collisions();
        let spatial = manager.compute_inner_collisions_spatial();

        let brute_count: usize = brute.values().map(std::vec::Vec::len).sum();
        let spatial_count: usize = spatial.values().map(std::vec::Vec::len).sum();
        assert_eq!(brute_count, spatial_count);
    }
}