New contacts are bouncy, old are resting

If a contact is new (previous impluse = 0), then we treat it as bouncy (respecting restitution).
If the contact is old we treat it as resting.

Exceptions for restitutions <=0 and >= 1.

1 files changed, 6 insertions, 8 deletions

diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs
index 20642ec..f147ccb 100644
--- a/src/dynamics/solver/velocity_ground_constraint.rs
+++ b/src/dynamics/solver/velocity_ground_constraint.rs
@@ -154,15 +154,13 @@ impl VelocityGroundConstraint {
 
                     let r = 1.0 / (rb2.effective_inv_mass + gcross2.gdot(gcross2));
 
-                    let mut rhs = (vel1 - vel2).dot(&force_dir1);
-
-                    if rhs <= -params.restitution_velocity_threshold {
-                        rhs += manifold_point.restitution * rhs
-                    }
-
-                    rhs += manifold_point.dist.max(0.0) * inv_dt;
+                    let rhs = if manifold_point.is_bouncy() {
+                        (1.0 + manifold_point.restitution) * (vel1 - vel2).dot(&force_dir1)
+                    } else {
+                        (vel1 - vel2).dot(&force_dir1) + manifold_point.dist.max(0.0) * inv_dt
+                    };
 
-                    let impulse = manifold_points[k].data.impulse * warmstart_coeff;
+                    let impulse = manifold_point.data.impulse * warmstart_coeff;
 
                     constraint.elements[k].normal_part = VelocityGroundConstraintElementPart {
                         gcross2,