CCD improvements

- Fix bug where the CCD thickness wasn’t initialized properly.
- Fix bug where the contact compliance would result in unwanted tunelling, despite CCD being enabled.

13 files changed, 122 insertions, 37 deletions

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/rigid_body.rs b/src/dynamics/rigid_body.rs
index 24e9754..5eca5a2 100644
--- a/src/dynamics/rigid_body.rs
+++ b/src/dynamics/rigid_body.rs
@@ -19,6 +19,10 @@ use num::Zero;
 pub struct RigidBody {
     pub(crate) pos: RigidBodyPosition,
     pub(crate) mprops: RigidBodyMassProps,
+    // NOTE: we need this so that the CCD can use the actual velocities obtained
+    //       by the velocity solver with bias. If we switch to intepolation, we
+    //       should remove this field.
+    pub(crate) integrated_vels: RigidBodyVelocity,
     pub(crate) vels: RigidBodyVelocity,
     pub(crate) damping: RigidBodyDamping,
     pub(crate) forces: RigidBodyForces,
@@ -47,6 +51,7 @@ impl RigidBody {
         Self {
             pos: RigidBodyPosition::default(),
             mprops: RigidBodyMassProps::default(),
+            integrated_vels: RigidBodyVelocity::default(),
             vels: RigidBodyVelocity::default(),
             damping: RigidBodyDamping::default(),
             forces: RigidBodyForces::default(),
diff --git a/src/dynamics/rigid_body_components.rs b/src/dynamics/rigid_body_components.rs
index b818ce2..3d35d17 100644
--- a/src/dynamics/rigid_body_components.rs
+++ b/src/dynamics/rigid_body_components.rs
@@ -752,7 +752,7 @@ pub struct RigidBodyCcd {
 impl Default for RigidBodyCcd {
     fn default() -> Self {
         Self {
-            ccd_thickness: 0.0,
+            ccd_thickness: Real::MAX,
             ccd_max_dist: 0.0,
             ccd_active: false,
             ccd_enabled: false,
diff --git a/src/dynamics/solver/generic_velocity_constraint.rs b/src/dynamics/solver/generic_velocity_constraint.rs
index 318727e..945590c 100644
--- a/src/dynamics/solver/generic_velocity_constraint.rs
+++ b/src/dynamics/solver/generic_velocity_constraint.rs
@@ -34,6 +34,7 @@ impl GenericVelocityConstraint {
         jacobian_id: &mut usize,
         insert_at: Option<usize>,
     ) {
+        let cfm_factor = params.cfm_factor();
         let inv_dt = params.inv_dt();
         let erp_inv_dt = params.erp_inv_dt();
 
@@ -45,6 +46,7 @@ impl GenericVelocityConstraint {
 
         let (vels1, mprops1, type1) = (&rb1.vels, &rb1.mprops, &rb1.body_type);
         let (vels2, mprops2, type2) = (&rb2.vels, &rb2.mprops, &rb2.body_type);
+        let ccd_thickness = rb1.ccd.ccd_thickness + rb2.ccd.ccd_thickness;
 
         let multibody1 = multibodies
             .rigid_body_link(handle1)
@@ -196,13 +198,18 @@ impl GenericVelocityConstraint {
                     let rhs_bias =
                         /* is_resting * */ erp_inv_dt * manifold_point.dist.clamp(-params.max_penetration_correction, 0.0);
 
+                    let rhs = rhs_wo_bias + rhs_bias;
+                    let is_fast_contact = -rhs * params.dt > ccd_thickness * 0.5;
+                    let cfm = if is_fast_contact { 1.0 } else { cfm_factor };
+
                     constraint.elements[k].normal_part = VelocityConstraintNormalPart {
                         gcross1,
                         gcross2,
-                        rhs: rhs_wo_bias + rhs_bias,
+                        rhs,
                         rhs_wo_bias,
                         impulse: na::zero(),
                         r,
+                        cfm,
                     };
                 }
 
diff --git a/src/dynamics/solver/generic_velocity_ground_constraint.rs b/src/dynamics/solver/generic_velocity_ground_constraint.rs
index 16648c4..24a9de5 100644
--- a/src/dynamics/solver/generic_velocity_ground_constraint.rs
+++ b/src/dynamics/solver/generic_velocity_ground_constraint.rs
@@ -32,6 +32,7 @@ impl GenericVelocityGroundConstraint {
         jacobian_id: &mut usize,
         insert_at: Option<usize>,
     ) {
+        let cfm_factor = params.cfm_factor();
         let inv_dt = params.inv_dt();
         let erp_inv_dt = params.erp_inv_dt();
 
@@ -130,19 +131,24 @@ impl GenericVelocityGroundConstraint {
                     let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
                     let is_resting = 1.0 - is_bouncy;
 
-                    let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution)
-                        * (vel1 - vel2).dot(&force_dir1);
+                    let dvel = (vel1 - vel2).dot(&force_dir1);
+                    let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution) * dvel;
                     rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt;
                     rhs_wo_bias *= is_bouncy + is_resting;
                     let rhs_bias =
                         /* is_resting * */ erp_inv_dt * manifold_point.dist.clamp(-params.max_penetration_correction, 0.0);
 
+                    let rhs = rhs_wo_bias + rhs_bias;
+                    let is_fast_contact = -rhs * params.dt > rb2.ccd.ccd_thickness * 0.5;
+                    let cfm = if is_fast_contact { 1.0 } else { cfm_factor };
+
                     constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart {
                         gcross2: na::zero(), // Unused for generic constraints.
-                        rhs: rhs_wo_bias + rhs_bias,
+                        rhs,
                         rhs_wo_bias,
                         impulse: na::zero(),
                         r,
+                        cfm,
                     };
                 }
 
diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs
index 23d4b97..bfb1e9d 100644
--- a/src/dynamics/solver/velocity_constraint.rs
+++ b/src/dynamics/solver/velocity_constraint.rs
@@ -153,6 +153,7 @@ impl VelocityConstraint {
     ) {
         assert_eq!(manifold.data.relative_dominance, 0);
 
+        let cfm_factor = params.cfm_factor();
         let inv_dt = params.inv_dt();
         let erp_inv_dt = params.erp_inv_dt();
 
@@ -163,6 +164,7 @@ impl VelocityConstraint {
         let (vels1, mprops1) = (&rb1.vels, &rb1.mprops);
         let rb2 = &bodies[handle2];
         let (vels2, mprops2) = (&rb2.vels, &rb2.mprops);
+        let ccd_thickness = rb1.ccd.ccd_thickness + rb2.ccd.ccd_thickness;
 
         let mj_lambda1 = rb1.ids.active_set_offset;
         let mj_lambda2 = rb2.ids.active_set_offset;
@@ -280,13 +282,19 @@ impl VelocityConstraint {
                     let rhs_bias = /* is_resting
                         * */  erp_inv_dt
                         * (manifold_point.dist + params.allowed_linear_error).clamp(-params.max_penetration_correction, 0.0);
+
+                    let rhs = rhs_wo_bias + rhs_bias;
+                    let is_fast_contact = -rhs * params.dt > ccd_thickness * 0.5;
+                    let cfm = if is_fast_contact { 1.0 } else { cfm_factor };
+
                     constraint.elements[k].normal_part = VelocityConstraintNormalPart {
                         gcross1,
                         gcross2,
-                        rhs: rhs_wo_bias + rhs_bias,
+                        rhs,
                         rhs_wo_bias,
                         impulse: na::zero(),
                         r: projected_mass,
+                        cfm,
                     };
                 }
 
diff --git a/src/dynamics/solver/velocity_constraint_element.rs b/src/dynamics/solver/velocity_constraint_element.rs
index 30afef5..6c1006d 100644
--- a/src/dynamics/solver/velocity_constraint_element.rs
+++ b/src/dynamics/solver/velocity_constraint_element.rs
@@ -112,6 +112,7 @@ pub(crate) struct VelocityConstraintNormalPart<N: WReal> {
     pub rhs_wo_bias: N,
     pub impulse: N,
     pub r: N,
+    pub cfm: N,
 }
 
 impl<N: WReal> VelocityConstraintNormalPart<N> {
@@ -123,6 +124,7 @@ impl<N: WReal> VelocityConstraintNormalPart<N> {
             rhs_wo_bias: na::zero(),
             impulse: na::zero(),
             r: na::zero(),
+            cfm: na::one(),
         }
     }
 
@@ -142,7 +144,7 @@ impl<N: WReal> VelocityConstraintNormalPart<N> {
             - dir1.dot(&mj_lambda2.linear)
             + self.gcross2.gdot(mj_lambda2.angular)
             + self.rhs;
-        let new_impulse = cfm_factor * (self.impulse - self.r * dvel).simd_max(N::zero());
+        let new_impulse = self.cfm * (self.impulse - self.r * dvel).simd_max(N::zero());
         let dlambda = new_impulse - self.impulse;
         self.impulse = new_impulse;
 
diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs
index 68246da..f235ddc 100644
--- a/src/dynamics/solver/velocity_constraint_wide.rs
+++ b/src/dynamics/solver/velocity_constraint_wide.rs
@@ -43,6 +43,8 @@ impl WVelocityConstraint {
             assert_eq!(manifolds[ii].data.relative_dominance, 0);
         }
 
+        let cfm_factor = SimdReal::splat(params.cfm_factor());
+        let dt = SimdReal::splat(params.dt);
         let inv_dt = SimdReal::splat(params.inv_dt());
         let allowed_lin_err = SimdReal::splat(params.allowed_linear_error);
         let erp_inv_dt = SimdReal::splat(params.erp_inv_dt());
@@ -58,6 +60,10 @@ impl WVelocityConstraint {
         let mprops1: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| &bodies[handles1[ii]].mprops];
         let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii]].mprops];
 
+        let ccd_thickness1 = SimdReal::from(gather![|ii| bodies[handles1[ii]].ccd.ccd_thickness]);
+        let ccd_thickness2 = SimdReal::from(gather![|ii| bodies[handles2[ii]].ccd.ccd_thickness]);
+        let ccd_thickness = ccd_thickness1 + ccd_thickness2;
+
         let world_com1 = Point::from(gather![|ii| mprops1[ii].world_com]);
         let im1 = Vector::from(gather![|ii| mprops1[ii].effective_inv_mass]);
         let ii1: AngularInertia<SimdReal> =
@@ -147,6 +153,10 @@ impl WVelocityConstraint {
                         .simd_clamp(-max_penetration_correction, SimdReal::zero())
                         * (erp_inv_dt/* * is_resting */);
 
+                    let rhs = rhs_wo_bias + rhs_bias;
+                    let is_fast_contact = (-rhs * dt).simd_gt(ccd_thickness * SimdReal::splat(0.5));
+                    let cfm = SimdReal::splat(1.0).select(is_fast_contact, cfm_factor);
+
                     constraint.elements[k].normal_part = VelocityConstraintNormalPart {
                         gcross1,
                         gcross2,
@@ -154,6 +164,7 @@ impl WVelocityConstraint {
                         rhs_wo_bias,
                         impulse: SimdReal::splat(0.0),
                         r: projected_mass,
+                        cfm,
                     };
                 }
 
diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs
index 39a2a19..99b71e0 100644
--- a/src/dynamics/solver/velocity_ground_constraint.rs
+++ b/src/dynamics/solver/velocity_ground_constraint.rs
@@ -34,6 +34,7 @@ impl VelocityGroundConstraint {
         out_constraints: &mut Vec<AnyVelocityConstraint>,
         insert_at: Option<usize>,
     ) {
+        let cfm_factor = params.cfm_factor();
         let inv_dt = params.inv_dt();
         let erp_inv_dt = params.erp_inv_dt();
 
@@ -156,20 +157,25 @@ impl VelocityGroundConstraint {
                     let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
                     let is_resting = 1.0 - is_bouncy;
 
-                    let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution)
-                        * (vel1 - vel2).dot(&force_dir1);
+                    let dvel = (vel1 - vel2).dot(&force_dir1);
+                    let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution) * dvel;
                     rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt;
                     rhs_wo_bias *= is_bouncy + is_resting;
                     let rhs_bias = /* is_resting
                         * */ erp_inv_dt
                         * (manifold_point.dist + params.allowed_linear_error).clamp(-params.max_penetration_correction, 0.0);
 
+                    let rhs = rhs_wo_bias + rhs_bias;
+                    let is_fast_contact = -rhs * params.dt > rb2.ccd.ccd_thickness * 0.5;
+                    let cfm = if is_fast_contact { 1.0 } else { cfm_factor };
+
                     constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart {
                         gcross2,
-                        rhs: rhs_wo_bias + rhs_bias,
+                        rhs,
                         rhs_wo_bias,
                         impulse: na::zero(),
                         r: projected_mass,
+                        cfm,
                     };
                 }
 
diff --git a/src/dynamics/solver/velocity_ground_constraint_element.rs b/src/dynamics/solver/velocity_ground_constraint_element.rs
index 06a727a..3b2ed86 100644
--- a/src/dynamics/solver/velocity_ground_constraint_element.rs
+++ b/src/dynamics/solver/velocity_ground_constraint_element.rs
@@ -93,6 +93,7 @@ pub(crate) struct VelocityGroundConstraintNormalPart<N: WReal> {
     pub rhs_wo_bias: N,
     pub impulse: N,
     pub r: N,
+    pub cfm: N,
 }
 
 impl<N: WReal> VelocityGroundConstraintNormalPart<N> {
@@ -103,6 +104,7 @@ impl<N: WReal> VelocityGroundConstraintNormalPart<N> {
             rhs_wo_bias: na::zero(),
             impulse: na::zero(),
             r: na::zero(),
+            cfm: na::one(),
         }
     }
 
@@ -117,7 +119,7 @@ impl<N: WReal> VelocityGroundConstraintNormalPart<N> {
         AngVector<N>: WDot<AngVector<N>, Result = N>,
     {
         let dvel = -dir1.dot(&mj_lambda2.linear) + self.gcross2.gdot(mj_lambda2.angular) + self.rhs;
-        let new_impulse = cfm_factor * (self.impulse - self.r * dvel).simd_max(N::zero());
+        let new_impulse = self.cfm * (self.impulse - self.r * dvel).simd_max(N::zero());
         let dlambda = new_impulse - self.impulse;
         self.impulse = new_impulse;
 
diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs
index 37f4688..e0e7649 100644
--- a/src/dynamics/solver/velocity_ground_constraint_wide.rs
+++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs
@@ -9,6 +9,7 @@ use crate::geometry::{ContactManifold, ContactManifoldIndex};
 use crate::math::{
     AngVector, AngularInertia, Point, Real, SimdReal, Vector, DIM, MAX_MANIFOLD_POINTS, SIMD_WIDTH,
 };
+use crate::prelude::RigidBody;
 #[cfg(feature = "dim2")]
 use crate::utils::WBasis;
 use crate::utils::{self, WAngularInertia, WCross, WDot};
@@ -38,6 +39,8 @@ impl WVelocityGroundConstraint {
         out_constraints: &mut Vec<AnyVelocityConstraint>,
         insert_at: Option<usize>,
     ) {
+        let cfm_factor = SimdReal::splat(params.cfm_factor());
+        let dt = SimdReal::splat(params.dt);
         let inv_dt = SimdReal::splat(params.inv_dt());
         let allowed_lin_err = SimdReal::splat(params.allowed_linear_error);
         let erp_inv_dt = SimdReal::splat(params.erp_inv_dt());
@@ -65,11 +68,12 @@ impl WVelocityGroundConstraint {
                 .unwrap_or_else(Point::origin)
         }]);
 
-        let vels2: [&RigidBodyVelocity; SIMD_WIDTH] =
-            gather![|ii| &bodies[handles2[ii].unwrap()].vels];
-        let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| &bodies[handles2[ii].unwrap()].ids];
-        let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] =
-            gather![|ii| &bodies[handles2[ii].unwrap()].mprops];
+        let bodies2 = gather![|ii| &bodies[handles2[ii].unwrap()]];
+
+        let vels2: [&RigidBodyVelocity; SIMD_WIDTH] = gather![|ii| &bodies2[ii].vels];
+        let ids2: [&RigidBodyIds; SIMD_WIDTH] = gather![|ii| &bodies2[ii].ids];
+        let mprops2: [&RigidBodyMassProps; SIMD_WIDTH] = gather![|ii| &bodies2[ii].mprops];
+        let ccd_thickness = SimdReal::from(gather![|ii| bodies2[ii].ccd.ccd_thickness]);
 
         let flipped_sign = SimdReal::from(flipped);
 
@@ -152,12 +156,17 @@ impl WVelocityGroundConstraint {
                         .simd_clamp(-max_penetration_correction, SimdReal::zero())
                         * (erp_inv_dt/* * is_resting */);
 
+                    let rhs = rhs_wo_bias + rhs_bias;
+                    let is_fast_contact = (-rhs * dt).simd_gt(ccd_thickness * SimdReal::splat(0.5));
+                    let cfm = SimdReal::splat(1.0).select(is_fast_contact, cfm_factor);
+
                     constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart {
                         gcross2,
-                        rhs: rhs_wo_bias + rhs_bias,
+                        rhs,
                         rhs_wo_bias,
                         impulse: na::zero(),
                         r: projected_mass,
+                        cfm,
                     };
                 }
 
diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs
index d5dc77d..ddd7086 100644
--- a/src/dynamics/solver/velocity_solver.rs
+++ b/src/dynamics/solver/velocity_solver.rs
@@ -168,6 +168,7 @@ impl VelocitySolver {
                     &rb.pos.position,
                     &rb.mprops.local_mprops.local_com,
                 );
+                rb.integrated_vels = new_vels;
                 rb.pos = new_pos;
             }
         }