Fix velocity computation for position-based kinematic bodies

2 files changed, 10 insertions, 3 deletions

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<Real>) -> 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 }
     }
 
diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs
index 80d75eb..4ba8bfa 100644
--- a/src/pipeline/physics_pipeline.rs
+++ b/src/pipeline/physics_pipeline.rs
@@ -450,7 +450,10 @@ impl PhysicsPipeline {
             match rb_type {
                 RigidBodyType::KinematicPositionBased => {
                     let rb_pos: &RigidBodyPosition = bodies.index(handle.0);
-                    let new_vel = rb_pos.interpolate_velocity(integration_parameters.inv_dt());
+                    let new_vel = rb_pos.interpolate_velocity(
+                        integration_parameters.inv_dt(),
+                        &rb_mprops.local_mprops.local_com,
+                    );
                     bodies.set_internal(handle.0, new_vel);
                 }
                 RigidBodyType::KinematicVelocityBased => {