Fix velocity computation for position-based kinematic bodies

3 files changed, 70 insertions, 31 deletions

diff --git a/examples2d/add_remove2.rs b/examples2d/add_remove2.rs
index 056e63e..4c830c3 100644
--- a/examples2d/add_remove2.rs
+++ b/examples2d/add_remove2.rs
@@ -2,44 +2,76 @@ use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
 
 pub fn init_world(testbed: &mut Testbed) {
-    let bodies = RigidBodySet::new();
-    let colliders = ColliderSet::new();
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
     let joints = JointSet::new();
     let rad = 0.5;
 
+    let positions = [vector![5.0, -1.0], vector![-5.0, -1.0]];
+
+    let platform_handles = std::array::IntoIter::new(positions)
+        .map(|pos| {
+            let rigid_body = RigidBodyBuilder::new_kinematic_position_based()
+                .translation(pos)
+                // .ccd_enabled(true)
+                .build();
+            let handle = bodies.insert(rigid_body);
+            let collider = ColliderBuilder::cuboid(rad * 10.0, rad)
+                .active_events(ActiveEvents::CONTACT_EVENTS)
+                .build();
+            let coll_handle = colliders.insert_with_parent(collider, handle, &mut bodies);
+            (pos, handle, coll_handle)
+        })
+        .collect::<Vec<_>>();
+
+    let mut rotation = 0.0;
+
     // Callback that will be executed on the main loop to handle proximities.
-    testbed.add_callback(move |mut graphics, physics, _, _| {
-        let rigid_body = RigidBodyBuilder::new_dynamic()
-            .translation(vector![0.0, 10.0])
-            .build();
-        let handle = physics.bodies.insert(rigid_body);
-        let collider = ColliderBuilder::cuboid(rad, rad).build();
-        physics
-            .colliders
-            .insert_with_parent(collider, handle, &mut physics.bodies);
-
-        if let Some(graphics) = &mut graphics {
-            graphics.add_body(handle, &physics.bodies, &physics.colliders);
+    testbed.add_callback(move |mut graphics, physics, events, state| {
+        let rot = state.time * -1.0;
+        let pos = state.time.sin() * 5.0;
+        for (p, i, _) in platform_handles.iter() {
+            let mut c = physics.bodies.get_mut(*i).unwrap();
+            c.set_next_kinematic_rotation(rot);
+            //c.set_next_kinematic_translation(vector![pos, 0.0]);
         }
 
-        let to_remove: Vec<_> = physics
-            .bodies
-            .iter()
-            .filter(|(_, b)| b.position().translation.vector.y < -10.0)
-            .map(|e| e.0)
-            .collect();
-        for handle in to_remove {
-            physics.bodies.remove(
-                handle,
-                &mut physics.islands,
-                &mut physics.colliders,
-                &mut physics.joints,
-            );
+        if state.timestep_id % 10 == 0 {
+            let x = rand::random::<f32>() * 10.0 - 5.0;
+            let y = rand::random::<f32>() * 10.0 + 10.0;
+            let rigid_body = RigidBodyBuilder::new_dynamic()
+                .translation(vector![x, y])
+                // .ccd_enabled(true)
+                .build();
+            let handle = physics.bodies.insert(rigid_body);
+            let collider = ColliderBuilder::cuboid(rad, rad).build();
+            physics
+                .colliders
+                .insert_with_parent(collider, handle, &mut physics.bodies);
 
             if let Some(graphics) = &mut graphics {
-                graphics.remove_body(handle);
+                graphics.add_body(handle, &physics.bodies, &physics.colliders);
             }
         }
+
+        // let to_remove: Vec<_> = physics
+        //     .bodies
+        //     .iter()
+        //     .filter(|(_, b)| b.position().translation.vector.y < -10.0)
+        //     .map(|e| e.0)
+        //     .collect();
+        // for handle in to_remove {
+        //     physics.bodies.remove(
+        //         handle,
+        //         &mut physics.islands,
+        //         &mut physics.colliders,
+        //         &mut physics.joints,
+        //     );
+        //
+        //     if let Some(graphics) = &mut graphics {
+        //         graphics.remove_body(handle);
+        //     }
+        // }
     });
 
     /*
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 => {