euv_engine/physics/

impl.rs

use super::*;

/// Implements `Default` for `BodyCollider`, returning an AABB collider with default values.
impl Default for BodyCollider {
    fn default() -> BodyCollider {
        BodyCollider::Aabb(AabbCollider::default())
    }
}

/// Implements `Default` for `BodyCollider3D`, returning a 3D AABB collider with default values.
impl Default for BodyCollider3D {
    fn default() -> BodyCollider3D {
        BodyCollider3D::Aabb(AabbCollider3D::default())
    }
}

/// Implements default configuration for `PhysicsConfig`.
impl Default for PhysicsConfig {
    fn default() -> PhysicsConfig {
        PhysicsConfig::new(
            Vector2D::new(0.0, DEFAULT_GRAVITY),
            DEFAULT_LINEAR_DAMPING,
            DEFAULT_ANGULAR_DAMPING,
        )
    }
}

/// Implements body creation and force management for `RigidBody2D`.
impl RigidBody2D {
    /// Creates a new dynamic rigid body with default mass and the given position.
    ///
    /// # Arguments
    ///
    /// - `u64` - The unique ID.
    /// - `Vector2D` - The initial position.
    ///
    /// # Returns
    ///
    /// - `RigidBody2D` - The new body.
    pub fn new_dynamic(id: u64, position: Vector2D) -> RigidBody2D {
        let mass: f64 = PHYSICS_DEFAULT_MASS;
        RigidBody2D::new(
            id,
            position,
            mass,
            1.0 / mass,
            DEFAULT_RESTITUTION,
            DEFAULT_FRICTION,
            BodyType::Dynamic,
        )
    }

    /// Creates a new static rigid body at the given position with infinite mass.
    ///
    /// # Arguments
    ///
    /// - `u64` - The unique ID.
    /// - `Vector2D` - The position.
    ///
    /// # Returns
    ///
    /// - `RigidBody2D` - The new static body.
    pub fn new_static(id: u64, position: Vector2D) -> RigidBody2D {
        RigidBody2D::new(
            id,
            position,
            PHYSICS_STATIC_MASS,
            0.0,
            DEFAULT_RESTITUTION,
            DEFAULT_FRICTION,
            BodyType::Static,
        )
    }

    /// Applies a force to the body's force accumulator.
    ///
    /// # Arguments
    ///
    /// - `Vector2D` - The force vector.
    pub fn apply_force(&mut self, force: Vector2D) {
        *self.get_mut_force_accumulator() += force;
    }

    /// Applies an instantaneous impulse, directly changing velocity.
    ///
    /// # Arguments
    ///
    /// - `Vector2D` - The impulse vector.
    pub fn apply_impulse(&mut self, impulse: Vector2D) {
        let inverse_mass: f64 = self.get_inverse_mass();
        if inverse_mass == 0.0 {
            return;
        }
        *self.get_mut_velocity() += impulse.scaled(inverse_mass);
    }

    /// Sets the mass of the body, updating the inverse mass.
    /// A mass of 0 makes the body static (infinite mass).
    ///
    /// # Arguments
    ///
    /// - `f64` - The new mass.
    pub fn update_mass(&mut self, mass: f64) {
        self.set_mass(mass);
        self.set_inverse_mass(if mass > 0.0 { 1.0 / mass } else { 0.0 });
    }

    /// Returns `true` if this body is affected by forces and collisions.
    ///
    /// # Returns
    ///
    /// - `bool` - True if the body is dynamic.
    pub fn is_dynamic(&self) -> bool {
        self.get_body_type() == BodyType::Dynamic
    }

    /// Attaches a collider shape to this body.
    ///
    /// # Arguments
    ///
    /// - `BodyCollider` - The collider to attach.
    pub fn update_collider(&mut self, collider: BodyCollider) {
        self.set_collider(Some(collider));
    }

    /// Returns the world-space bounding box of the attached collider, if any.
    ///
    /// # Returns
    ///
    /// - `Option<Rect>` - The bounding box, or `None` if no collider is attached.
    pub fn bounding_box(&self) -> Option<Rect> {
        let collider: Option<BodyCollider> = self.get_collider();
        match collider? {
            BodyCollider::Aabb(aabb) => {
                let aabb_rect: Rect = aabb.get_rect();
                let mut offset_rect: Rect = aabb_rect;
                offset_rect.set_x(
                    offset_rect.get_x() + self.get_position().get_x() - aabb_rect.get_width() * 0.5,
                );
                offset_rect.set_y(
                    offset_rect.get_y() + self.get_position().get_y()
                        - aabb_rect.get_height() * 0.5,
                );
                Some(offset_rect)
            }
            BodyCollider::Circle(circle) => {
                let diameter: f64 = circle.get_circle().get_radius() * 2.0;
                Some(Rect::from_center(self.get_position(), diameter, diameter))
            }
        }
    }
}

/// Implements body management and simulation for `PhysicsWorld2D`.
impl PhysicsWorld2D {
    /// Creates a new physics world with the given configuration.
    ///
    /// # Arguments
    ///
    /// - `PhysicsConfig` - The simulation configuration.
    ///
    /// # Returns
    ///
    /// - `PhysicsWorld2D` - The new world.
    pub fn with_config(config: PhysicsConfig) -> PhysicsWorld2D {
        let mut world: PhysicsWorld2D = PhysicsWorld2D::new(config);
        world.set_grid(SpatialHashGrid2D::with_default_size());
        world
    }

    /// Adds a rigid body to the world.
    ///
    /// # Arguments
    ///
    /// - `RigidBody2D` - The body to add.
    pub fn add_body(&mut self, body: RigidBody2D) {
        self.get_mut_bodies().push(body);
    }

    /// Removes the body with the given ID.
    ///
    /// # Arguments
    ///
    /// - `u64` - The ID of the body to remove.
    pub fn remove_body(&mut self, id: u64) {
        self.get_mut_bodies()
            .retain(|body: &RigidBody2D| body.get_id() != id);
    }

    /// Returns a reference to the body with the given ID.
    ///
    /// # Arguments
    ///
    /// - `u64` - The body ID.
    ///
    /// # Returns
    ///
    /// - `Option<&RigidBody2D>` - The body reference, if found.
    pub fn get_body(&self, id: u64) -> Option<&RigidBody2D> {
        self.get_bodies()
            .iter()
            .find(|body: &&RigidBody2D| body.get_id() == id)
    }

    /// Returns a mutable reference to the body with the given ID.
    ///
    /// # Arguments
    ///
    /// - `u64` - The body ID.
    ///
    /// # Returns
    ///
    /// - `Option<&mut RigidBody2D>` - The mutable body reference, if found.
    pub fn get_body_mut(&mut self, id: u64) -> Option<&mut RigidBody2D> {
        self.get_mut_bodies()
            .iter_mut()
            .find(|body: &&mut RigidBody2D| body.get_id() == id)
    }
}

/// Implements `Default` for `PhysicsWorld2D` as an empty world.
impl Default for PhysicsWorld2D {
    fn default() -> PhysicsWorld2D {
        PhysicsWorld2D::with_config(PhysicsConfig::default())
    }
}

/// Implements collision detection and resolution for `RigidBody2D`.
impl RigidBody2D {
    /// Checks collision with another body based on both bodies' collider shapes.
    ///
    /// # Arguments
    ///
    /// - `&RigidBody2D` - The other body to check against.
    ///
    /// # Returns
    ///
    /// - `Option<CollisionResult>` - The collision result, or `None`.
    fn check_collision_with(&self, other: &RigidBody2D) -> Option<CollisionResult> {
        let a_bbox: Rect = self.bounding_box()?;
        let b_bbox: Rect = other.bounding_box()?;
        if !Rect::broad_phase_alias(a_bbox, b_bbox) {
            return None;
        }
        let self_collider: Option<BodyCollider> = self.get_collider();
        let other_collider: Option<BodyCollider> = other.get_collider();
        let position_delta: Vector2D = other.get_position() - self.get_position();
        match (self_collider, other_collider) {
            (Some(BodyCollider::Aabb(aabb_a)), Some(BodyCollider::Aabb(aabb_b))) => {
                let aabb_b_rect: Rect = aabb_b.get_rect();
                let offset_aabb_b: AabbCollider = AabbCollider::new(Rect::new(
                    aabb_b_rect.get_x() + position_delta.get_x(),
                    aabb_b_rect.get_y() + position_delta.get_y(),
                    aabb_b_rect.get_width(),
                    aabb_b_rect.get_height(),
                ));
                aabb_a.collide_with_aabb(&offset_aabb_b)
            }
            (Some(BodyCollider::Circle(circle_a)), Some(BodyCollider::Circle(circle_b))) => {
                let circle_b_inner: Circle = circle_b.get_circle();
                let offset_circle_b: CircleCollider = CircleCollider::new(Circle::new(
                    circle_b_inner.get_center() + position_delta,
                    circle_b_inner.get_radius(),
                ));
                circle_a.collide_with_circle(&offset_circle_b)
            }
            (Some(BodyCollider::Aabb(aabb)), Some(BodyCollider::Circle(circle))) => {
                let circle_inner: Circle = circle.get_circle();
                let offset_circle: CircleCollider = CircleCollider::new(Circle::new(
                    circle_inner.get_center() + position_delta,
                    circle_inner.get_radius(),
                ));
                aabb.collide_with_circle(&offset_circle)
            }
            (Some(BodyCollider::Circle(circle)), Some(BodyCollider::Aabb(aabb))) => {
                let aabb_rect: Rect = aabb.get_rect();
                let offset_aabb: AabbCollider = AabbCollider::new(Rect::new(
                    aabb_rect.get_x() + position_delta.get_x(),
                    aabb_rect.get_y() + position_delta.get_y(),
                    aabb_rect.get_width(),
                    aabb_rect.get_height(),
                ));
                offset_aabb
                    .collide_with_circle(&circle)
                    .map(|mut result: CollisionResult| {
                        result.set_normal(-result.get_normal());
                        result
                    })
            }
            _ => None,
        }
    }

    /// Resolves a collision with another body using impulse-based response
    /// and position correction.
    ///
    /// # Arguments
    ///
    /// - `&mut RigidBody2D` - The other body involved in the collision.
    /// - `&CollisionResult` - The collision data.
    fn resolve_collision_with(&mut self, other: &mut RigidBody2D, result: &CollisionResult) {
        let self_inverse_mass: f64 = self.get_inverse_mass();
        let other_inverse_mass: f64 = other.get_inverse_mass();
        let relative_velocity: Vector2D = other.get_velocity() - self.get_velocity();
        let velocity_along_normal: f64 = relative_velocity.dot(result.get_normal());
        if velocity_along_normal > 0.0 {
            return;
        }
        let restitution: f64 = self.get_restitution().min(other.get_restitution());
        let inverse_mass_sum: f64 = self_inverse_mass + other_inverse_mass;
        if inverse_mass_sum == 0.0 {
            return;
        }
        let impulse_magnitude: f64 =
            -(1.0 + restitution) * velocity_along_normal / inverse_mass_sum;
        let impulse: Vector2D = result.get_normal().scaled(impulse_magnitude);
        *self.get_mut_velocity() -= impulse.scaled(self_inverse_mass);
        *other.get_mut_velocity() += impulse.scaled(other_inverse_mass);
        let correction: Vector2D = result
            .get_normal()
            .scaled((result.get_depth() * PHYSICS_POSITION_PERCENT / inverse_mass_sum).max(0.0));
        *self.get_mut_position() -= correction.scaled(self_inverse_mass);
        *other.get_mut_position() += correction.scaled(other_inverse_mass);
    }
}

/// Implements simulation stepping and collision resolution for `PhysicsWorld2D`.
impl PhysicsWorld2D {
    /// Performs one physics simulation step using semi-implicit Euler integration.
    ///
    /// Applies gravity to dynamic bodies, integrates velocity from accumulated forces,
    /// applies damping, integrates position, and resolves collisions.
    ///
    /// # Arguments
    ///
    /// - `f64` - The fixed delta time in seconds.
    pub fn step(&mut self, delta_time: f64) {
        let config: PhysicsConfig = self.get_config();
        // Hoist loop-invariant damping factors out of the per-body loop.
        let damping_factor: f64 = (1.0 - config.get_linear_damping() * delta_time).max(0.0);
        let angular_damping: f64 = (1.0 - config.get_angular_damping() * delta_time).max(0.0);
        let gravity: Vector2D = config.get_gravity();
        for body in self.get_mut_bodies() {
            if !body.is_dynamic() {
                continue;
            }
            let body_mass: f64 = body.get_mass();
            let body_inverse_mass: f64 = body.get_inverse_mass();
            *body.get_mut_force_accumulator() += gravity.scaled(body_mass);
            let force: Vector2D = body.get_force_accumulator();
            *body.get_mut_velocity() += force.scaled(body_inverse_mass * delta_time);
            // In-place damping and integration avoid temporary vector copies.
            *body.get_mut_velocity() *= damping_factor;
            let current_velocity: Vector2D = body.get_velocity();
            *body.get_mut_position() += current_velocity.scaled(delta_time);
            body.set_force_accumulator(Vector2D::zero());
            *body.get_mut_angular_velocity() *= angular_damping;
            let current_angular_velocity: f64 = body.get_angular_velocity();
            *body.get_mut_rotation() += current_angular_velocity * delta_time;
        }
        self.resolve_collisions();
    }

    /// Detects and resolves all collisions between bodies in the world.
    ///
    /// Uses a spatial hash grid for broad-phase culling followed by narrow-phase
    /// shape-specific collision detection, then applies impulse-based resolution.
    /// This reduces the broad-phase from O(n²) to near O(n) for typical scenes.
    fn resolve_collisions(&mut self) {
        let body_count: usize = self.get_bodies().len();
        if body_count < 2 {
            return;
        }
        // Rebuild the persistent grid once per step and collect the candidate
        // pair list once; every solver iteration then reuses both (the grid is
        // unchanged between iterations), eliminating per-iteration re-queries and
        // per-query allocations.
        let mut pairs: Vec<(usize, usize)> = Vec::new();
        {
            let (bodies, grid, query_buffer, query_seen) = (
                &self.bodies,
                &mut self.grid,
                &mut self.query_buffer,
                &mut self.query_seen,
            );
            grid.clear();
            for (index, body) in bodies.iter().enumerate() {
                if let Some(bbox) = body.bounding_box() {
                    grid.insert(index, bbox.min(), bbox.max());
                }
            }
            for (i, body) in bodies.iter().enumerate() {
                let Some(bbox) = body.bounding_box() else {
                    continue;
                };
                grid.query_into(bbox.min(), bbox.max(), query_buffer, query_seen);
                for &j in query_buffer.iter() {
                    if j > i {
                        pairs.push((i, j));
                    }
                }
            }
        }
        for iteration in 0..PHYSICS_MAX_ITERATIONS {
            let mut any_collision: bool = false;
            for &(i, j) in pairs.iter() {
                let (left, right) = self.get_mut_bodies().split_at_mut(j);
                let body_a: &mut RigidBody2D = &mut left[i];
                let body_b: &mut RigidBody2D = &mut right[0];
                if body_a.get_inverse_mass() == 0.0 && body_b.get_inverse_mass() == 0.0 {
                    continue;
                }
                if let Some(result) = body_a.check_collision_with(body_b) {
                    body_a.resolve_collision_with(body_b, &result);
                    any_collision = true;
                }
            }
            if !any_collision {
                break;
            }
            let _ = iteration;
        }
    }
}

/// Implements default configuration for `PhysicsConfig3D`.
impl Default for PhysicsConfig3D {
    fn default() -> PhysicsConfig3D {
        PhysicsConfig3D::new(
            Vector3D::new(0.0, DEFAULT_GRAVITY_3D, 0.0),
            DEFAULT_LINEAR_DAMPING,
            DEFAULT_ANGULAR_DAMPING,
        )
    }
}

/// Implements body creation and force management for `RigidBody3D`.
impl RigidBody3D {
    /// Creates a new dynamic 3D rigid body with default mass and the given position.
    ///
    /// # Arguments
    ///
    /// - `u64` - The unique ID.
    /// - `Vector3D` - The initial position.
    ///
    /// # Returns
    ///
    /// - `RigidBody3D` - The new body.
    pub fn new_dynamic(id: u64, position: Vector3D) -> RigidBody3D {
        let mass: f64 = PHYSICS_DEFAULT_MASS;
        RigidBody3D::new(
            id,
            position,
            mass,
            1.0 / mass,
            DEFAULT_RESTITUTION,
            DEFAULT_FRICTION,
            BodyType::Dynamic,
        )
    }

    /// Creates a new static 3D rigid body at the given position with infinite mass.
    ///
    /// # Arguments
    ///
    /// - `u64` - The unique ID.
    /// - `Vector3D` - The position.
    ///
    /// # Returns
    ///
    /// - `RigidBody3D` - The new static body.
    pub fn new_static(id: u64, position: Vector3D) -> RigidBody3D {
        RigidBody3D::new(
            id,
            position,
            PHYSICS_STATIC_MASS,
            0.0,
            DEFAULT_RESTITUTION,
            DEFAULT_FRICTION,
            BodyType::Static,
        )
    }

    /// Applies a force to the body's force accumulator.
    ///
    /// # Arguments
    ///
    /// - `Vector3D` - The force vector.
    pub fn apply_force(&mut self, force: Vector3D) {
        *self.get_mut_force_accumulator() += force;
    }

    /// Applies a torque to the body's torque accumulator.
    ///
    /// # Arguments
    ///
    /// - `Vector3D` - The torque vector.
    pub fn apply_torque(&mut self, torque: Vector3D) {
        *self.get_mut_torque_accumulator() += torque;
    }

    /// Applies an instantaneous impulse, directly changing velocity.
    ///
    /// # Arguments
    ///
    /// - `Vector3D` - The impulse vector.
    pub fn apply_impulse(&mut self, impulse: Vector3D) {
        let inverse_mass: f64 = self.get_inverse_mass();
        if inverse_mass == 0.0 {
            return;
        }
        *self.get_mut_velocity() += impulse.scaled(inverse_mass);
    }

    /// Sets the mass of the body, updating the inverse mass.
    /// A mass of 0 makes the body static (infinite mass).
    ///
    /// # Arguments
    ///
    /// - `f64` - The new mass.
    pub fn update_mass(&mut self, mass: f64) {
        self.set_mass(mass);
        self.set_inverse_mass(if mass > 0.0 { 1.0 / mass } else { 0.0 });
    }

    /// Returns `true` if this body is affected by forces and collisions.
    ///
    /// # Returns
    ///
    /// - `bool` - True if the body is dynamic.
    pub fn is_dynamic(&self) -> bool {
        self.get_body_type() == BodyType::Dynamic
    }

    /// Attaches a 3D collider shape to this body.
    ///
    /// # Arguments
    ///
    /// - `BodyCollider3D` - The collider to attach.
    pub fn update_collider(&mut self, collider: BodyCollider3D) {
        self.set_collider(Some(collider));
    }

    /// Returns the world-space 3D bounding box of the attached collider, if any.
    ///
    /// # Returns
    ///
    /// - `Option<AABB3D>` - The bounding box, or `None` if no collider is attached.
    pub fn bounding_box(&self) -> Option<AABB3D> {
        let collider: Option<BodyCollider3D> = self.get_collider();
        let position: Vector3D = self.get_position();
        match collider? {
            BodyCollider3D::Aabb(aabb) => {
                let center: Vector3D = aabb.get_aabb().center();
                let size: Vector3D = aabb.get_aabb().size();
                Some(AABB3D::from_center(
                    position + center,
                    size.get_x(),
                    size.get_y(),
                    size.get_z(),
                ))
            }
            BodyCollider3D::Sphere(sphere) => {
                let sphere_inner: Sphere = sphere.get_sphere();
                let diameter: f64 = sphere_inner.get_radius() * 2.0;
                Some(AABB3D::from_center(
                    position + sphere_inner.get_center(),
                    diameter,
                    diameter,
                    diameter,
                ))
            }
        }
    }
}

/// Implements body management and simulation for `PhysicsWorld3D`.
impl PhysicsWorld3D {
    /// Creates a new 3D physics world with the given configuration.
    ///
    /// # Arguments
    ///
    /// - `PhysicsConfig3D` - The simulation configuration.
    ///
    /// # Returns
    ///
    /// - `PhysicsWorld3D` - The new world.
    pub fn with_config(config: PhysicsConfig3D) -> PhysicsWorld3D {
        let mut world: PhysicsWorld3D = PhysicsWorld3D::new(config);
        world.set_grid(SpatialHashGrid3D::with_default_size());
        world
    }

    /// Adds a rigid body to the world.
    ///
    /// # Arguments
    ///
    /// - `RigidBody3D` - The body to add.
    pub fn add_body(&mut self, body: RigidBody3D) {
        self.get_mut_bodies().push(body);
    }

    /// Removes the body with the given ID.
    ///
    /// # Arguments
    ///
    /// - `u64` - The ID of the body to remove.
    pub fn remove_body(&mut self, id: u64) {
        self.get_mut_bodies()
            .retain(|body: &RigidBody3D| body.get_id() != id);
    }

    /// Returns a reference to the body with the given ID.
    ///
    /// # Arguments
    ///
    /// - `u64` - The body ID.
    ///
    /// # Returns
    ///
    /// - `Option<&RigidBody3D>` - The body reference, if found.
    pub fn get_body(&self, id: u64) -> Option<&RigidBody3D> {
        self.get_bodies()
            .iter()
            .find(|body: &&RigidBody3D| body.get_id() == id)
    }

    /// Returns a mutable reference to the body with the given ID.
    ///
    /// # Arguments
    ///
    /// - `u64` - The body ID.
    ///
    /// # Returns
    ///
    /// - `Option<&mut RigidBody3D>` - The mutable body reference, if found.
    pub fn get_body_mut(&mut self, id: u64) -> Option<&mut RigidBody3D> {
        self.get_mut_bodies()
            .iter_mut()
            .find(|body: &&mut RigidBody3D| body.get_id() == id)
    }

    /// Performs one physics simulation step using semi-implicit Euler integration.
    ///
    /// Applies gravity to dynamic bodies, integrates velocity from accumulated forces,
    /// applies damping, integrates position, and resolves collisions.
    ///
    /// # Arguments
    ///
    /// - `f64` - The fixed delta time in seconds.
    pub fn step(&mut self, delta_time: f64) {
        let config: PhysicsConfig3D = self.get_config();
        // Hoist loop-invariant damping factors out of the per-body loop.
        let damping_factor: f64 = (1.0 - config.get_linear_damping() * delta_time).max(0.0);
        let angular_damping: f64 = (1.0 - config.get_angular_damping() * delta_time).max(0.0);
        let gravity: Vector3D = config.get_gravity();
        for body in self.get_mut_bodies() {
            if !body.is_dynamic() {
                continue;
            }
            let body_mass: f64 = body.get_mass();
            let body_inverse_mass: f64 = body.get_inverse_mass();
            *body.get_mut_force_accumulator() += gravity.scaled(body_mass);
            let force: Vector3D = body.get_force_accumulator();
            *body.get_mut_velocity() += force.scaled(body_inverse_mass * delta_time);
            // In-place damping and integration avoid temporary vector copies.
            *body.get_mut_velocity() *= damping_factor;
            let current_velocity: Vector3D = body.get_velocity();
            *body.get_mut_position() += current_velocity.scaled(delta_time);
            body.set_force_accumulator(Vector3D::zero());
            *body.get_mut_angular_velocity() *= angular_damping;
            let angular_velocity: Vector3D = body.get_angular_velocity();
            let rotation_delta: Quaternion = Quaternion::new(
                angular_velocity.get_x() * delta_time * 0.5,
                angular_velocity.get_y() * delta_time * 0.5,
                angular_velocity.get_z() * delta_time * 0.5,
                1.0,
            );
            body.set_rotation((rotation_delta * body.get_rotation()).normalized());
            body.set_torque_accumulator(Vector3D::zero());
        }
        self.resolve_collisions();
    }

    /// Detects and resolves all collisions between bodies in the 3D world.
    ///
    /// Uses a spatial hash grid for broad-phase culling followed by narrow-phase
    /// shape-specific collision detection, then applies impulse-based resolution.
    /// This reduces the broad-phase from O(n²) to near O(n) for typical scenes.
    fn resolve_collisions(&mut self) {
        let body_count: usize = self.get_bodies().len();
        if body_count < 2 {
            return;
        }
        // Rebuild the persistent grid once per step and collect the candidate
        // pair list once; every solver iteration then reuses both (the grid is
        // unchanged between iterations), eliminating per-iteration re-queries and
        // per-query allocations.
        let mut pairs: Vec<(usize, usize)> = Vec::new();
        {
            let (bodies, grid, query_buffer, query_seen) = (
                &self.bodies,
                &mut self.grid,
                &mut self.query_buffer,
                &mut self.query_seen,
            );
            grid.clear();
            for (index, body) in bodies.iter().enumerate() {
                if let Some(bbox) = body.bounding_box() {
                    grid.insert(index, bbox.get_min(), bbox.get_max());
                }
            }
            for (i, body) in bodies.iter().enumerate() {
                let Some(bbox) = body.bounding_box() else {
                    continue;
                };
                grid.query_into(bbox.get_min(), bbox.get_max(), query_buffer, query_seen);
                for &j in query_buffer.iter() {
                    if j > i {
                        pairs.push((i, j));
                    }
                }
            }
        }
        for iteration in 0..PHYSICS_MAX_ITERATIONS {
            let mut any_collision: bool = false;
            for &(i, j) in pairs.iter() {
                let (left, right) = self.get_mut_bodies().split_at_mut(j);
                let body_a: &mut RigidBody3D = &mut left[i];
                let body_b: &mut RigidBody3D = &mut right[0];
                if body_a.get_inverse_mass() == 0.0 && body_b.get_inverse_mass() == 0.0 {
                    continue;
                }
                if let Some(result) = Self::check_collision_3d(body_a, body_b) {
                    Self::resolve_collision_3d(body_a, body_b, &result);
                    any_collision = true;
                }
            }
            if !any_collision {
                break;
            }
            let _ = iteration;
        }
    }

    /// Checks collision between two 3D bodies based on both bodies' collider shapes.
    ///
    /// # Arguments
    ///
    /// - `&RigidBody3D` - The first body.
    /// - `&RigidBody3D` - The second body.
    ///
    /// # Returns
    ///
    /// - `Option<CollisionResult3D>` - The collision result, or `None`.
    fn check_collision_3d(a: &RigidBody3D, b: &RigidBody3D) -> Option<CollisionResult3D> {
        let a_bbox: AABB3D = a.bounding_box()?;
        let b_bbox: AABB3D = b.bounding_box()?;
        if !AABB3D::broad_phase(a_bbox, b_bbox) {
            return None;
        }
        let a_collider: Option<BodyCollider3D> = a.get_collider();
        let b_collider: Option<BodyCollider3D> = b.get_collider();
        let position_delta: Vector3D = b.get_position() - a.get_position();
        match (a_collider, b_collider) {
            (Some(BodyCollider3D::Aabb(aabb_a)), Some(BodyCollider3D::Aabb(aabb_b))) => {
                let aabb_b_inner: AABB3D = aabb_b.get_aabb();
                let offset_aabb: AabbCollider3D = AabbCollider3D::new(AABB3D::new(
                    aabb_b_inner.get_min() + position_delta,
                    aabb_b_inner.get_max() + position_delta,
                ));
                aabb_a.collide_with_aabb(&offset_aabb)
            }
            (Some(BodyCollider3D::Sphere(sphere_a)), Some(BodyCollider3D::Sphere(sphere_b))) => {
                let sphere_b_inner: Sphere = sphere_b.get_sphere();
                let offset_sphere: SphereCollider3D = SphereCollider3D::new(Sphere::new(
                    sphere_b_inner.get_center() + position_delta,
                    sphere_b_inner.get_radius(),
                ));
                sphere_a.collide_with_sphere(&offset_sphere)
            }
            (Some(BodyCollider3D::Aabb(aabb)), Some(BodyCollider3D::Sphere(sphere))) => {
                let sphere_inner: Sphere = sphere.get_sphere();
                let offset_sphere: SphereCollider3D = SphereCollider3D::new(Sphere::new(
                    sphere_inner.get_center() + position_delta,
                    sphere_inner.get_radius(),
                ));
                aabb.collide_with_sphere(&offset_sphere)
            }
            (Some(BodyCollider3D::Sphere(sphere)), Some(BodyCollider3D::Aabb(aabb))) => {
                let aabb_inner: AABB3D = aabb.get_aabb();
                let offset_aabb: AabbCollider3D = AabbCollider3D::new(AABB3D::new(
                    aabb_inner.get_min() + position_delta,
                    aabb_inner.get_max() + position_delta,
                ));
                offset_aabb
                    .collide_with_sphere(&sphere)
                    .map(|mut result: CollisionResult3D| {
                        result.set_normal(-result.get_normal());
                        result
                    })
            }
            _ => None,
        }
    }

    /// Resolves a collision between two 3D bodies using impulse-based response
    /// and position correction.
    ///
    /// # Arguments
    ///
    /// - `&mut RigidBody3D` - The first body.
    /// - `&mut RigidBody3D` - The second body.
    /// - `&CollisionResult3D` - The collision data.
    fn resolve_collision_3d(a: &mut RigidBody3D, b: &mut RigidBody3D, result: &CollisionResult3D) {
        let a_inverse_mass: f64 = a.get_inverse_mass();
        let b_inverse_mass: f64 = b.get_inverse_mass();
        let relative_velocity: Vector3D = b.get_velocity() - a.get_velocity();
        let velocity_along_normal: f64 = relative_velocity.dot(result.get_normal());
        if velocity_along_normal > 0.0 {
            return;
        }
        let restitution: f64 = a.get_restitution().min(b.get_restitution());
        let inverse_mass_sum: f64 = a_inverse_mass + b_inverse_mass;
        if inverse_mass_sum == 0.0 {
            return;
        }
        let impulse_magnitude: f64 =
            -(1.0 + restitution) * velocity_along_normal / inverse_mass_sum;
        let impulse: Vector3D = result.get_normal().scaled(impulse_magnitude);
        *a.get_mut_velocity() -= impulse.scaled(a_inverse_mass);
        *b.get_mut_velocity() += impulse.scaled(b_inverse_mass);
        let correction: Vector3D = result
            .get_normal()
            .scaled((result.get_depth() * PHYSICS_POSITION_PERCENT / inverse_mass_sum).max(0.0));
        *a.get_mut_position() -= correction.scaled(a_inverse_mass);
        *b.get_mut_position() += correction.scaled(b_inverse_mass);
    }
}

/// Implements `Default` for `PhysicsWorld3D` as an empty world.
impl Default for PhysicsWorld3D {
    fn default() -> PhysicsWorld3D {
        PhysicsWorld3D::with_config(PhysicsConfig3D::default())
    }
}