From c273e7cfe580037c6cb7073b0050e0ce7d6da231 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 1 Apr 2021 22:59:20 +0200 Subject: [PATCH] Fix issue in the computation of the friction vectors in contact solver --- src/systems/ContactSolverSystem.cpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/src/systems/ContactSolverSystem.cpp b/src/systems/ContactSolverSystem.cpp index f384a78a..d50de136 100644 --- a/src/systems/ContactSolverSystem.cpp +++ b/src/systems/ContactSolverSystem.cpp @@ -532,8 +532,10 @@ void ContactSolverSystem::solve() { // Compute the bias "b" of the constraint decimal biasPenetrationDepth = 0.0; - if (mContactPoints[contactPointIndex].penetrationDepth > SLOP) biasPenetrationDepth = -(beta/mTimeStep) * + if (mContactPoints[contactPointIndex].penetrationDepth > SLOP) { + biasPenetrationDepth = -(beta/mTimeStep) * max(0.0f, float(mContactPoints[contactPointIndex].penetrationDepth - SLOP)); + } decimal b = biasPenetrationDepth + mContactPoints[contactPointIndex].restitutionBias; // Compute the Lagrange multiplier lambda @@ -662,7 +664,6 @@ void ContactSolverSystem::solve() { mContactConstraints[c].r2CrossT1.y * deltaLambda, mContactConstraints[c].r2CrossT1.z * deltaLambda); - // Update the velocities of the body 1 by applying the impulse P mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].x -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.x; mRigidBodyComponents.mConstrainedLinearVelocities[mContactConstraints[c].rigidBodyComponentIndexBody1].y -= mContactConstraints[c].massInverseBody1 * linearImpulseBody2.y; @@ -825,17 +826,15 @@ void ContactSolverSystem::storeImpulses() { // Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane // for a contact manifold. The two vectors have to be such that : t1 x t2 = contactNormal. -void ContactSolverSystem::computeFrictionVectors(const Vector3& deltaVelocity, - ContactManifoldSolver& contact) const { +void ContactSolverSystem::computeFrictionVectors(const Vector3& deltaVelocity, ContactManifoldSolver& contact) const { RP3D_PROFILE("ContactSolver::computeFrictionVectors()", mProfiler); assert(contact.normal.length() > decimal(0.0)); // Compute the velocity difference vector in the tangential plane - Vector3 normalVelocity(deltaVelocity.x * contact.normal.x * contact.normal.x, - deltaVelocity.y * contact.normal.y * contact.normal.y, - deltaVelocity.z * contact.normal.z * contact.normal.z); + decimal deltaVDotNormal = deltaVelocity.dot(contact.normal); + Vector3 normalVelocity = deltaVDotNormal * contact.normal; Vector3 tangentVelocity(deltaVelocity.x - normalVelocity.x, deltaVelocity.y - normalVelocity.y, deltaVelocity.z - normalVelocity.z);