Merge pull request #334 from dimforge/fixes

Some CCD and debug-render improvements

35 files changed, 419 insertions, 208 deletions

diff --git a/CHANGELOG.md b/CHANGELOG.md
index c4516ba..e46ad40 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -1,3 +1,11 @@
+## Unreleased
+### Modified
+- Add a `wake_up: bool` argument to the `ImpulseJointSet::insert` and `MultibodyJointSet::insert` to
+  automatically wake-up the rigid-bodies attached to the inserted joint.
+- The methods `ImpulseJointSet::remove/remove_joints_attached_to_rigid_body`,
+  `MultibodyJointSet::remove/remove_joints_attached_to_rigid_body` and
+  `MultibodyjointSet::remove_multibody_articulations` no longer require the `bodies`
+  and `islands` arguments.
 
 ## v0.12.0 (30 Apr. 2022)
 ### Fixed
diff --git a/benchmarks2d/joint_ball2.rs b/benchmarks2d/joint_ball2.rs
index 9fd61ff..1988274 100644
--- a/benchmarks2d/joint_ball2.rs
+++ b/benchmarks2d/joint_ball2.rs
@@ -42,7 +42,7 @@ pub fn init_world(testbed: &mut Testbed) {
             if i > 0 {
                 let parent_handle = *body_handles.last().unwrap();
                 let joint = RevoluteJointBuilder::new().local_anchor2(point![0.0, shift]);
-                impulse_joints.insert(parent_handle, child_handle, joint);
+                impulse_joints.insert(parent_handle, child_handle, joint, true);
             }
 
             // Horizontal joint.
@@ -50,7 +50,7 @@ pub fn init_world(testbed: &mut Testbed) {
                 let parent_index = body_handles.len() - numi;
                 let parent_handle = body_handles[parent_index];
                 let joint = RevoluteJointBuilder::new().local_anchor2(point![-shift, 0.0]);
-                impulse_joints.insert(parent_handle, child_handle, joint);
+                impulse_joints.insert(parent_handle, child_handle, joint, true);
             }
 
             body_handles.push(child_handle);
diff --git a/benchmarks2d/joint_fixed2.rs b/benchmarks2d/joint_fixed2.rs
index 36ff62e..c4cabcb 100644
--- a/benchmarks2d/joint_fixed2.rs
+++ b/benchmarks2d/joint_fixed2.rs
@@ -48,7 +48,7 @@ pub fn init_world(testbed: &mut Testbed) {
                         let parent_handle = *body_handles.last().unwrap();
                         let joint = FixedJointBuilder::new()
                             .local_frame2(Isometry::translation(0.0, shift));
-                        impulse_joints.insert(parent_handle, child_handle, joint);
+                        impulse_joints.insert(parent_handle, child_handle, joint, true);
                     }
 
                     // Horizontal joint.
@@ -57,7 +57,7 @@ pub fn init_world(testbed: &mut Testbed) {
                         let parent_handle = body_handles[parent_index];
                         let joint = FixedJointBuilder::new()
                             .local_frame2(Isometry::translation(-shift, 0.0));
-                        impulse_joints.insert(parent_handle, child_handle, joint);
+                        impulse_joints.insert(parent_handle, child_handle, joint, true);
                     }
 
                     body_handles.push(child_handle);
diff --git a/benchmarks2d/joint_prismatic2.rs b/benchmarks2d/joint_prismatic2.rs
index d4d328b..676a433 100644
--- a/benchmarks2d/joint_prismatic2.rs
+++ b/benchmarks2d/joint_prismatic2.rs
@@ -46,7 +46,7 @@ pub fn init_world(testbed: &mut Testbed) {
                 let prism = PrismaticJointBuilder::new(axis)
                     .local_anchor2(point![0.0, shift])
                     .limits([-1.5, 1.5]);
-                impulse_joints.insert(curr_parent, curr_child, prism);
+                impulse_joints.insert(curr_parent, curr_child, prism, true);
 
                 curr_parent = curr_child;
             }
diff --git a/benchmarks3d/joint_ball3.rs b/benchmarks3d/joint_ball3.rs
index f137c69..a968138 100644
--- a/benchmarks3d/joint_ball3.rs
+++ b/benchmarks3d/joint_ball3.rs
@@ -37,7 +37,7 @@ pub fn init_world(testbed: &mut Testbed) {
             if i > 0 {
                 let parent_handle = *body_handles.last().unwrap();
                 let joint = SphericalJointBuilder::new().local_anchor2(point![0.0, 0.0, -shift]);
-                impulse_joints.insert(parent_handle, child_handle, joint);
+                impulse_joints.insert(parent_handle, child_handle, joint, true);
             }
 
             // Horizontal joint.
@@ -45,7 +45,7 @@ pub fn init_world(testbed: &mut Testbed) {
                 let parent_index = body_handles.len() - num;
                 let parent_handle = body_handles[parent_index];
                 let joint = SphericalJointBuilder::new().local_anchor2(point![-shift, 0.0, 0.0]);
-                impulse_joints.insert(parent_handle, child_handle, joint);
+                impulse_joints.insert(parent_handle, child_handle, joint, true);
             }
 
             body_handles.push(child_handle);
diff --git a/benchmarks3d/joint_fixed3.rs b/benchmarks3d/joint_fixed3.rs
index b15515c..c949857 100644
--- a/benchmarks3d/joint_fixed3.rs
+++ b/benchmarks3d/joint_fixed3.rs
@@ -54,7 +54,7 @@ pub fn init_world(testbed: &mut Testbed) {
                             let parent_handle = *body_handles.last().unwrap();
                             let joint =
                                 FixedJointBuilder::new().local_anchor2(point![0.0, 0.0, -shift]);
-                            impulse_joints.insert(parent_handle, child_handle, joint);
+                            impulse_joints.insert(parent_handle, child_handle, joint, true);
                         }
 
                         // Horizontal joint.
@@ -63,7 +63,7 @@ pub fn init_world(testbed: &mut Testbed) {
                             let parent_handle = body_handles[parent_index];
                             let joint =
                                 FixedJointBuilder::new().local_anchor2(point![-shift, 0.0, 0.0]);
-                            impulse_joints.insert(parent_handle, child_handle, joint);
+                            impulse_joints.insert(parent_handle, child_handle, joint, true);
                         }
 
                         body_handles.push(child_handle);
diff --git a/benchmarks3d/joint_prismatic3.rs b/benchmarks3d/joint_prismatic3.rs
index a97dc56..78953e4 100644
--- a/benchmarks3d/joint_prismatic3.rs
+++ b/benchmarks3d/joint_prismatic3.rs
@@ -45,7 +45,7 @@ pub fn init_world(testbed: &mut Testbed) {
                     let prism = PrismaticJointBuilder::new(axis)
                         .local_anchor2(point![0.0, 0.0, -shift])
                         .limits([-2.0, 0.0]);
-                    impulse_joints.insert(curr_parent, curr_child, prism);
+                    impulse_joints.insert(curr_parent, curr_child, prism, true);
 
                     curr_parent = curr_child;
                 }
diff --git a/benchmarks3d/joint_revolute3.rs b/benchmarks3d/joint_revolute3.rs
index 779248a..d434515 100644
--- a/benchmarks3d/joint_revolute3.rs
+++ b/benchmarks3d/joint_revolute3.rs
@@ -55,10 +55,10 @@ pub fn init_world(testbed: &mut Testbed) {
                     RevoluteJointBuilder::new(x).local_anchor2(point![shift, 0.0, 0.0]),
                 ];
 
-                impulse_joints.insert(curr_parent, handles[0], revs[0]);
-                impulse_joints.insert(handles[0], handles[1], revs[1]);
-                impulse_joints.insert(handles[1], handles[2], revs[2]);
-                impulse_joints.insert(handles[2], handles[3], revs[3]);
+                impulse_joints.insert(curr_parent, handles[0], revs[0], true);
+                impulse_joints.insert(handles[0], handles[1], revs[1], true);
+                impulse_joints.insert(handles[1], handles[2], revs[2], true);
+                impulse_joints.insert(handles[2], handles[3], revs[3], true);
 
                 curr_parent = handles[3];
             }
diff --git a/examples2d/joints2.rs b/examples2d/joints2.rs
index 31fda33..6162589 100644
--- a/examples2d/joints2.rs
+++ b/examples2d/joints2.rs
@@ -45,7 +45,7 @@ pub fn init_world(testbed: &mut Testbed) {
             if i > 0 {
                 let parent_handle = *body_handles.last().unwrap();
                 let joint = RevoluteJointBuilder::new().local_anchor2(point![0.0, shift]);
-                impulse_joints.insert(parent_handle, child_handle, joint);
+                impulse_joints.insert(parent_handle, child_handle, joint, true);
             }
 
             // Horizontal joint.
@@ -53,7 +53,7 @@ pub fn init_world(testbed: &mut Testbed) {
                 let parent_index = body_handles.len() - numi;
                 let parent_handle = body_handles[parent_index];
                 let joint = RevoluteJointBuilder::new().local_anchor2(point![-shift, 0.0]);
-                impulse_joints.insert(parent_handle, child_handle, joint);
+                impulse_joints.insert(parent_handle, child_handle, joint, true);
             }
 
             body_handles.push(child_handle);
diff --git a/examples3d/debug_articulations3.rs b/examples3d/debug_articulations3.rs
index e844630..c7f6cd3 100644
--- a/examples3d/debug_articulations3.rs
+++ b/examples3d/debug_articulations3.rs
@@ -39,7 +39,7 @@ fn create_ball_articulations(
                 let parent_handle = *body_handles.last().unwrap();
                 let joint =
                     SphericalJointBuilder::new().local_anchor2(point![0.0, 0.0, -shift * 2.0]);
-                multibody_joints.insert(parent_handle, child_handle, joint);
+                multibody_joints.insert(parent_handle, child_handle, joint, true);
             }
 
             // Horizontal multibody_joint.
@@ -52,7 +52,7 @@ fn create_ball_articulations(
                 // let joint = FixedJoint::new().local_anchor2(point![-shift, 0.0, 0.0]);
                 // let joint =
                 //     RevoluteJoint::new(Vector::x_axis()).local_anchor2(point![-shift, 0.0, 0.0]);
-                impulse_joints.insert(parent_handle, child_handle, joint);
+                impulse_joints.insert(parent_handle, child_handle, joint, true);
             }
 
             body_handles.push(child_handle);
diff --git a/examples3d/debug_excentric_boxes3.rs b/examples3d/debug_excentric_boxes3.rs
new file mode 100644
index 0000000..938de91
--- /dev/null
+++ b/examples3d/debug_excentric_boxes3.rs
@@ -0,0 +1,44 @@
+use rapier3d::prelude::*;
+use rapier_testbed3d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let impulse_joints = ImpulseJointSet::new();
+    let multibody_joints = MultibodyJointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = 100.1;
+    let ground_height = 0.1;
+
+    let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -ground_height, 0.0]);
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size);
+    colliders.insert_with_parent(collider, handle, &mut bodies);
+
+    // Build the dynamic box rigid body.
+    let (mut vtx, idx) = Cuboid::new(vector![1.0, 1.0, 1.0]).to_trimesh();
+    vtx.iter_mut()
+        .for_each(|pt| *pt += vector![100.0, 100.0, 100.0]);
+    let shape = SharedShape::convex_mesh(vtx, &idx).unwrap();
+
+    for _ in 0..2 {
+        let rigid_body = RigidBodyBuilder::dynamic()
+            .translation(vector![-100.0, -100.0 + 10.0, -100.0])
+            .can_sleep(false);
+        let handle = bodies.insert(rigid_body);
+        let collider = ColliderBuilder::new(shape.clone());
+        colliders.insert_with_parent(collider, handle, &mut bodies);
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![10.0, 10.0, 10.0], Point::origin());
+}
diff --git a/examples3d/debug_prismatic3.rs b/examples3d/debug_prismatic3.rs
index 815f4e5..776c50c 100644
--- a/examples3d/debug_prismatic3.rs
+++ b/examples3d/debug_prismatic3.rs
@@ -36,7 +36,7 @@ fn prismatic_repro(
         let prismatic = PrismaticJointBuilder::new(Vector::y_axis())
             .local_anchor1(point![pos.x, pos.y, pos.z])
             .motor_position(0.0, stiffness, damping);
-        impulse_joints.insert(box_rb, wheel_rb, prismatic);
+        impulse_joints.insert(box_rb, wheel_rb, prismatic, true);
     }
 
     // put a small box under one of the wheels
diff --git a/examples3d/joints3.rs b/examples3d/joints3.rs
index f7c6cb3..dc4deed 100644
--- a/examples3d/joints3.rs
+++ b/examples3d/joints3.rs
@@ -24,9 +24,9 @@ fn create_coupled_joints(
         .coupled_axes(JointAxesMask::Y | JointAxesMask::Z);
 
     if use_articulations {
-        multibody_joints.insert(ground, body1, joint1);
+        multibody_joints.insert(ground, body1, joint1, true);
     } else {
-        impulse_joints.insert(ground, body1, joint1);
+        impulse_joints.insert(ground, body1, joint1, true);
     }
 }
 
@@ -66,9 +66,9 @@ fn create_prismatic_joints(
             .limits([-2.0, 2.0]);
 
         if use_articulations {
-            multibody_joints.insert(curr_parent, curr_child, prism);
+            multibody_joints.insert(curr_parent, curr_child, prism, true);
         } else {
-            impulse_joints.insert(curr_parent, curr_child, prism);
+            impulse_joints.insert(curr_parent, curr_child, prism, true);
         }
         curr_parent = curr_child;
     }
@@ -130,9 +130,9 @@ fn create_actuated_prismatic_joints(
         }
 
         if use_articulations {
-            multibody_joints.insert(curr_parent, curr_child, prism);
+            multibody_joints.insert(curr_parent, curr_child, prism, true);
         } else {
-            impulse_joints.insert(curr_parent, curr_child, prism);
+            impulse_joints.insert(curr_parent, curr_child, prism, true);
         }
 
         curr_parent = curr_child;
@@ -185,15 +185,15 @@ fn create_revolute_joints(
         ];
 
         if use_articulations {
-            multibody_joints.insert(curr_parent, handles[0], revs[0]);
-            multibody_joints.insert(handles[0], handles[1], revs[1]);
-            multibody_joints.insert(handles[1], handles[2], revs[2]);
-            multibody_joints.insert(handles[2], handles[3], revs[3]);
+            multibody_joints.insert(curr_parent, handles[0], revs[0], true);
+            multibody_joints.insert(handles[0], handles[1], revs[1], true);
+            multibody_joints.insert(handles[1], handles[2], revs[2], true);
+            multibody_joints.insert(handles[2], handles[3], revs[3], true);
         } else {
-            impulse_joints.insert(curr_parent, handles[0], revs[0]);
-            impulse_joints.insert(handles[0], handles[1], revs[1]);
-            impulse_joints.insert(handles[1], handles[2], revs[2]);
-            impulse_joints.insert(handles[2], handles[3], revs[3]);
+            impulse_joints.insert(curr_parent, handles[0], revs[0], true);
+            impulse_joints.insert(handles[0], handles[1], revs[1], true);
+            impulse_joints.insert(handles[1], handles[2], revs[2], true);
+            impulse_joints.insert(handles[2], handles[3], revs[3], true);
         }
 
         curr_parent = handles[3];
@@ -228,9 +228,9 @@ fn create_revolute_joints_with_limits(
     //     .coupled_axes(JointAxesMask::ANG_Y | JointAxesMask::ANG_Z);
 
     if use_articulations {
-        multibody_joints.insert(ground, platform1, joint1);
+        multibody_joints.insert(ground, platform1, joint1, true);
     } else {
-        impulse_joints.insert(ground, platform1, joint1);
+        impulse_joints.insert(ground, platform1, joint1, true);
     }
 
     let joint2 = RevoluteJointBuilder::new(z)
@@ -247,9 +247,9 @@ fn create_revolute_joints_with_limits(
     //     .coupled_axes(JointAxesMask::ANG_Y | JointAxesMask::ANG_Z);
 
     if use_articulations {
-        multibody_joints.insert(platform1, platform2, joint2);
+        multibody_joints.insert(platform1, platform2, joint2, true);
     } else {
-        impulse_joints.insert(platform1, platform2, joint2);
+        impulse_joints.insert(platform1, platform2, joint2, true);
     }
 
     // Let’s add a couple of cuboids that will fall on the platforms, triggering the joint limits.
@@ -315,9 +315,9 @@ fn create_fixed_joints(
                 let joint = FixedJointBuilder::new().local_anchor2(point![0.0, 0.0, -shift]);
 
                 if use_articulations {
-                    multibody_joints.insert(parent_handle, child_handle, joint);
+                    multibody_joints.insert(parent_handle, child_handle, joint, true);
                 } else {
-                    impulse_joints.insert(parent_handle, child_handle, joint);
+                    impulse_joints.insert(parent_handle, child_handle, joint, true);
                 }
             }
 
@@ -326,7 +326,7 @@ fn create_fixed_joints(
                 let parent_index = body_handles.len() - 1;
                 let parent_handle = body_handles[parent_index];
                 let joint = FixedJointBuilder::new().local_anchor2(point![-shift, 0.0, 0.0]);
-                impulse_joints.insert(parent_handle, child_handle, joint);
+                impulse_joints.insert(parent_handle, child_handle, joint, true);
             }
 
             body_handles.push(child_handle);
@@ -374,9 +374,9 @@ fn create_spherical_joints(
                     SphericalJointBuilder::new().local_anchor2(point![0.0, 0.0, -shift * 2.0]);
 
                 if use_articulations {
-                    multibody_joints.insert(parent_handle, child_handle, joint);
+                    multibody_joints.insert(parent_handle, child_handle, joint, true);
                 } else {
-                    impulse_joints.insert(parent_handle, child_handle, joint);
+                    impulse_joints.insert(parent_handle, child_handle, joint, true);
                 }
             }
 
@@ -385,7 +385,7 @@ fn create_spherical_joints(
                 let parent_index = body_handles.len() - num;
                 let parent_handle = body_handles[parent_index];
                 let joint = SphericalJointBuilder::new().local_anchor2(point![-shift, 0.0, 0.0]);
-                impulse_joints.insert(parent_handle, child_handle, joint);
+                impulse_joints.insert(parent_handle, child_handle, joint, true);
             }
 
             body_handles.push(child_handle);
@@ -426,11 +426,11 @@ fn create_spherical_joints_with_limits(
         .limits(JointAxis::Y, [-0.3, 0.3]);
 
     if use_articulations {
-        multibody_joints.insert(ground, ball1, joint1);
-        multibody_joints.insert(ball1, ball2, joint2);
+        multibody_joints.insert(ground, ball1, joint1, true);
+        multibody_joints.insert(ball1, ball2, joint2, true);
     } else {
-        impulse_joints.insert(ground, ball1, joint1);
-        impulse_joints.insert(ball1, ball2, joint2);
+        impulse_joints.insert(ground, ball1, joint1, true);
+        impulse_joints.insert(ball1, ball2, joint2, true);
     }
 }
 
@@ -493,9 +493,9 @@ fn create_actuated_revolute_joints(
             }
 
             if use_articulations {
-                multibody_joints.insert(parent_handle, child_handle, joint);
+                multibody_joints.insert(parent_handle, child_handle, joint, true);
             } else {
-                impulse_joints.insert(parent_handle, child_handle, joint);
+                impulse_joints.insert(parent_handle, child_handle, joint, true);
             }
         }
 
@@ -559,9 +559,9 @@ fn create_actuated_spherical_joints(
             }
 
             if use_articulations {
-                multibody_joints.insert(parent_handle, child_handle, joint);
+                multibody_joints.insert(parent_handle, child_handle, joint, true);
             } else {
-                impulse_joints.insert(parent_handle, child_handle, joint);
+                impulse_joints.insert(parent_handle, child_handle, joint, true);
             }
         }
 
diff --git a/src/dynamics/ccd/ccd_solver.rs b/src/dynamics/ccd/ccd_solver.rs
index bdde135..9e0ab8e 100644
--- a/src/dynamics/ccd/ccd_solver.rs
+++ b/src/dynamics/ccd/ccd_solver.rs
@@ -60,12 +60,20 @@ impl CCDSolver {
 
                     let min_toi = (rb.ccd.ccd_thickness
                         * 0.15
-                        * crate::utils::inv(rb.ccd.max_point_velocity(&rb.vels)))
+                        * crate::utils::inv(rb.ccd.max_point_velocity(&rb.integrated_vels)))
                     .min(dt);
-                    // println!("Min toi: {}, Toi: {}", min_toi, toi);
-                    let new_pos = rb
-                        .vels
-                        .integrate(toi.max(min_toi), &rb.pos.position, &local_com);
+                    // println!(
+                    //     "Min toi: {}, Toi: {}, thick: {}, max_vel: {}",
+                    //     min_toi,
+                    //     toi,
+                    //     rb.ccd.ccd_thickness,
+                    //     rb.ccd.max_point_velocity(&rb.integrated_vels)
+                    // );
+                    let new_pos = rb.integrated_vels.integrate(
+                        toi.max(min_toi),
+                        &rb.pos.position,
+                        &local_com,
+                    );
                     rb.pos.next_position = new_pos;
                 }
             }
@@ -95,7 +103,7 @@ impl CCDSolver {
                 } else {
                     None
                 };
-                let moving_fast = rb.ccd.is_moving_fast(dt, &rb.vels, forces);
+                let moving_fast = rb.ccd.is_moving_fast(dt, &rb.integrated_vels, forces);
                 rb.ccd.ccd_active = moving_fast;
                 ccd_active = ccd_active || moving_fast;
             }
@@ -131,7 +139,7 @@ impl CCDSolver {
                 let predicted_body_pos1 = rb1.pos.integrate_forces_and_velocities(
                     dt,
                     &rb1.forces,
-                    &rb1.vels,
+                    &rb1.integrated_vels,
                     &rb1.mprops,
                 );
 
@@ -256,7 +264,7 @@ impl CCDSolver {
                 let predicted_body_pos1 = rb1.pos.integrate_forces_and_velocities(
                     dt,
                     &rb1.forces,
-                    &rb1.vels,
+                    &rb1.integrated_vels,
                     &rb1.mprops,
                 );
 
@@ -491,7 +499,10 @@ impl CCDSolver {
                 let local_com1 = &rb1.mprops.local_mprops.local_com;
                 let frozen1 = frozen.get(&b1);
                 let pos1 = frozen1
-                    .map(|t| rb1.vels.integrate(*t, &rb1.pos.position, local_com1))
+                    .map(|t| {
+                        rb1.integrated_vels
+                            .integrate(*t, &rb1.pos.position, local_com1)
+                    })
                     .unwrap_or(rb1.pos.next_position);
                 pos1 * co_parent1.pos_wrt_parent
             } else {
@@ -504,7 +515,10 @@ impl CCDSolver {
                 let local_com2 = &rb2.mprops.local_mprops.local_com;
                 let frozen2 = frozen.get(&b2);
                 let pos2 = frozen2
-                    .map(|t| rb2.vels.integrate(*t, &rb2.pos.position, local_com2))
+                    .map(|t| {
+                        rb2.integrated_vels
+                            .integrate(*t, &rb2.pos.position, local_com2)
+                    })
                     .unwrap_or(rb2.pos.next_position);
                 pos2 * co_parent2.pos_wrt_parent
             } else {
diff --git a/src/dynamics/ccd/toi_entry.rs b/src/dynamics/ccd/toi_entry.rs
index 4591825..6f5a47d 100644
--- a/src/dynamics/ccd/toi_entry.rs
+++ b/src/dynamics/ccd/toi_entry.rs
@@ -56,14 +56,14 @@ impl TOIEntry {
             return None;
         }
 
-        let linvel1 =
-            frozen1.is_none() as u32 as Real * rb1.map(|b| b.vels.linvel).unwrap_or(na::zero());
-        let linvel2 =
-            frozen2.is_none() as u32 as Real * rb2.map(|b| b.vels.linvel).unwrap_or(na::zero());
-        let angvel1 =
-            frozen1.is_none() as u32 as Real * rb1.map(|b| b.vels.angvel).unwrap_or(na::zero());
-        let angvel2 =
-            frozen2.is_none() as u32 as Real * rb2.map(|b| b.vels.angvel).unwrap_or(na::zero());
+        let linvel1 = frozen1.is_none() as u32 as Real
+            * rb1.map(|b| b.integrated_vels.linvel).unwrap_or(na::zero());
+        let linvel2 = frozen2.is_none() as u32 as Real
+            * rb2.map(|b| b.integrated_vels.linvel).unwrap_or(na::zero());
+        let angvel1 = frozen1.is_none() as u32 as Real
+            * rb1.map(|b| b.integrated_vels.angvel).unwrap_or(na::zero());
+        let angvel2 = frozen2.is_none() as u32 as Real
+            * rb2.map(|b| b.integrated_vels.angvel).unwrap_or(na::zero());
 
         #[cfg(feature = "dim2")]
         let vel12 = (linvel2 - linvel1).norm()
@@ -114,6 +114,20 @@ impl TOIEntry {
         // because the colliders may be in a separating trajectory.
         let stop_at_penetration = is_pseudo_intersection_test;
 
+        // let pos12 = motion_c1
+        //     .position_at_time(start_time)
+        //     .inv_mul(&motion_c2.position_at_time(start_time));
+        // let vel12 = linvel2 - linvel1;
+        // let res_toi = query_dispatcher
+        //     .time_of_impact(
+        //         &pos12,
+        //         &vel12,
+        //         co1.shape.as_ref(),
+        //         co2.shape.as_ref(),
+        //         end_time - start_time,
+        //     )
+        //     .ok();
+
         let res_toi = query_dispatcher
             .nonlinear_time_of_impact(
                 &motion_c1,
@@ -144,8 +158,8 @@ impl TOIEntry {
             NonlinearRigidMotion::new(
                 rb.pos.position,
                 rb.mprops.local_mprops.local_com,
-                rb.vels.linvel,
-                rb.vels.angvel,
+                rb.integrated_vels.linvel,
+                rb.integrated_vels.angvel,
             )
         } else {
             NonlinearRigidMotion::constant_position(rb.pos.next_position)
diff --git a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs
index 0309cff..c4ec734 100644
--- a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs
+++ b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs
@@ -42,6 +42,7 @@ pub struct ImpulseJointSet {
     rb_graph_ids: Coarena<RigidBodyGraphIndex>,
     joint_ids: Arena<TemporaryInteractionIndex>, // Map joint handles to edge ids on the graph.
     joint_graph: InteractionGraph<RigidBodyHandle, ImpulseJoint>,
+    pub(crate) to_wake_up: Vec<RigidBodyHandle>, // A set of rigid-body handles to wake-up during the next timestep.
 }
 
 impl ImpulseJointSet {
@@ -51,6 +52,7 @@ impl ImpulseJointSet {
             rb_graph_ids: Coarena::new(),
             joint_ids: Arena::new(),
             joint_graph: InteractionGraph::new(),
+            to_wake_up: vec![],
         }
     }
 
@@ -180,11 +182,15 @@ impl ImpulseJointSet {
     }
 
     /// Inserts a new joint into this set and retrieve its handle.
+    ///
+    /// If `wake_up` is set to `true`, then the bodies attached to this joint will be
+    /// automatically woken up during the next timestep.
     pub fn insert(
         &mut self,
         body1: RigidBodyHandle,
         body2: RigidBodyHandle,
         data: impl Into<GenericJoint>,
+