diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/dynamics/rigid_body_components.rs | 8 | ||||
| -rw-r--r-- | src/pipeline/physics_pipeline.rs | 5 |
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 => { |