From a2c172b208af8c5fdd58f1b39be439ef847a9944 Mon Sep 17 00:00:00 2001 From: Marco Buono Date: Fri, 7 Feb 2025 00:51:28 -0300 Subject: [PATCH 1/3] Also clamp velocities to max speeds while solving constraints --- src/dynamics/rigid_body/world_query.rs | 26 ++++++++++++++++++++++++++ src/dynamics/solver/contact/mod.rs | 8 ++++++++ 2 files changed, 34 insertions(+) diff --git a/src/dynamics/rigid_body/world_query.rs b/src/dynamics/rigid_body/world_query.rs index 3e56b05c2..8c81f8d9f 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.ang_vel.abs() > max_angular_speed.0 { + self.ang_vel.0 = max_angular_speed.copysign(self.ang_vel.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(); } } } From 0a841fe50c1944d9f77923b04ce983121f54cba6 Mon Sep 17 00:00:00 2001 From: Marco Buono Date: Fri, 7 Feb 2025 01:48:27 -0300 Subject: [PATCH 2/3] Fix wrong field name when feature 2d is on --- src/dynamics/rigid_body/world_query.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/dynamics/rigid_body/world_query.rs b/src/dynamics/rigid_body/world_query.rs index 8c81f8d9f..a30dc3bef 100644 --- a/src/dynamics/rigid_body/world_query.rs +++ b/src/dynamics/rigid_body/world_query.rs @@ -126,8 +126,8 @@ impl RigidBodyQueryItem<'_> { } if let Some(max_angular_speed) = self.max_angular_speed { #[cfg(feature = "2d")] - if self.ang_vel.abs() > max_angular_speed.0 { - self.ang_vel.0 = max_angular_speed.copysign(self.ang_vel.0); + if self.angular_velocity.abs() > max_angular_speed.0 { + self.angular_velocity.0 = max_angular_speed.copysign(self.angular_velocity.0); } #[cfg(feature = "3d")] { From d1397e170349fb56491c60010edf5602be4537a0 Mon Sep 17 00:00:00 2001 From: Marco Buono Date: Fri, 7 Feb 2025 01:52:14 -0300 Subject: [PATCH 3/3] Fix documentation link lint error --- src/dynamics/rigid_body/world_query.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/dynamics/rigid_body/world_query.rs b/src/dynamics/rigid_body/world_query.rs index a30dc3bef..15d9da066 100644 --- a/src/dynamics/rigid_body/world_query.rs +++ b/src/dynamics/rigid_body/world_query.rs @@ -115,8 +115,8 @@ impl RigidBodyQueryItem<'_> { } } - /// Clamps both [LinearVelocity] as well as [AngularVelocity] to the - /// limits determined by [MaxLinearSpeed] and [MaxAngularSpeed], if present + /// 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();