diff options
| author | Sébastien Crozet <developer@crozet.re> | 2022-01-16 08:20:22 -0800 |
|---|---|---|
| committer | GitHub <noreply@github.com> | 2022-01-16 08:20:22 -0800 |
| commit | 1880619d29029c99985ffae9ed12a1c8d2cc796b (patch) | |
| tree | 806e7d950015875ebfcca5520784aea6e7c5ae10 /src/dynamics/solver/velocity_constraint_wide.rs | |
| parent | 4454a845e98b990abf3929ca46b59d0fca5a18ec (diff) | |
| parent | 0703e5527fd95d86bb6621e61dbcb1a6e7f9329a (diff) | |
| download | rapier-1880619d29029c99985ffae9ed12a1c8d2cc796b.tar.gz rapier-1880619d29029c99985ffae9ed12a1c8d2cc796b.tar.bz2 rapier-1880619d29029c99985ffae9ed12a1c8d2cc796b.zip | |
Merge pull request #277 from dimforge/solver-fixes
Fix some solver issues
Diffstat (limited to 'src/dynamics/solver/velocity_constraint_wide.rs')
| -rw-r--r-- | src/dynamics/solver/velocity_constraint_wide.rs | 27 |
1 files changed, 20 insertions, 7 deletions
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])); } } |