diff options
Diffstat (limited to 'src/dynamics/solver/velocity_ground_constraint.rs')
| -rw-r--r-- | src/dynamics/solver/velocity_ground_constraint.rs | 14 |
1 files changed, 9 insertions, 5 deletions
diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index a3a5563..4e195cd 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -64,6 +64,8 @@ impl VelocityGroundConstraint { push: bool, ) { let inv_dt = params.inv_dt(); + let velocity_based_erp_inv_dt = params.velocity_based_erp_inv_dt(); + let mut rb1 = &bodies[manifold.data.body_pair.body1]; let mut rb2 = &bodies[manifold.data.body_pair.body2]; let flipped = manifold.data.relative_dominance < 0; @@ -156,16 +158,18 @@ impl VelocityGroundConstraint { let r = 1.0 / (rb2.effective_inv_mass + 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 *= is_bouncy + is_resting * params.velocity_solve_fraction; + rhs += is_resting * velocity_based_erp_inv_dt * manifold_point.dist.min(0.0); constraint.elements[k].normal_part = VelocityGroundConstraintElementPart { gcross2, rhs, - impulse, + impulse: manifold_point.data.impulse * warmstart_coeff, r, }; } |