Move the cfm factor to the velocity constraints instead of the elements.

1 files changed, 11 insertions, 7 deletions

diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs
index e0e7649..25bb30d 100644
--- a/src/dynamics/solver/velocity_ground_constraint_wide.rs
+++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs
@@ -9,11 +9,11 @@ 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};
 use num::Zero;
+use parry::math::SimdBool;
 use simba::simd::{SimdPartialOrd, SimdValue};
 
 #[derive(Copy, Clone, Debug)]
@@ -24,6 +24,7 @@ pub(crate) struct WVelocityGroundConstraint {
     pub elements: [VelocityGroundConstraintElement<SimdReal>; MAX_MANIFOLD_POINTS],
     pub num_contacts: u8,
     pub im2: Vector<SimdReal>,
+    pub cfm_factor: SimdReal,
     pub limit: SimdReal,
     pub mj_lambda2: [usize; SIMD_WIDTH],
     pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH],
@@ -105,12 +106,14 @@ impl WVelocityGroundConstraint {
             let manifold_points = gather![|ii| &manifolds[ii].data.solver_contacts[l..]];
             let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS);
 
+            let mut is_fast_contact = SimdBool::splat(false);
             let mut constraint = WVelocityGroundConstraint {
                 dir1: force_dir1,
                 #[cfg(feature = "dim3")]
                 tangent1: tangents1[0],
                 elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS],
                 im2,
+                cfm_factor,
                 limit: SimdReal::splat(0.0),
                 mj_lambda2,
                 manifold_id,
@@ -157,8 +160,8 @@ impl WVelocityGroundConstraint {
                         * (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);
+                    is_fast_contact =
+                        is_fast_contact | (-rhs * dt).simd_gt(ccd_thickness * SimdReal::splat(0.5));
 
                     constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart {
                         gcross2,
@@ -166,7 +169,6 @@ impl WVelocityGroundConstraint {
                         rhs_wo_bias,
                         impulse: na::zero(),
                         r: projected_mass,
-                        cfm,
                     };
                 }
 
@@ -196,6 +198,8 @@ impl WVelocityGroundConstraint {
                 }
             }
 
+            constraint.cfm_factor = SimdReal::splat(1.0).select(is_fast_contact, cfm_factor);
+
             if let Some(at) = insert_at {
                 out_constraints[at + l / MAX_MANIFOLD_POINTS] =
                     AnyVelocityConstraint::GroupedGround(constraint);
@@ -207,7 +211,6 @@ impl WVelocityGroundConstraint {
 
     pub fn solve(
         &mut self,
-        cfm_factor: Real,
         mj_lambdas: &mut [DeltaVel<Real>],
         solve_normal: bool,
         solve_friction: bool,
@@ -220,7 +223,7 @@ impl WVelocityGroundConstraint {
         };
 
         VelocityGroundConstraintElement::solve_group(
-            SimdReal::splat(cfm_factor),
+            self.cfm_factor,
             &mut self.elements[..self.num_contacts as usize],
             &self.dir1,
             #[cfg(feature = "dim3")]
@@ -265,7 +268,8 @@ impl WVelocityGroundConstraint {
         }
     }
 
-    pub fn remove_bias_from_rhs(&mut self) {
+    pub fn remove_cfm_and_bias_from_rhs(&mut self) {
+        self.cfm_factor = SimdReal::splat(1.0);
         for elt in &mut self.elements {
             elt.normal_part.rhs = elt.normal_part.rhs_wo_bias;
         }