Merge pull request #114 from EmbarkStudios/split-contacts-2

Split bouncy and resting contacts (take 2)

1 files changed, 6 insertions, 7 deletions

diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs
index a8384de..3e068e7 100644
--- a/src/dynamics/solver/velocity_constraint_wide.rs
+++ b/src/dynamics/solver/velocity_constraint_wide.rs
@@ -96,8 +96,6 @@ impl WVelocityConstraint {
         let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
         let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
 
-        let restitution_velocity_threshold = SimdReal::splat(params.restitution_velocity_threshold);
-
         let warmstart_multiplier =
             SimdReal::from(array![|ii| manifolds[ii].data.warmstart_multiplier; SIMD_WIDTH]);
         let warmstart_coeff = warmstart_multiplier * SimdReal::splat(params.warmstart_coeff);
@@ -127,6 +125,9 @@ impl WVelocityConstraint {
                     SimdReal::from(array![|ii| manifold_points[ii][k].friction; SIMD_WIDTH]);
                 let restitution =
                     SimdReal::from(array![|ii| manifold_points[ii][k].restitution; SIMD_WIDTH]);
+                let is_bouncy = SimdReal::from(
+                    array![|ii| manifold_points[ii][k].is_bouncy() as u32 as Real; SIMD_WIDTH],
+                );
                 let point = Point::from(array![|ii| manifold_points[ii][k].point; SIMD_WIDTH]);
                 let dist = SimdReal::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]);
 
@@ -148,11 +149,9 @@ impl WVelocityConstraint {
 
                     let r = SimdReal::splat(1.0)
                         / (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2));
-                    let mut rhs = (vel1 - vel2).dot(&force_dir1);
-                    let use_restitution = rhs.simd_le(-restitution_velocity_threshold);
-                    let rhs_with_restitution = rhs + rhs * restitution;
-                    rhs = rhs_with_restitution.select(use_restitution, rhs);
-                    rhs += dist.simd_max(SimdReal::zero()) * inv_dt;
+                    let projected_velocity = (vel1 - vel2).dot(&force_dir1);
+                    let rhs = (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity
+                        + dist.simd_max(SimdReal::zero()) * inv_dt;
 
                     constraint.elements[k].normal_part = WVelocityConstraintElementPart {
                         gcross1,