diff options
Diffstat (limited to 'src/dynamics/solver/velocity_ground_constraint.rs')
| -rw-r--r-- | src/dynamics/solver/velocity_ground_constraint.rs | 12 |
1 files changed, 9 insertions, 3 deletions
diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index d9229ff..d8ef8be 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -169,9 +169,15 @@ impl VelocityGroundConstraint { .transform_vector(dp2.gcross(-force_dir1)); let r = 1.0 / (rb2.mass_properties.inv_mass + gcross2.gdot(gcross2)); - let rhs = -vel2.dot(&force_dir1) - + vel1.dot(&force_dir1) - + manifold_point.dist.max(0.0) * params.inv_dt(); + + let mut rhs = (vel1 - vel2).dot(&force_dir1); + + if rhs <= -params.restitution_velocity_threshold { + rhs += manifold.restitution * rhs + } + + rhs += manifold_point.dist.max(0.0) * params.inv_dt(); + let impulse = manifold_points[k].impulse * warmstart_coeff; constraint.elements[k].normal_part = VelocityGroundConstraintElementPart { |