diff --git a/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp b/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp index 2b690f05..cd9668d1 100644 --- a/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp +++ b/SHADE_Engine/src/Physics/Dynamics/SHContactSolver.cpp @@ -214,7 +214,9 @@ namespace SHADE * restituion bias = restitution * (relative velocity /dot normal) */ - const float ERROR_BIAS = -SHPHYSICS_BAUMGARTE * INV_DT * std::max(0.0f, contact.penetration - SHPHYSICS_LINEAR_SLOP); + float errorBias = 0.0f; + if (std::fabs(contact.penetration) > SHPHYSICS_LINEAR_SLOP) + errorBias = -SHPHYSICS_BAUMGARTE * INV_DT * std::max(0.0f, contact.penetration - SHPHYSICS_LINEAR_SLOP); // Warm starting // Compute impulses @@ -235,7 +237,7 @@ namespace SHADE const float RESTITUTION_BIAS = std::fabs(RV_N) > SHPHYSICS_RESTITUTION_THRESHOLD ? -constraint.restitution * RV_N : 0.0f; - contact.bias = ERROR_BIAS + RESTITUTION_BIAS; + contact.bias = errorBias + RESTITUTION_BIAS; } velocityStateA.LinearVelocity = vA; @@ -247,7 +249,7 @@ namespace SHADE void SHContactSolver::solve() noexcept { - static const float TEMP_INF = std::numeric_limits::infinity(); + for (auto& [key, constraint] : contactConstraints) {