From 296602fc576fd71dadec32d4bfb3ad7bcea5ea72 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Thu, 23 Apr 2020 01:54:37 -0700 Subject: [PATCH] add max split impulse push out distance and switch contact to be double sided --- src/BulletSoftBody/btConjugateResidual.h | 20 ++---------------- .../btDeformableContactConstraint.cpp | 21 ++++++++++++++++--- .../btDeformableContactConstraint.h | 1 + .../btDeformableContactProjection.cpp | 19 +++++++++-------- .../btDeformableMultiBodyConstraintSolver.cpp | 6 ++++-- .../btDeformableMultiBodyDynamicsWorld.cpp | 3 ++- 6 files changed, 37 insertions(+), 33 deletions(-) diff --git a/src/BulletSoftBody/btConjugateResidual.h b/src/BulletSoftBody/btConjugateResidual.h index 7b211c417..cb2a48762 100644 --- a/src/BulletSoftBody/btConjugateResidual.h +++ b/src/BulletSoftBody/btConjugateResidual.h @@ -36,7 +36,7 @@ public: btConjugateResidual(const int max_it_in) : max_iterations(max_it_in) { - tolerance_squared = 1e-2; + tolerance_squared = 1e-5; } virtual ~btConjugateResidual(){} @@ -55,11 +55,6 @@ public: r = z; btScalar residual_norm = norm(r); if (residual_norm <= tolerance_squared) { - if (verbose) - { - std::cout << "Iteration = 0" << std::endl; - std::cout << "Two norm of the residual = " << residual_norm << std::endl; - } return 0; } p = r; @@ -84,19 +79,8 @@ public: best_x = x; best_r = norm_r; if (norm_r < tolerance_squared) { - if (verbose) - { - std::cout << "ConjugateResidual iterations " << k << std::endl; - } return k; } - else - { - if (verbose) - { - std::cout << "ConjugateResidual iterations " << k << " has residual "<< norm_r << std::endl; - } - } } // temp_r = A * r; A.multiply(r, temp_r); @@ -110,7 +94,7 @@ public: } if (verbose) { - std::cout << "ConjugateResidual max iterations reached " << max_iterations << std::endl; + std::cout << "ConjugateResidual max iterations reached, residual = " << best_r << std::endl; } x = best_x; return max_iterations; diff --git a/src/BulletSoftBody/btDeformableContactConstraint.cpp b/src/BulletSoftBody/btDeformableContactConstraint.cpp index e4a2001db..1d9e28d73 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.cpp +++ b/src/BulletSoftBody/btDeformableContactConstraint.cpp @@ -142,6 +142,7 @@ btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btS m_total_tangent_dv.setZero(); // The magnitude of penetration is the depth of penetration. m_penetration = c.m_cti.m_offset; + m_total_split_impulse = 0; // m_penetration = btMin(btScalar(0),c.m_cti.m_offset); } @@ -149,6 +150,7 @@ btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btD : m_contact(other.m_contact) , btDeformableContactConstraint(other) , m_penetration(other.m_penetration) +, m_total_split_impulse(other.m_total_split_impulse) { m_total_normal_dv = other.m_total_normal_dv; m_total_tangent_dv = other.m_total_tangent_dv; @@ -219,7 +221,11 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv 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 - btScalar residualSquare = dn*dn; + if (dn > 0) + { + return 0; + } + btScalar residualSquare = dn*dn; btVector3 impulse = m_contact->m_c0 * vr; if (!infoGlobal.m_splitImpulse) { @@ -265,8 +271,6 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv impulse = impulse_normal + impulse_tangent; // apply impulse to deformable nodes involved and change their velocities applyImpulse(impulse); - if (residualSquare < 1e-7) - return residualSquare; // apply impulse to the rigid/multibodies involved and change their velocities if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) { @@ -301,12 +305,23 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv btScalar btDeformableRigidContactConstraint::solveSplitImpulse(const btContactSolverInfo& infoGlobal) { + btScalar MAX_PENETRATION_CORRECTION = 0.1; const btSoftBody::sCti& cti = m_contact->m_cti; btVector3 vb = getSplitVb(); btScalar p = m_penetration; btScalar dn = btDot(vb, cti.m_normal) + p * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep; if (dn > 0) return 0; + if (m_total_split_impulse + dn > MAX_PENETRATION_CORRECTION) + { + dn = MAX_PENETRATION_CORRECTION - m_total_split_impulse; + } + if (m_total_split_impulse + dn < -MAX_PENETRATION_CORRECTION) + { + dn = -MAX_PENETRATION_CORRECTION - m_total_split_impulse; + } + m_total_split_impulse += dn; + btScalar residualSquare = dn*dn; const btVector3 impulse = 1.0/m_contact->m_c2 * (cti.m_normal * dn); applySplitImpulse(impulse); diff --git a/src/BulletSoftBody/btDeformableContactConstraint.h b/src/BulletSoftBody/btDeformableContactConstraint.h index 8e73e4da4..134199f7e 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.h +++ b/src/BulletSoftBody/btDeformableContactConstraint.h @@ -148,6 +148,7 @@ public: btVector3 m_total_normal_dv; btVector3 m_total_tangent_dv; btScalar m_penetration; + btScalar m_total_split_impulse; const btSoftBody::DeformableRigidContact* m_contact; btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal); diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index 20a720273..8f20d80b6 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -147,15 +147,16 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in continue; } btDeformableFaceRigidContactConstraint constraint(contact, infoGlobal, m_useStrainLimiting); - 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_faceRigidConstraints[i].push_back(constraint); - } + m_faceRigidConstraints[i].push_back(constraint); + // 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_faceRigidConstraints[i].push_back(constraint); + // } } } } diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp index 6a3f677f6..e289d87a5 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp @@ -41,7 +41,8 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1))) { #ifdef VERBOSE_RESIDUAL_PRINTF - printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration); + if (iteration >= (maxIterations - 1)) + printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration); #endif m_analyticsData.m_numSolverCalls++; m_analyticsData.m_numIterationsUsed = iteration+1; @@ -133,7 +134,8 @@ void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseI if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1)) { #ifdef VERBOSE_RESIDUAL_PRINTF - printf("residual = %f at iteration #%d\n", leastSquaresResidual, iteration); + if (iteration >= (infoGlobal.m_numIterations - 1)) + printf("split impulse residual = %f at iteration #%d\n", leastSquaresResidual, iteration); #endif break; } diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 46004bd37..3fc3ce964 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -61,7 +61,7 @@ m_deformableBodySolver(deformableBodySolver), m_solverCallback(0) m_internalTime = 0.0; m_implicit = false; m_lineSearch = false; - m_useProjection = true; + m_useProjection = false; m_ccdIterations = 5; m_solverDeformableBodyIslandCallback = new DeformableBodyInplaceSolverIslandCallback(constraintSolver, dispatcher); } @@ -537,6 +537,7 @@ void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep) } else { + m_deformableBodySolver->m_useProjection = false; m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_KKTPreconditioner; }