diff options
Diffstat (limited to 'src/dynamics')
10 files changed, 76 insertions, 68 deletions
diff --git a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs index fd2fa67..f1b2ffe 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs @@ -1,11 +1,8 @@ use crate::data::{Arena, Coarena, Index}; use crate::dynamics::joint::MultibodyLink; -use crate::dynamics::{ - GenericJoint, IslandManager, Multibody, MultibodyJoint, RigidBodyHandle, RigidBodySet, -}; +use crate::dynamics::{GenericJoint, Multibody, MultibodyJoint, RigidBodyHandle}; use crate::geometry::{InteractionGraph, RigidBodyGraphIndex}; use crate::parry::partitioning::IndexedData; -use crate::prelude::RigidBody; /// The unique handle of an multibody_joint added to a `MultibodyJointSet`. #[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)] diff --git a/src/dynamics/solver/generic_velocity_constraint.rs b/src/dynamics/solver/generic_velocity_constraint.rs index 945590c..ed8c569 100644 --- a/src/dynamics/solver/generic_velocity_constraint.rs +++ b/src/dynamics/solver/generic_velocity_constraint.rs @@ -94,6 +94,7 @@ impl GenericVelocityConstraint { .enumerate() { let chunk_j_id = *jacobian_id; + let mut is_fast_contact = false; let mut constraint = VelocityConstraint { dir1: force_dir1, #[cfg(feature = "dim3")] @@ -109,6 +110,7 @@ impl GenericVelocityConstraint { } else { na::zero() }, + cfm_factor, limit: 0.0, mj_lambda1, mj_lambda2, @@ -199,8 +201,7 @@ impl GenericVelocityConstraint { /* 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 }; + is_fast_contact = is_fast_contact || (-rhs * params.dt > ccd_thickness * 0.5); constraint.elements[k].normal_part = VelocityConstraintNormalPart { gcross1, @@ -209,7 +210,6 @@ impl GenericVelocityConstraint { rhs_wo_bias, impulse: na::zero(), r, - cfm, }; } @@ -290,6 +290,8 @@ impl GenericVelocityConstraint { } } + constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor }; + let ndofs1 = multibody1.map(|mb| mb.0.ndofs()).unwrap_or(0); let ndofs2 = multibody2.map(|mb| mb.0.ndofs()).unwrap_or(0); // NOTE: we use the generic constraint for non-dynamic bodies because this will @@ -317,7 +319,6 @@ impl GenericVelocityConstraint { pub fn solve( &mut self, - cfm_factor: Real, jacobians: &DVector<Real>, mj_lambdas: &mut [DeltaVel<Real>], generic_mj_lambdas: &mut DVector<Real>, @@ -339,7 +340,7 @@ impl GenericVelocityConstraint { let elements = &mut self.velocity_constraint.elements [..self.velocity_constraint.num_contacts as usize]; VelocityConstraintElement::generic_solve_group( - cfm_factor, + self.velocity_constraint.cfm_factor, elements, jacobians, &self.velocity_constraint.dir1, @@ -371,7 +372,7 @@ impl GenericVelocityConstraint { self.velocity_constraint.writeback_impulses(manifolds_all); } - pub fn remove_bias_from_rhs(&mut self) { - self.velocity_constraint.remove_bias_from_rhs(); + pub fn remove_cfm_and_bias_from_rhs(&mut self) { + self.velocity_constraint.remove_cfm_and_bias_from_rhs(); } } diff --git a/src/dynamics/solver/generic_velocity_ground_constraint.rs b/src/dynamics/solver/generic_velocity_ground_constraint.rs index 24a9de5..91bb9fb 100644 --- a/src/dynamics/solver/generic_velocity_ground_constraint.rs +++ b/src/dynamics/solver/generic_velocity_ground_constraint.rs @@ -86,12 +86,14 @@ impl GenericVelocityGroundConstraint { .enumerate() { let chunk_j_id = *jacobian_id; + let mut is_fast_contact = false; let mut constraint = VelocityGroundConstraint { dir1: force_dir1, #[cfg(feature = "dim3")] tangent1: tangents1[0], elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS], im2: mprops2.effective_inv_mass, + cfm_factor, limit: 0.0, mj_lambda2, manifold_id, @@ -139,8 +141,8 @@ impl GenericVelocityGroundConstraint { /* 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 }; + is_fast_contact = + is_fast_contact || (-rhs * params.dt > rb2.ccd.ccd_thickness * 0.5); constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart { gcross2: na::zero(), // Unused for generic constraints. @@ -148,7 +150,6 @@ impl GenericVelocityGroundConstraint { rhs_wo_bias, impulse: na::zero(), r, - cfm, }; } @@ -187,6 +188,8 @@ impl GenericVelocityGroundConstraint { } } + constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor }; + let constraint = GenericVelocityGroundConstraint { velocity_constraint: constraint, j_id: chunk_j_id, @@ -204,7 +207,6 @@ impl GenericVelocityGroundConstraint { pub fn solve( &mut self, - cfm_factor: Real, jacobians: &DVector<Real>, generic_mj_lambdas: &mut DVector<Real>, solve_restitution: bool, @@ -215,7 +217,7 @@ impl GenericVelocityGroundConstraint { let elements = &mut self.velocity_constraint.elements [..self.velocity_constraint.num_contacts as usize]; VelocityGroundConstraintElement::generic_solve_group( - cfm_factor, + self.velocity_constraint.cfm_factor, elements, jacobians, self.velocity_constraint.limit, @@ -232,7 +234,7 @@ impl GenericVelocityGroundConstraint { self.velocity_constraint.writeback_impulses(manifolds_all); } - pub fn remove_bias_from_rhs(&mut self) { - self.velocity_constraint.remove_bias_from_rhs(); + pub fn remove_cfm_and_bias_from_rhs(&mut self) { + self.velocity_constraint.remove_cfm_and_bias_from_rhs(); } } diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index bfb1e9d..7d2294e 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -47,21 +47,20 @@ impl AnyVelocityConstraint { pub fn remove_bias_from_rhs(&mut self) { match self { - AnyVelocityConstraint::Nongrouped(c) => c.remove_bias_from_rhs(), - AnyVelocityConstraint::NongroupedGround(c) => c.remove_bias_from_rhs(), + AnyVelocityConstraint::Nongrouped(c) => c.remove_cfm_and_bias_from_rhs(), + AnyVelocityConstraint::NongroupedGround(c) => c.remove_cfm_and_bias_from_rhs(), #[cfg(feature = "simd-is-enabled")] - AnyVelocityConstraint::Grouped(c) => c.remove_bias_from_rhs(), + AnyVelocityConstraint::Grouped(c) => c.remove_cfm_and_bias_from_rhs(), #[cfg(feature = "simd-is-enabled")] - AnyVelocityConstraint::GroupedGround(c) => c.remove_bias_from_rhs(), - AnyVelocityConstraint::NongroupedGeneric(c) => c.remove_bias_from_rhs(), - AnyVelocityConstraint::NongroupedGenericGround(c) => c.remove_bias_from_rhs(), + AnyVelocityConstraint::GroupedGround(c) => c.remove_cfm_and_bias_from_rhs(), + AnyVelocityConstraint::NongroupedGeneric(c) => c.remove_cfm_and_bias_from_rhs(), + AnyVelocityConstraint::NongroupedGenericGround(c) => c.remove_cfm_and_bias_from_rhs(), AnyVelocityConstraint::Empty => unreachable!(), } } pub fn solve( &mut self, - cfm_factor: Real, jacobians: &DVector<Real>, mj_lambdas: &mut [DeltaVel<Real>], generic_mj_lambdas: &mut DVector<Real>, @@ -70,21 +69,20 @@ impl AnyVelocityConstraint { ) { match self { AnyVelocityConstraint::NongroupedGround(c) => { - c.solve(cfm_factor, mj_lambdas, solve_restitution, solve_friction) + c.solve(mj_lambdas, solve_restitution, solve_friction) } AnyVelocityConstraint::Nongrouped(c) => { - c.solve(cfm_factor, mj_lambdas, solve_restitution, solve_friction) + c.solve(mj_lambdas, solve_restitution, solve_friction) } #[cfg(feature = "simd-is-enabled")] AnyVelocityConstraint::GroupedGround(c) => { - c.solve(cfm_factor, mj_lambdas, solve_restitution, solve_friction) + c.solve(mj_lambdas, solve_restitution, solve_friction) } #[cfg(feature = "simd-is-enabled")] AnyVelocityConstraint::Grouped(c) => { - c.solve(cfm_factor, mj_lambdas, solve_restitution, solve_friction) + c.solve(mj_lambdas, solve_restitution, solve_friction) } AnyVelocityConstraint::NongroupedGeneric(c) => c.solve( - cfm_factor, jacobians, mj_lambdas, generic_mj_lambdas, @@ -92,7 +90,6 @@ impl AnyVelocityConstraint { solve_friction, ), AnyVelocityConstraint::NongroupedGenericGround(c) => c.solve( - cfm_factor, jacobians, generic_mj_lambdas, solve_restitution, @@ -124,6 +121,7 @@ pub(crate) struct VelocityConstraint { pub tangent1: Vector<Real>, // One of the friction force directions. pub im1: Vector<Real>, pub im2: Vector<Real>, + pub cfm_factor: Real, pub limit: Real, pub mj_lambda1: usize, pub mj_lambda2: usize, @@ -182,6 +180,8 @@ impl VelocityConstraint { .chunks(MAX_MANIFOLD_POINTS) .enumerate() { + let mut is_fast_contact = false; + #[cfg(not(target_arch = "wasm32"))] let mut constraint = VelocityConstraint { dir1: force_dir1, @@ -190,6 +190,7 @@ impl VelocityConstraint { elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS], im1: mprops1.effective_inv_mass, im2: mprops2.effective_inv_mass, + cfm_factor, limit: 0.0, mj_lambda1, mj_lambda2, @@ -237,6 +238,7 @@ impl VelocityConstraint { } constraint.im1 = mprops1.effective_inv_mass; constraint.im2 = mprops2.effective_inv_mass; + constraint.cfm_factor = cfm_factor; constraint.limit = 0.0; constraint.mj_lambda1 = mj_lambda1; constraint.mj_lambda2 = mj_lambda2; @@ -284,8 +286,7 @@ impl VelocityConstraint { * (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 }; + is_fast_contact = is_fast_contact || (-rhs * params.dt > ccd_thickness * 0.5); constraint.elements[k].normal_part = VelocityConstraintNormalPart { gcross1, @@ -294,7 +295,6 @@ impl VelocityConstraint { rhs_wo_bias, impulse: na::zero(), r: projected_mass, - cfm, }; } @@ -337,6 +337,8 @@ impl VelocityConstraint { } } + constraint.cfm_factor = if is_fast_contact { 1.0 } else { cfm_factor }; + #[cfg(not(target_arch = "wasm32"))] if let Some(at) = insert_at { out_constraints[at + _l] = AnyVelocityConstraint::Nongrouped(constraint); @@ -348,7 +350,6 @@ impl VelocityConstraint { pub fn solve( &mut self, - cfm_factor: Real, mj_lambdas: &mut [DeltaVel<Real>], solve_normal: bool, solve_friction: bool, @@ -357,7 +358,7 @@ impl VelocityConstraint { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; VelocityConstraintElement::solve_group( - cfm_factor, + self.cfm_factor, &mut self.elements[..self.num_contacts as usize], &self.dir1, #[cfg(feature = "dim3")] @@ -394,7 +395,8 @@ impl VelocityConstraint { } } - pub fn remove_bias_from_rhs(&mut self) { + pub fn remove_cfm_and_bias_from_rhs(&mut self) { + self.cfm_factor = 1.0; for elt in &mut self.elements { elt.normal_part.rhs = elt.normal_part.rhs_wo_bias; } diff --git a/src/dynamics/solver/velocity_constraint_element.rs b/src/dynamics/solver/velocity_constraint_element.rs index 6c1006d..30afef5 100644 --- a/src/dynamics/solver/velocity_constraint_element.rs +++ b/src/dynamics/solver/velocity_constraint_element.rs @@ -112,7 +112,6 @@ 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> { @@ -124,7 +123,6 @@ impl<N: WReal> VelocityConstraintNormalPart<N> { rhs_wo_bias: na::zero(), impulse: na::zero(), r: na::zero(), - cfm: na::one(), } } @@ -144,7 +142,7 @@ impl<N: WReal> VelocityConstraintNormalPart<N> { - dir1.dot(&mj_lambda2.linear) + self.gcross2.gdot(mj_lambda2.angular) + self.rhs; - let new_impulse = self.cfm * (self.impulse - self.r * dvel).simd_max(N::zero()); + let new_impulse = cfm_factor * (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 f235ddc..b8e0a7e 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -12,6 +12,7 @@ use crate::math::{ 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)] @@ -23,6 +24,7 @@ pub(crate) struct WVelocityConstraint { pub num_contacts: u8, pub im1: Vector<SimdReal>, pub im2: Vector<SimdReal>, + pub cfm_factor: SimdReal, pub limit: SimdReal, pub mj_lambda1: [usize; SIMD_WIDTH], pub mj_lambda2: [usize; SIMD_WIDTH], @@ -97,6 +99,7 @@ impl WVelocityConstraint { gather![|ii| &manifolds[ii].data.solver_contacts[l..num_active_contacts]]; let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS); + let mut is_fast_contact = SimdBool::splat(false); let mut constraint = WVelocityConstraint { dir1: force_dir1, #[cfg(feature = "dim3")] @@ -104,6 +107,7 @@ impl WVelocityConstraint { elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS], im1, im2, + cfm_factor, limit: SimdReal::splat(0.0), mj_lambda1, mj_lambda2, @@ -154,8 +158,8 @@ impl WVelocityConstraint { * (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 = VelocityConstraintNormalPart { gcross1, @@ -164,7 +168,6 @@ impl WVelocityConstraint { rhs_wo_bias, impulse: SimdReal::splat(0.0), r: projected_mass, - cfm, }; } @@ -200,6 +203,8 @@ impl WVelocityConstraint { } } + 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::Grouped(constraint); @@ -211,7 +216,6 @@ impl WVelocityConstraint { pub fn solve( &mut self, - cfm_factor: Real, mj_lambdas: &mut [DeltaVel<Real>], solve_normal: bool, solve_friction: bool, @@ -231,7 +235,7 @@ impl WVelocityConstraint { }; VelocityConstraintElement::solve_group( - SimdReal::splat(cfm_factor), + self.cfm_factor, &mut self.elements[..self.num_contacts as usize], &self.dir1, #[cfg(feature = "dim3")] @@ -281,7 +285,8 @@ impl WVelocityConstraint { } } - 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; } diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index 99b71e0..e574112 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -17,6 +17,7 @@ pub(crate) struct VelocityGroundConstraint { #[cfg(feature = "dim3")] pub tangent1: Vector<Real>, // One of the friction force directions. pub im2: Vector<Real>, + pub cfm_factor: Real, pub limit: Real, pub elements: [VelocityGroundConstraintElement<Real>; MAX_MANIFOLD_POINTS], @@ -81,6 +82,7 @@ impl VelocityGroundConstraint { tangent1: tangents1[0], elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS], im2: mprops2.effective_inv_mass, + cfm_factor, limit: 0.0, mj_lambda2, manifold_id, @@ -133,6 +135,8 @@ impl VelocityGroundConstraint { constraint.num_contacts = manifold_points.len() as u8; } + let mut is_fast_contact = false; + for k in 0..manifold_points.len() { let manifold_point = &manifold_points[k]; let dp2 = manifold_point.point - mprops2.world_com; @@ -166,8 +170,8 @@ impl VelocityGroundConstraint { * (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 }; + is_fast_contact = + is_fast_contact || -rhs * params.dt > rb2.ccd.ccd_thickness * 0.5; constraint.elements[k].normal_part = VelocityGroundConstraintNormalPart { gcross2, @@ -175,7 +179,6 @@ impl VelocityGroundConstraint { rhs_wo_bias, impulse: na::zero(), r: projected_mass, - cfm, }; } @@ -212,6 +215,10 @@ impl VelocityGroundConstraint { } } + if is_fast_contact { + constraint.cfm_factor = 1.0; + } + #[cfg(not(target_arch = "wasm32"))] if let Some(at) = insert_at { out_constraints[at + _l] = AnyVelocityConstraint::NongroupedGround(constraint); @@ -223,7 +230,6 @@ impl VelocityGroundConstraint { pub fn solve( &mut self, - cfm_factor: Real, mj_lambdas: &mut [DeltaVel<Real>], solve_normal: bool, solve_friction: bool, @@ -231,7 +237,7 @@ impl VelocityGroundConstraint { let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; VelocityGroundConstraintElement::solve_group( - cfm_factor, + self.cfm_factor, &mut self.elements[..self.num_contacts as usize], &self.dir1, #[cfg(feature = "dim3")] @@ -266,7 +272,8 @@ impl VelocityGroundConstraint { } } - pub fn remove_bias_from_rhs(&mut self) { + pub fn remove_cfm_and_bias_from_rhs(&mut self) { + self.cfm_factor = 1.0; for elt in &mut self.elements { elt.normal_part.rhs = elt.normal_part.rhs_wo_bias; } diff --git a/src/dynamics/solver/velocity_ground_constraint_element.rs b/src/dynamics/solver/velocity_ground_constraint_element.rs index 3b2ed86..06a727a 100644 --- a/src/dynamics/solver/velocity_ground_constraint_element.rs +++ b/src/dynamics/solver/velocity_ground_constraint_element.rs @@ -93,7 +93,6 @@ 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> { @@ -104,7 +103,6 @@ impl<N: WReal> VelocityGroundConstraintNormalPart<N> { rhs_wo_bias: na::zero(), impulse: na::zero(), r: na::zero(), - cfm: na::one(), } } @@ -119,7 +117,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 = self.cfm * (self.impulse - self.r * dvel).simd_max(N::zero()); + let new_impulse = cfm_factor * (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 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; } diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs index ddd7086..d15ea68 100644 --- a/src/dynamics/solver/velocity_solver.rs +++ b/src/dynamics/solver/velocity_solver.rs @@ -35,7 +35,6 @@ impl VelocitySolver { joint_constraints: &mut [AnyJointVelocityConstraint], generic_joint_jacobians: &DVector<Real>, ) { - let cfm_factor = params.cfm_factor(); self.mj_lambdas.clear(); self.mj_lambdas .resize(islands.active_island(island_id).len(), DeltaVel::zero()); @@ -86,7 +85,6 @@ impl VelocitySolver { for constraint in &mut *contact_constraints { constraint.solve( - cfm_factor, generic_contact_jacobians, &mut self.mj_lambdas[..], &mut self.generic_mj_lambdas, @@ -98,7 +96,6 @@ impl VelocitySolver { if solve_friction { for constraint in &mut *contact_constraints { constraint.solve( - cfm_factor, generic_contact_jacobians, &mut self.mj_lambdas[..], &mut self.generic_mj_lambdas, @@ -121,7 +118,6 @@ impl VelocitySolver { for _ in 0..remaining_friction_iterations { for constraint in &mut *contact_constraints { constraint.solve( - cfm_factor, generic_contact_jacobians, &mut self.mj_lambdas[..], &mut self.generic_mj_lambdas, @@ -191,7 +187,6 @@ impl VelocitySolver { for constraint in &mut *contact_constraints { constraint.solve( - 1.0, generic_contact_jacobians, &mut self.mj_lambdas[..], &mut self.generic_mj_lambdas, @@ -202,7 +197,6 @@ impl VelocitySolver { for constraint in &mut *contact_constraints { constraint.solve( - 1.0, generic_contact_jacobians, &mut self.mj_lambdas[..], &mut self.generic_mj_lambdas, |