Add restorative impulse in velocity solver

1 files changed, 10 insertions, 5 deletions

diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs
index abc46c9..dff8ff0 100644
--- a/src/dynamics/solver/velocity_constraint.rs
+++ b/src/dynamics/solver/velocity_constraint.rs
@@ -244,17 +244,22 @@ impl VelocityConstraint {
                             + gcross2.gdot(gcross2));
 
                     let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
-                    let rhs = (1.0 + is_bouncy * manifold_point.restitution)
-                        * (vel1 - vel2).dot(&force_dir1)
-                        + manifold_point.dist.max(0.0) * inv_dt;
+                    let is_resting = 1.0 - is_bouncy;
 
-                    let impulse = manifold_point.data.impulse * warmstart_coeff;
+                    let mut rhs = (1.0 + is_bouncy * manifold_point.restitution)
+                        * (vel1 - vel2).dot(&force_dir1);
+                    rhs += manifold_point.dist.max(0.0) * inv_dt;
+                    rhs *= params.velocity_solve_fraction;
+                    rhs += is_resting
+                        * params.velocity_based_erp
+                        * inv_dt
+                        * manifold_point.dist.min(0.0);
 
                     constraint.elements[k].normal_part = VelocityConstraintElementPart {
                         gcross1,
                         gcross2,
                         rhs,
-                        impulse,
+                        impulse: manifold_point.data.impulse * warmstart_coeff,
                         r,
                     };
                 }