feat: remove the "fast constraint" special case

6 files changed, 16 insertions, 70 deletions

diff --git a/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs b/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs
index 301ce71..e254995 100644
--- a/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs
+++ b/src/dynamics/solver/contact_constraint/generic_one_body_constraint.rs
@@ -231,13 +231,8 @@ impl GenericOneBodyConstraintBuilder {
             .unwrap()
             .local_to_world;
 
-        self.inner.update_with_positions(
-            params,
-            solved_dt,
-            pos2,
-            self.ccd_thickness,
-            &mut constraint.inner,
-        );
+        self.inner
+            .update_with_positions(params, solved_dt, pos2, &mut constraint.inner);
     }
 }
 
diff --git a/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs b/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs
index be839f0..dd63d1f 100644
--- a/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs
+++ b/src/dynamics/solver/contact_constraint/generic_two_body_constraint.rs
@@ -342,14 +342,8 @@ impl GenericTwoBodyConstraintBuilder {
             .map(|m| &multibodies[m.multibody].link(m.id).unwrap().local_to_world)
             .unwrap_or_else(|| &bodies[constraint.inner.solver_vel2].position);
 
-        self.inner.update_with_positions(
-            params,
-            solved_dt,
-            pos1,
-            pos2,
-            self.ccd_thickness,
-            &mut constraint.inner,
-        );
+        self.inner
+            .update_with_positions(params, solved_dt, pos1, pos2, &mut constraint.inner);
     }
 }
 
diff --git a/src/dynamics/solver/contact_constraint/one_body_constraint.rs b/src/dynamics/solver/contact_constraint/one_body_constraint.rs
index cf9fe67..1998659 100644
--- a/src/dynamics/solver/contact_constraint/one_body_constraint.rs
+++ b/src/dynamics/solver/contact_constraint/one_body_constraint.rs
@@ -260,13 +260,7 @@ impl OneBodyConstraintBuilder {
         constraint: &mut OneBodyConstraint,
     ) {
         let rb2 = &bodies[constraint.solver_vel2];
-        self.update_with_positions(
-            params,
-            solved_dt,
-            &rb2.position,
-            rb2.ccd_thickness,
-            constraint,
-        )
+        self.update_with_positions(params, solved_dt, &rb2.position, constraint)
     }
 
     // TODO: this code is SOOOO similar to TwoBodyConstraint::update.
@@ -276,7 +270,6 @@ impl OneBodyConstraintBuilder {
         params: &IntegrationParameters,
         solved_dt: Real,
         rb2_pos: &Isometry<Real>,
-        ccd_thickness: Real,
         constraint: &mut OneBodyConstraint,
     ) {
         let cfm_factor = params.cfm_factor();
@@ -290,7 +283,6 @@ impl OneBodyConstraintBuilder {
         let new_pos1 = self
             .vels1
             .integrate(solved_dt, &rb1.position, &rb1.local_com);
-        let mut is_fast_contact = false;
 
         #[cfg(feature = "dim2")]
         let tangents1 = constraint.dir1.orthonormal_basis();
@@ -309,11 +301,9 @@ impl OneBodyConstraintBuilder {
             // Normal part.
             {
                 let rhs_wo_bias = info.normal_rhs_wo_bias + dist.max(0.0) * inv_dt;
-                let rhs_bias = erp_inv_dt
-                    * (dist + params.allowed_linear_error())
-                        .clamp(-params.max_penetration_correction(), 0.0);
+                let rhs_bias = (erp_inv_dt * (dist + params.allowed_linear_error()))
+                    .clamp(-params.max_corrective_velocity(), 0.0);
                 let new_rhs = rhs_wo_bias + rhs_bias;
-                is_fast_contact = is_fast_contact || (-new_rhs * params.dt > ccd_thickness * 0.5);
 
                 element.normal_part.rhs_wo_bias = rhs_wo_bias;
                 element.normal_part.rhs = new_rhs;
@@ -334,7 +324,6 @@ impl OneBodyConstraintBuilder {
         }
 
         constraint.cfm_factor = cfm_factor;
-        // constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor };
     }
 }
 
diff --git a/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs b/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs
index 131702c..1e470b0 100644
--- a/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs
+++ b/src/dynamics/solver/contact_constraint/one_body_constraint_simd.rs
@@ -15,7 +15,6 @@ use crate::math::{
 use crate::utils::SimdBasis;
 use crate::utils::{self, SimdAngularInertia, SimdCross, SimdDot};
 use num::Zero;
-use parry::math::SimdBool;
 use parry::utils::SdpMatrix2;
 use simba::simd::{SimdPartialOrd, SimdValue};
 
@@ -263,15 +262,13 @@ impl SimdOneBodyConstraintBuilder {
         constraint: &mut OneBodyConstraintSimd,
     ) {
         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());
-        let max_penetration_correction = SimdReal::splat(params.max_penetration_correction());
+        let max_corrective_velocity = SimdReal::splat(params.max_corrective_velocity());
         let warmstart_coeff = SimdReal::splat(params.warmstart_coefficient);
 
         let rb2 = gather![|ii| &bodies[constraint.solver_vel2[ii]]];
-        let ccd_thickness = SimdReal::from(gather![|ii| rb2[ii].ccd_thickness]);
         let poss2 = Isometry::from(gather![|ii| rb2[ii].position]);
 
         let all_infos = &self.infos[..constraint.num_contacts as usize];
@@ -292,7 +289,6 @@ impl SimdOneBodyConstraintBuilder {
             constraint.dir1.cross(&constraint.tangent1),
         ];
 
-        let mut is_fast_contact = SimdBool::splat(false);
         let solved_dt = SimdReal::splat(solved_dt);
 
         for (info, element) in all_infos.iter().zip(all_elements.iter_mut()) {
@@ -305,12 +301,9 @@ impl SimdOneBodyConstraintBuilder {
             {
                 let rhs_wo_bias =
                     info.normal_rhs_wo_bias + dist.simd_max(SimdReal::zero()) * inv_dt;
-                let rhs_bias = (dist + allowed_lin_err)
-                    .simd_clamp(-max_penetration_correction, SimdReal::zero())
-                    * erp_inv_dt;
+                let rhs_bias = ((dist + allowed_lin_err) * erp_inv_dt)
+                    .simd_clamp(-max_corrective_velocity, SimdReal::zero());
                 let new_rhs = rhs_wo_bias + rhs_bias;
-                is_fast_contact =
-                    is_fast_contact | (-new_rhs * dt).simd_gt(ccd_thickness * SimdReal::splat(0.5));
 
                 element.normal_part.rhs_wo_bias = rhs_wo_bias;
                 element.normal_part.rhs = new_rhs;
@@ -331,7 +324,6 @@ impl SimdOneBodyConstraintBuilder {
         }
 
         constraint.cfm_factor = cfm_factor;
-        // constraint.cfm_factor = SimdReal::splat(1.0).select(is_fast_contact, cfm_factor);
     }
 }
 
diff --git a/src/dynamics/solver/contact_constraint/two_body_constraint.rs b/src/dynamics/solver/contact_constraint/two_body_constraint.rs
index e98305a..cb13664 100644
--- a/src/dynamics/solver/contact_constraint/two_body_constraint.rs
+++ b/src/dynamics/solver/contact_constraint/two_body_constraint.rs
@@ -361,15 +361,7 @@ impl TwoBodyConstraintBuilder {
     ) {
         let rb1 = &bodies[constraint.solver_vel1];
         let rb2 = &bodies[constraint.solver_vel2];
-        let ccd_thickness = rb1.ccd_thickness + rb2.ccd_thickness;
-        self.update_with_positions(
-            params,
-            solved_dt,
-            &rb1.position,
-            &rb2.position,
-            ccd_thickness,
-            constraint,
-        )
+        self.update_with_positions(params, solved_dt, &rb1.position, &rb2.position, constraint)
     }
 
     // Used by both generic and non-generic builders..
@@ -379,7 +371,6 @@ impl TwoBodyConstraintBuilder {
         solved_dt: Real,
         rb1_pos: &Isometry<Real>,
         rb2_pos: &Isometry<Real>,
-        ccd_thickness: Real,
         constraint: &mut TwoBodyConstraint,
     ) {
         let cfm_factor = params.cfm_factor();
@@ -389,8 +380,6 @@ impl TwoBodyConstraintBuilder {
         let all_infos = &self.infos[..constraint.num_contacts as usize];
         let all_elements = &mut constraint.elements[..constraint.num_contacts as usize];
 
-        let mut is_fast_contact = false;
-
         #[cfg(feature = "dim2")]
         let tangents1 = constraint.dir1.orthonormal_basis();
         #[cfg(feature = "dim3")]
@@ -408,11 +397,9 @@ impl TwoBodyConstraintBuilder {
             // Normal part.
             {
                 let rhs_wo_bias = info.normal_rhs_wo_bias + dist.max(0.0) * inv_dt;
-                let rhs_bias = erp_inv_dt
-                    * (dist + params.allowed_linear_error())
-                        .clamp(-params.max_penetration_correction(), 0.0);
+                let rhs_bias = (erp_inv_dt * (dist + params.allowed_linear_error()))
+                    .clamp(-params.max_corrective_velocity(), 0.0);
                 let new_rhs = rhs_wo_bias + rhs_bias;
-                is_fast_contact = is_fast_contact || (-new_rhs * params.dt > ccd_thickness * 0.5);
 
                 element.normal_part.rhs_wo_bias = rhs_wo_bias;
                 element.normal_part.rhs = new_rhs;
@@ -433,7 +420,6 @@ impl TwoBodyConstraintBuilder {
         }
 
         constraint.cfm_factor = cfm_factor;
-        // constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor };
     }
 }
 
diff --git a/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs b/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs
index 4af0d10..33e5558 100644
--- a/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs
+++ b/src/dynamics/solver/contact_constraint/two_body_constraint_simd.rs
@@ -15,7 +15,6 @@ use crate::math::{
 use crate::utils::SimdBasis;
 use crate::utils::{self, SimdAngularInertia, SimdCross, SimdDot};
 use num::Zero;
-use parry::math::SimdBool;
 use parry::utils::SdpMatrix2;
 use simba::simd::{SimdPartialOrd, SimdValue};
 
@@ -252,19 +251,15 @@ impl TwoBodyConstraintBuilderSimd {
         constraint: &mut TwoBodyConstraintSimd,
     ) {
         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());
-        let max_penetration_correction = SimdReal::splat(params.max_penetration_correction());
+        let max_corrective_velocity = SimdReal::splat(params.max_corrective_velocity());
         let warmstart_coeff = SimdReal::splat(params.warmstart_coefficient);
 
         let rb1 = gather![|ii| &bodies[constraint.solver_vel1[ii]]];
         let rb2 = gather![|ii| &bodies[constraint.solver_vel2[ii]]];
 
-        let ccd_thickness = SimdReal::from(gather![|ii| rb1[ii].ccd_thickness])
-            + SimdReal::from(gather![|ii| rb2[ii].ccd_thickness]);
-
         let poss1 = Isometry::from(gather![|ii| rb1[ii].position]);
         let poss2 = Isometry::from(gather![|ii| rb2[ii].position]);
 
@@ -279,7 +274,6 @@ impl TwoBodyConstraintBuilderSimd {
             constraint.dir1.cross(&constraint.tangent1),
         ];
 
-        let mut is_fast_contact = SimdBool::splat(false);
         let solved_dt = SimdReal::splat(solved_dt);
 
         for (info, element) in all_infos.iter().zip(all_elements.iter_mut()) {
@@ -292,12 +286,9 @@ impl TwoBodyConstraintBuilderSimd {
             {
                 let rhs_wo_bias =
                     info.normal_rhs_wo_bias + dist.simd_max(SimdReal::zero()) * inv_dt;
-                let rhs_bias = (dist + allowed_lin_err)
-                    .simd_clamp(-max_penetration_correction, SimdReal::zero())
-                    * erp_inv_dt;
+                let rhs_bias = ((dist + allowed_lin_err) * erp_inv_dt)
+                    .simd_clamp(-max_corrective_velocity, SimdReal::zero());
                 let new_rhs = rhs_wo_bias + rhs_bias;
-                is_fast_contact =
-                    is_fast_contact | (-new_rhs * dt).simd_gt(ccd_thickness * SimdReal::splat(0.5));
 
                 element.normal_part.rhs_wo_bias = rhs_wo_bias;
                 element.normal_part.rhs = new_rhs;
@@ -318,7 +309,6 @@ impl TwoBodyConstraintBuilderSimd {
         }
 
         constraint.cfm_factor = cfm_factor;
-        // constraint.cfm_factor = SimdReal::splat(1.0).select(is_fast_contact, cfm_factor);
     }
 }