diff options
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp')
| -rw-r--r-- | thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp | 31 |
1 files changed, 18 insertions, 13 deletions
diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index ffae5300f0..2788367431 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -30,23 +30,28 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl btScalar leastSquaredResidual = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); //solve featherstone non-contact constraints - + btScalar nonContactResidual = 0; //printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size()); - - for (int j = 0; j < m_multiBodyNonContactConstraints.size(); j++) + for (int i = 0; i < infoGlobal.m_numNonContactInnerIterations; ++i) { - int index = iteration & 1 ? j : m_multiBodyNonContactConstraints.size() - 1 - j; + // reset the nonContactResdual to 0 at start of each inner iteration + nonContactResidual = 0; + for (int j = 0; j < m_multiBodyNonContactConstraints.size(); j++) + { + int index = iteration & 1 ? j : m_multiBodyNonContactConstraints.size() - 1 - j; - btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[index]; + btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[index]; - btScalar residual = resolveSingleConstraintRowGeneric(constraint); - leastSquaredResidual = btMax(leastSquaredResidual, residual * residual); + btScalar residual = resolveSingleConstraintRowGeneric(constraint); + nonContactResidual = btMax(nonContactResidual, residual * residual); - if (constraint.m_multiBodyA) - constraint.m_multiBodyA->setPosUpdated(false); - if (constraint.m_multiBodyB) - constraint.m_multiBodyB->setPosUpdated(false); + if (constraint.m_multiBodyA) + constraint.m_multiBodyA->setPosUpdated(false); + if (constraint.m_multiBodyB) + constraint.m_multiBodyB->setPosUpdated(false); + } } + leastSquaredResidual = btMax(leastSquaredResidual, nonContactResidual); //solve featherstone normal contact for (int j0 = 0; j0 < m_multiBodyNormalContactConstraints.size(); j0++) @@ -1250,7 +1255,7 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* { const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0()); const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1()); - + btMultiBody* mbA = fcA ? fcA->m_multiBody : 0; btMultiBody* mbB = fcB ? fcB->m_multiBody : 0; @@ -1270,7 +1275,7 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* // return; //only a single rollingFriction per manifold - int rollingFriction = 1; + int rollingFriction = 4; for (int j = 0; j < manifold->getNumContacts(); j++) { |