From b364a2b052f6a846e0d040a756c13ee6a7f5ced8 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sun, 12 Sep 2021 09:53:50 +0200 Subject: Fix velocity computation for position-based kinematic bodies --- src/dynamics/rigid_body_components.rs | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) (limited to 'src/dynamics') diff --git a/src/dynamics/rigid_body_components.rs b/src/dynamics/rigid_body_components.rs index dcd92f7..82ecacf 100644 --- a/src/dynamics/rigid_body_components.rs +++ b/src/dynamics/rigid_body_components.rs @@ -147,8 +147,11 @@ impl RigidBodyPosition { /// Computes the velocity need to travel from `self.position` to `self.next_position` in /// a time equal to `1.0 / inv_dt`. #[must_use] - pub fn interpolate_velocity(&self, inv_dt: Real) -> RigidBodyVelocity { - let dpos = self.next_position * self.position.inverse(); + pub fn interpolate_velocity(&self, inv_dt: Real, local_com: &Point) -> RigidBodyVelocity { + let com = self.position * local_com; + let shift = Translation::from(com.coords); + let dpos = shift.inverse() * self.next_position * self.position.inverse() * shift; + let angvel; #[cfg(feature = "dim2")] { @@ -159,6 +162,7 @@ impl RigidBodyPosition { angvel = dpos.rotation.scaled_axis() * inv_dt; } let linvel = dpos.translation.vector * inv_dt; + RigidBodyVelocity { linvel, angvel } } -- cgit