euv-engine 0.9.0

use crate::*;

/// 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 {
        PhysicsWorld2D::new(config)
    }

    /// 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::new(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_check(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();
        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() += config.get_gravity().scaled(body_mass);
            let force: Vector2D = body.get_force_accumulator();
            *body.get_mut_velocity() += force.scaled(body_inverse_mass * delta_time);
            let damping_factor: f64 = (1.0 - config.get_linear_damping() * delta_time).max(0.0);
            let velocity: Vector2D = body.get_velocity();
            body.set_velocity(velocity.scaled(damping_factor));
            let current_velocity: Vector2D = body.get_velocity();
            *body.get_mut_position() += current_velocity.scaled(delta_time);
            body.set_force_accumulator(Vector2D::zero());
            let angular_damping: f64 = (1.0 - config.get_angular_damping() * delta_time).max(0.0);
            let angular_velocity: f64 = body.get_angular_velocity();
            *body.get_mut_angular_velocity() = 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 broad-phase bounding box checks followed by narrow-phase shape-specific
    /// collision detection, then applies impulse-based resolution.
    fn resolve_collisions(&mut self) {
        let body_count: usize = self.get_bodies().len();
        for iteration in 0..PHYSICS_MAX_ITERATIONS {
            let mut any_collision: bool = false;
            for i in 0..body_count {
                for j in (i + 1)..body_count {
                    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 {
        PhysicsWorld3D::new(config)
    }

    /// 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();
        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() += config.get_gravity().scaled(body_mass);
            let force: Vector3D = body.get_force_accumulator();
            *body.get_mut_velocity() += force.scaled(body_inverse_mass * delta_time);
            let damping_factor: f64 = (1.0 - config.get_linear_damping() * delta_time).max(0.0);
            let velocity: Vector3D = body.get_velocity();
            body.set_velocity(velocity.scaled(damping_factor));
            let current_velocity: Vector3D = body.get_velocity();
            *body.get_mut_position() += current_velocity.scaled(delta_time);
            body.set_force_accumulator(Vector3D::zero());
            let angular_damping: f64 = (1.0 - config.get_angular_damping() * delta_time).max(0.0);
            let angular_velocity: Vector3D = body.get_angular_velocity().scaled(angular_damping);
            body.set_angular_velocity(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 broad-phase bounding box checks followed by narrow-phase shape-specific
    /// collision detection, then applies impulse-based resolution.
    fn resolve_collisions(&mut self) {
        let body_count: usize = self.get_bodies().len();
        for iteration in 0..PHYSICS_MAX_ITERATIONS {
            let mut any_collision: bool = false;
            for i in 0..body_count {
                for j in (i + 1)..body_count {
                    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_check(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::new(PhysicsConfig3D::default())
    }
}