From 0703e5527fd95d86bb6621e61dbcb1a6e7f9329a Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sun, 16 Jan 2022 16:40:59 +0100 Subject: Fix some solver issues - Fix the wrong codepath taken by the solver for contacts involving a collider without parent. - Properly adress the non-linear treatment of the friction direction - Simplify the sleeping strategy - Add an impulse resolution multiplier --- src/dynamics/solver/velocity_constraint_wide.rs | 27 ++++++++++++++++++------- 1 file changed, 20 insertions(+), 7 deletions(-) (limited to 'src/dynamics/solver/velocity_constraint_wide.rs') diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs index f072ad8..7fcb7f4 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -49,6 +49,7 @@ impl WVelocityConstraint { let inv_dt = SimdReal::splat(params.inv_dt()); let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction); let erp_inv_dt = SimdReal::splat(params.erp_inv_dt()); + let delassus_inv_factor = SimdReal::splat(params.delassus_inv_factor); let allowed_lin_err = SimdReal::splat(params.allowed_linear_error); let handles1 = gather![|ii| manifolds[ii].data.rigid_body1.unwrap()]; @@ -136,14 +137,14 @@ impl WVelocityConstraint { let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1)); let imsum = im1 + im2; - let r = SimdReal::splat(1.0) + let r = delassus_inv_factor / (force_dir1.dot(&imsum.component_mul(&force_dir1)) + gcross1.gdot(gcross1) + gcross2.gdot(gcross2)); let projected_velocity = (vel1 - vel2).dot(&force_dir1); let mut rhs_wo_bias = (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity; - rhs_wo_bias += (dist + allowed_lin_err).simd_max(SimdReal::zero()) * inv_dt; + rhs_wo_bias += dist.simd_max(SimdReal::zero()) * inv_dt; rhs_wo_bias *= is_bouncy + is_resting * velocity_solve_fraction; let rhs_bias = (dist + allowed_lin_err).simd_min(SimdReal::zero()) * (erp_inv_dt/* * is_resting */); @@ -165,16 +166,28 @@ impl WVelocityConstraint { let gcross1 = ii1.transform_vector(dp1.gcross(tangents1[j])); let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j])); let imsum = im1 + im2; - let r = SimdReal::splat(1.0) - / (tangents1[j].dot(&imsum.component_mul(&tangents1[j])) - + gcross1.gdot(gcross1) - + gcross2.gdot(gcross2)); + let r = tangents1[j].dot(&imsum.component_mul(&tangents1[j])) + + gcross1.gdot(gcross1) + + gcross2.gdot(gcross2); let rhs = (vel1 - vel2 + tangent_velocity).dot(&tangents1[j]); constraint.elements[k].tangent_part.gcross1[j] = gcross1; constraint.elements[k].tangent_part.gcross2[j] = gcross2; constraint.elements[k].tangent_part.rhs[j] = rhs; - constraint.elements[k].tangent_part.r[j] = r; + constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") { + SimdReal::splat(1.0) / r + } else { + r + }; + } + + #[cfg(feature = "dim3")] + { + constraint.elements[k].tangent_part.r[2] = SimdReal::splat(2.0) + * (constraint.elements[k].tangent_part.gcross1[0] + .gdot(constraint.elements[k].tangent_part.gcross1[1]) + + constraint.elements[k].tangent_part.gcross2[0] + .gdot(constraint.elements[k].tangent_part.gcross2[1])); } } -- cgit