From 6ce26f3818492682a8572c895264f1e63f94b9d5 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Mon, 30 May 2022 18:21:35 +0200 Subject: CCD improvements MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 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. --- src/dynamics/solver/velocity_ground_constraint.rs | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) (limited to 'src/dynamics/solver/velocity_ground_constraint.rs') 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, insert_at: Option, ) { + 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, }; } -- cgit