From cb1cd969a8d62e6ed775649d61c2f3a7f94edf8f Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 8 May 2020 16:51:53 -0700 Subject: [PATCH] allow positive penetration to pose inactive constraints for more stability --- .../btDeformableContactConstraint.cpp | 50 +++++++++++-------- .../btDeformableContactProjection.cpp | 10 +--- .../btDeformableMultiBodyConstraintSolver.cpp | 4 +- .../btDeformableMultiBodyDynamicsWorld.cpp | 3 +- src/BulletSoftBody/btSoftBody.cpp | 4 +- 5 files changed, 34 insertions(+), 37 deletions(-) diff --git a/src/BulletSoftBody/btDeformableContactConstraint.cpp b/src/BulletSoftBody/btDeformableContactConstraint.cpp index 8d6d0173d..5ef809384 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.cpp +++ b/src/BulletSoftBody/btDeformableContactConstraint.cpp @@ -267,28 +267,30 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv btVector3 va = getVa(); btVector3 vb = getVb(); btVector3 vr = vb - va; - btScalar dn = btDot(vr, cti.m_normal); - if (!infoGlobal.m_splitImpulse) - { - dn += m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep; - } + btScalar dn = btDot(vr, cti.m_normal) + m_total_normal_dv.dot(cti.m_normal) * infoGlobal.deformable_cfm; + if (m_penetration > 0) + { + dn += m_penetration / infoGlobal.m_timeStep; + } + if (!infoGlobal.m_splitImpulse) + { + dn += m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep; + } // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt - btVector3 impulse = m_contact->m_c0 * vr; - if (!infoGlobal.m_splitImpulse) - { - impulse += m_contact->m_c0 * (m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep * cti.m_normal); - } + btVector3 impulse = m_contact->m_c0 * (vr + m_total_normal_dv * infoGlobal.deformable_cfm + ((m_penetration > 0) ? m_penetration / infoGlobal.m_timeStep * cti.m_normal : btVector3(0,0,0))); + if (!infoGlobal.m_splitImpulse) + { + impulse += m_contact->m_c0 * (m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep * cti.m_normal); + } btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * dn); btVector3 impulse_tangent = impulse - impulse_normal; - if (dn > 0) - { - dn = 0; - impulse_normal.setZero(); - m_binding = false; - return 0; - } - m_binding = true; - btScalar residualSquare = dn*dn; + if (dn > 0) + { + m_binding = false; + return 0; + } + m_binding = true; + btScalar residualSquare = dn*dn; btVector3 old_total_tangent_dv = m_total_tangent_dv; // m_c2 is the inverse mass of the deformable node/face m_total_normal_dv -= impulse_normal * m_contact->m_c2; @@ -360,12 +362,16 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv btScalar btDeformableRigidContactConstraint::solveSplitImpulse(const btContactSolverInfo& infoGlobal) { - btScalar MAX_PENETRATION_CORRECTION = 0.1; + btScalar MAX_PENETRATION_CORRECTION = 0.01; const btSoftBody::sCti& cti = m_contact->m_cti; btVector3 vb = getSplitVb(); - btVector3 va = getSplitVa(); + btVector3 va = getSplitVa(); btScalar p = m_penetration; - btVector3 vr = vb - va; + if (p > 0) + { + return 0; + } + btVector3 vr = vb - va; btScalar dn = btDot(vr, cti.m_normal) + p * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep; if (dn > 0) { diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index 4e88c79d9..a0e184f43 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -126,15 +126,7 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in continue; } btDeformableNodeRigidContactConstraint constraint(contact, infoGlobal); - btVector3 va = constraint.getVa(); - btVector3 vb = constraint.getVb(); - const btVector3 vr = vb - va; - const btSoftBody::sCti& cti = contact.m_cti; - const btScalar dn = btDot(vr, cti.m_normal); - if (dn < SIMD_EPSILON) - { - m_nodeRigidConstraints[i].push_back(constraint); - } + m_nodeRigidConstraints[i].push_back(constraint); } // set Deformable Face vs. Rigid constraint diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp index 8c1f4bd7b..e289d87a5 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp @@ -24,7 +24,6 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, deformableBodies, numDeformableBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; - maxIterations = 150; for (int iteration = 0; iteration < maxIterations; iteration++) { // rigid bodies are solved using solver body velocity, but rigid/deformable contact directly uses the velocity of the actual rigid body. So we have to do the following: Solve one iteration of the rigid/rigid contact, get the updated velocity in the solver body and update the velocity of the underlying rigid body. Then solve the rigid/deformable contact. Finally, grab the (once again) updated rigid velocity and update the velocity of the wrapping solver body @@ -114,8 +113,7 @@ void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseI if (infoGlobal.m_splitImpulse) { { - for (iteration = 0; iteration < 150; iteration++) -// for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++) + for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++) { btScalar leastSquaresResidual = 0.f; { diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 3fc3ce964..969ff1ad0 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -532,12 +532,13 @@ void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep) if (m_useProjection) { m_deformableBodySolver->m_useProjection = true; -// m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = true; + m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = true; m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_massPreconditioner; } else { m_deformableBodySolver->m_useProjection = false; + m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = false; m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_KKTPreconditioner; } diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index e70e94f5f..1616340ee 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -2943,8 +2943,8 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO btVector3 guess(0,0,0); const btConvexShape* csh = static_cast(shp); btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results); - dst = results.distance-csh->getMargin(); - dst -= margin; + dst = results.distance-2.0*csh->getMargin(); + dst -= 2.0*margin; if (dst >= 0) return false; wtr = colObjWrap->getWorldTransform();