diff --git a/src/dynamics/rigid_body/world_query.rs b/src/dynamics/rigid_body/world_query.rs index 3e56b05c2..15d9da066 100644 --- a/src/dynamics/rigid_body/world_query.rs +++ b/src/dynamics/rigid_body/world_query.rs @@ -25,6 +25,8 @@ pub struct RigidBodyQuery { #[cfg(feature = "3d")] pub global_angular_inertia: &'static mut GlobalAngularInertia, pub center_of_mass: &'static mut ComputedCenterOfMass, + pub max_linear_speed: Option<&'static MaxLinearSpeed>, + pub max_angular_speed: Option<&'static MaxAngularSpeed>, pub friction: Option<&'static Friction>, pub restitution: Option<&'static Restitution>, pub locked_axes: Option<&'static LockedAxes>, @@ -112,6 +114,30 @@ impl RigidBodyQueryItem<'_> { self.dominance.map_or(0, |dominance| dominance.0) } } + + /// Clamps both [`LinearVelocity`] as well as [`AngularVelocity`] to the + /// limits determined by [`MaxLinearSpeed`] and [`MaxAngularSpeed`], if present + pub fn clamp_velocities(&mut self) { + if let Some(max_linear_speed) = self.max_linear_speed { + let linear_speed_squared = self.linear_velocity.0.length_squared(); + if linear_speed_squared > max_linear_speed.0.powi(2) { + self.linear_velocity.0 *= max_linear_speed.0 / linear_speed_squared.sqrt(); + } + } + if let Some(max_angular_speed) = self.max_angular_speed { + #[cfg(feature = "2d")] + if self.angular_velocity.abs() > max_angular_speed.0 { + self.angular_velocity.0 = max_angular_speed.copysign(self.angular_velocity.0); + } + #[cfg(feature = "3d")] + { + let angular_speed_squared = self.angular_velocity.0.length_squared(); + if angular_speed_squared > max_angular_speed.0.powi(2) { + self.angular_velocity.0 *= max_angular_speed.0 / angular_speed_squared.sqrt(); + } + } + } + } } impl RigidBodyQueryReadOnlyItem<'_> { diff --git a/src/dynamics/solver/contact/mod.rs b/src/dynamics/solver/contact/mod.rs index 6a818bb13..8a58b7f14 100644 --- a/src/dynamics/solver/contact/mod.rs +++ b/src/dynamics/solver/contact/mod.rs @@ -242,10 +242,12 @@ impl ContactConstraint { if body1.rb.is_dynamic() && body1.dominance() <= body2.dominance() { body1.linear_velocity.0 -= p * inv_mass1; body1.angular_velocity.0 -= inv_inertia1 * cross(r1, p); + body1.clamp_velocities(); } if body2.rb.is_dynamic() && body2.dominance() <= body1.dominance() { body2.linear_velocity.0 += p * inv_mass2; body2.angular_velocity.0 += inv_inertia2 * cross(r2, p); + body2.clamp_velocities(); } } } @@ -305,10 +307,12 @@ impl ContactConstraint { if body1.rb.is_dynamic() && body1.dominance() <= body2.dominance() { body1.linear_velocity.0 -= impulse * inv_mass1; body1.angular_velocity.0 -= inv_inertia1 * cross(r1, impulse); + body1.clamp_velocities(); } if body2.rb.is_dynamic() && body2.dominance() <= body1.dominance() { body2.linear_velocity.0 += impulse * inv_mass2; body2.angular_velocity.0 += inv_inertia2 * cross(r2, impulse); + body2.clamp_velocities(); } } @@ -341,10 +345,12 @@ impl ContactConstraint { if body1.rb.is_dynamic() && body1.dominance() <= body2.dominance() { body1.linear_velocity.0 -= impulse * inv_mass1; body1.angular_velocity.0 -= inv_inertia1 * cross(r1, impulse); + body1.clamp_velocities(); } if body2.rb.is_dynamic() && body2.dominance() <= body1.dominance() { body2.linear_velocity.0 += impulse * inv_mass2; body2.angular_velocity.0 += inv_inertia2 * cross(r2, impulse); + body2.clamp_velocities(); } } } @@ -393,10 +399,12 @@ impl ContactConstraint { if body1.rb.is_dynamic() && body1.dominance() <= body2.dominance() { body1.linear_velocity.0 -= impulse * inv_mass1; body1.angular_velocity.0 -= inv_inertia1 * cross(r1, impulse); + body1.clamp_velocities(); } if body2.rb.is_dynamic() && body2.dominance() <= body1.dominance() { body2.linear_velocity.0 += impulse * inv_mass2; body2.angular_velocity.0 += inv_inertia2 * cross(r2, impulse); + body2.clamp_velocities(); } } }