From ed8645e34cfee653846f3cc1c91400b6f0d05ab5 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sun, 19 May 2024 15:49:42 +0200 Subject: feat: remove the "fast constraint" special case --- .../generic_one_body_constraint.rs | 9 ++------- .../generic_two_body_constraint.rs | 10 ++-------- .../solver/contact_constraint/one_body_constraint.rs | 17 +++-------------- .../contact_constraint/one_body_constraint_simd.rs | 14 +++----------- .../solver/contact_constraint/two_body_constraint.rs | 20 +++----------------- .../contact_constraint/two_body_constraint_simd.rs | 16 +++------------- 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, - 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, rb2_pos: &Isometry, - 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); } } -- cgit