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

1 files changed, 14 insertions, 8 deletions

diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs
index bd60633..76de3f9 100644
--- a/src/dynamics/solver/velocity_ground_constraint.rs
+++ b/src/dynamics/solver/velocity_ground_constraint.rs
@@ -153,7 +153,7 @@ impl VelocityGroundConstraint {
                         .effective_world_inv_inertia_sqrt
                         .transform_vector(dp2.gcross(-force_dir1));
 
-                    let r = 1.0
+                    let r = params.delassus_inv_factor
                         / (force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1))
                             + gcross2.gdot(gcross2));
 
@@ -162,8 +162,7 @@ impl VelocityGroundConstraint {
 
                     let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution)
                         * (vel1 - vel2).dot(&force_dir1);
-                    rhs_wo_bias +=
-                        (manifold_point.dist + params.allowed_linear_error).max(0.0) * inv_dt;
+                    rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt;
                     rhs_wo_bias *= is_bouncy + is_resting * params.velocity_solve_fraction;
                     let rhs_bias = /* is_resting
                         * */ erp_inv_dt
@@ -186,17 +185,24 @@ impl VelocityGroundConstraint {
                         let gcross2 = mprops2
                             .effective_world_inv_inertia_sqrt
                             .transform_vector(dp2.gcross(-tangents1[j]));
-                        let r = 1.0
-                            / (tangents1[j]
-                                .dot(&mprops2.effective_inv_mass.component_mul(&tangents1[j]))
-                                + gcross2.gdot(gcross2));
+                        let r = tangents1[j]
+                            .dot(&mprops2.effective_inv_mass.component_mul(&tangents1[j]))
+                            + gcross2.gdot(gcross2);
                         let rhs = (vel1 - vel2
                             + flipped_multiplier * manifold_point.tangent_velocity)
                             .dot(&tangents1[j]);
 
                         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") { 1.0 / r } else { r };
+                    }
+
+                    #[cfg(feature = "dim3")]
+                    {
+                        constraint.elements[k].tangent_part.r[2] = 2.0
+                            * constraint.elements[k].tangent_part.gcross2[0]
+                                .gdot(constraint.elements[k].tangent_part.gcross2[1]);
                     }
                 }
             }