btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer) { btScalar val = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer); //solve featherstone non-contact constraints //printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size()); for (int j=0;j<m_multiBodyNonContactConstraints.size();j++) { btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[j]; //if (iteration < constraint.m_overrideNumSolverIterations) //resolveSingleConstraintRowGenericMultiBody(constraint); resolveSingleConstraintRowGeneric(constraint); if(constraint.m_multiBodyA) constraint.m_multiBodyA->setPosUpdated(false); if(constraint.m_multiBodyB) constraint.m_multiBodyB->setPosUpdated(false); } //solve featherstone normal contact for (int j=0;j<m_multiBodyNormalContactConstraints.size();j++) { btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[j]; if (iteration < infoGlobal.m_numIterations) resolveSingleConstraintRowGeneric(constraint); if(constraint.m_multiBodyA) constraint.m_multiBodyA->setPosUpdated(false); if(constraint.m_multiBodyB) constraint.m_multiBodyB->setPosUpdated(false); } //solve featherstone frictional contact for (int j=0;j<this->m_multiBodyFrictionContactConstraints.size();j++) { if (iteration < infoGlobal.m_numIterations) { btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[j]; btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; //adjust friction limits here if (totalImpulse>btScalar(0)) { frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse); frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse; resolveSingleConstraintRowGeneric(frictionConstraint); if(frictionConstraint.m_multiBodyA) frictionConstraint.m_multiBodyA->setPosUpdated(false); if(frictionConstraint.m_multiBodyB) frictionConstraint.m_multiBodyB->setPosUpdated(false); } } } return val; }
// Project Gauss Seidel or the equivalent Sequential Impulse void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) { #ifdef USE_SIMD __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm))); __m128 deltaVel1Dotn = _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128)); __m128 deltaVel2Dotn = _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.m_deltaLinearVelocity.mVec128)); deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); btSimdScalar resultLowerLess,resultUpperLess; resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1); resultUpperLess = _mm_cmplt_ps(sum,upperLimit1); __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); __m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp); deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) ); c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) ); __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.m_invMass.mVec128); __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.m_invMass.mVec128); __m128 impulseMagnitude = deltaImpulse; body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); body2.m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); body2.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body2.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); #else resolveSingleConstraintRowGeneric(body1,body2,c); #endif }