diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index 73b8969..210713b 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -75,8 +75,10 @@ pub struct RigidBody { pub linear_damping: Real, /// Damping factor for gradually slowing down the angular motion of the rigid-body. pub angular_damping: Real, - pub(crate) linacc: Vector, - pub(crate) angacc: AngVector, + /// Accumulation of external forces (only for dynamic bodies). + pub(crate) force: Vector, + /// Accumulation of external torques (only for dynamic bodies). + pub(crate) torque: AngVector, pub(crate) colliders: Vec, pub(crate) gravity_scale: Real, /// Whether or not this rigid-body is sleeping. @@ -105,8 +107,8 @@ impl RigidBody { effective_world_inv_inertia_sqrt: AngularInertia::zero(), linvel: Vector::zeros(), angvel: na::zero(), - linacc: Vector::zeros(), - angacc: na::zero(), + force: Vector::zeros(), + torque: na::zero(), gravity_scale: 1.0, linear_damping: 0.0, angular_damping: 0.0, @@ -135,12 +137,15 @@ impl RigidBody { pub(crate) fn integrate_accelerations(&mut self, dt: Real, gravity: Vector) { if self.effective_inv_mass != 0.0 { - self.linvel += (gravity * self.gravity_scale + self.linacc) * dt; - self.linacc = na::zero(); + let acceleration = self.force * self.effective_inv_mass; + self.linvel += (gravity * self.gravity_scale + acceleration) * dt; + self.force = na::zero(); } - self.angvel += self.angacc * dt; - self.angacc = na::zero(); + let angular_acceleration = self.effective_world_inv_inertia_sqrt + * (self.effective_world_inv_inertia_sqrt * self.torque); + self.angvel += angular_acceleration * dt; + self.torque = na::zero(); } /// The mass properties of this rigid-body. @@ -467,14 +472,16 @@ impl RigidBody { } } } +} - /* - * Application of forces/impulses. - */ +/// ## Applying forces and torques +impl RigidBody { /// Applies a force at the center-of-mass of this rigid-body. + /// The force will be applied in the next simulation step. + /// This does nothing on non-dynamic bodies. pub fn apply_force(&mut self, force: Vector, wake_up: bool) { if self.body_status == BodyStatus::Dynamic { - self.linacc += force * self.effective_inv_mass; + self.force += force; if wake_up { self.wake_up(true); @@ -482,7 +489,54 @@ impl RigidBody { } } + /// Applies a torque at the center-of-mass of this rigid-body. + /// The torque will be applied in the next simulation step. + /// This does nothing on non-dynamic bodies. + #[cfg(feature = "dim2")] + pub fn apply_torque(&mut self, torque: Real, wake_up: bool) { + if self.body_status == BodyStatus::Dynamic { + self.torque += torque; + + if wake_up { + self.wake_up(true); + } + } + } + + /// Applies a torque at the center-of-mass of this rigid-body. + /// The torque will be applied in the next simulation step. + /// This does nothing on non-dynamic bodies. + #[cfg(feature = "dim3")] + pub fn apply_torque(&mut self, torque: Vector, wake_up: bool) { + if self.body_status == BodyStatus::Dynamic { + self.torque += torque; + + if wake_up { + self.wake_up(true); + } + } + } + + /// Applies a force at the given world-space point of this rigid-body. + /// The force will be applied in the next simulation step. + /// This does nothing on non-dynamic bodies. + pub fn apply_force_at_point(&mut self, force: Vector, point: Point, wake_up: bool) { + if self.body_status == BodyStatus::Dynamic { + self.force += force; + self.torque += (point - self.world_com).gcross(force); + + if wake_up { + self.wake_up(true); + } + } + } +} + +/// ## Applying impulses and angular impulses +impl RigidBody { /// Applies an impulse at the center-of-mass of this rigid-body. + /// The impulse is applied right away, changing the linear velocity. + /// This does nothing on non-dynamic bodies. pub fn apply_impulse(&mut self, impulse: Vector, wake_up: bool) { if self.body_status == BodyStatus::Dynamic { self.linvel += impulse * self.effective_inv_mass; @@ -493,33 +547,9 @@ impl RigidBody { } } - /// Applies a torque at the center-of-mass of this rigid-body. - #[cfg(feature = "dim2")] - pub fn apply_torque(&mut self, torque: Real, wake_up: bool) { - if self.body_status == BodyStatus::Dynamic { - self.angacc += self.effective_world_inv_inertia_sqrt - * (self.effective_world_inv_inertia_sqrt * torque); - - if wake_up { - self.wake_up(true); - } - } - } - - /// Applies a torque at the center-of-mass of this rigid-body. - #[cfg(feature = "dim3")] - pub fn apply_torque(&mut self, torque: Vector, wake_up: bool) { - if self.body_status == BodyStatus::Dynamic { - self.angacc += self.effective_world_inv_inertia_sqrt - * (self.effective_world_inv_inertia_sqrt * torque); - - if wake_up { - self.wake_up(true); - } - } - } - - /// Applies an impulsive torque at the center-of-mass of this rigid-body. + /// Applies an angular impulse at the center-of-mass of this rigid-body. + /// The impulse is applied right away, changing the angular velocity. + /// This does nothing on non-dynamic bodies. #[cfg(feature = "dim2")] pub fn apply_torque_impulse(&mut self, torque_impulse: Real, wake_up: bool) { if self.body_status == BodyStatus::Dynamic { @@ -532,7 +562,9 @@ impl RigidBody { } } - /// Applies an impulsive torque at the center-of-mass of this rigid-body. + /// Applies an angular impulse at the center-of-mass of this rigid-body. + /// The impulse is applied right away, changing the angular velocity. + /// This does nothing on non-dynamic bodies. #[cfg(feature = "dim3")] pub fn apply_torque_impulse(&mut self, torque_impulse: Vector, wake_up: bool) { if self.body_status == BodyStatus::Dynamic { @@ -545,14 +577,9 @@ impl RigidBody { } } - /// Applies a force at the given world-space point of this rigid-body. - pub fn apply_force_at_point(&mut self, force: Vector, point: Point, wake_up: bool) { - let torque = (point - self.world_com).gcross(force); - self.apply_force(force, wake_up); - self.apply_torque(torque, wake_up); - } - /// Applies an impulse at the given world-space point of this rigid-body. + /// The impulse is applied right away, changing the linear and/or angular velocities. + /// This does nothing on non-dynamic bodies. pub fn apply_impulse_at_point( &mut self, impulse: Vector, @@ -563,7 +590,9 @@ impl RigidBody { self.apply_impulse(impulse, wake_up); self.apply_torque_impulse(torque_impulse, wake_up); } +} +impl RigidBody { /// The velocity of the given world-space point on this rigid-body. pub fn velocity_at_point(&self, point: &Point) -> Vector { let dpt = point - self.world_com;