Properly take the tangent_velocity into account in the velocity solver.

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")]