diff options
Diffstat (limited to 'src/dynamics/solver/velocity_ground_constraint.rs')
| -rw-r--r-- | src/dynamics/solver/velocity_ground_constraint.rs | 12 |
1 files changed, 7 insertions, 5 deletions
diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index 6f61b17..ddd5628 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -68,11 +68,11 @@ impl VelocityGroundConstraint { let mut rb2 = &bodies[manifold.data.body_pair.body2]; let flipped = !rb2.is_dynamic(); - let force_dir1 = if flipped { + let (force_dir1, flipped_multiplier) = if flipped { std::mem::swap(&mut rb1, &mut rb2); - manifold.data.normal + (manifold.data.normal, -1.0) } else { - -manifold.data.normal + (-manifold.data.normal, 1.0) }; let mj_lambda2 = rb2.active_set_offset; @@ -123,7 +123,7 @@ impl VelocityGroundConstraint { .as_nongrouped_ground_mut() .unwrap() } else { - unreachable!(); // We don't have parallelization on WASMÂ yet, so this is unreachable. + unreachable!(); // We don't have parallelization on WASM yet, so this is unreachable. }; #[cfg(target_arch = "wasm32")] @@ -179,7 +179,9 @@ impl VelocityGroundConstraint { .effective_world_inv_inertia_sqrt .transform_vector(dp2.gcross(-tangents1[j])); let r = 1.0 / (rb2.effective_inv_mass + gcross2.gdot(gcross2)); - let rhs = -vel2.dot(&tangents1[j]) + vel1.dot(&tangents1[j]); + let rhs = (vel1 - vel2 + + flipped_multiplier * manifold_point.tangent_velocity) + .dot(&tangents1[j]); #[cfg(feature = "dim2")] let impulse = manifold_points[k].data.tangent_impulse * warmstart_coeff; #[cfg(feature = "dim3")] |