00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #include "btSequentialImpulseConstraintSolver.h"
00020 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
00021
00022 #include "LinearMath/btIDebugDraw.h"
00023
00024 #include "LinearMath/btMinMax.h"
00025 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
00026 #include <new>
00027 #include "LinearMath/btStackAlloc.h"
00028 #include "LinearMath/btQuickprof.h"
00029
00030
00031 #include "LinearMath/btAlignedObjectArray.h"
00032 #include <string.h>
00033
00034 int gNumSplitImpulseRecoveries = 0;
00035
00036 #include "BulletDynamics/Dynamics/btRigidBody.h"
00037
00038 btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
00039 :m_btSeed2(0)
00040 {
00041
00042 }
00043
00044 btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
00045 {
00046 }
00047
00048 #ifdef USE_SIMD
00049 #include <emmintrin.h>
00050 #define btVecSplat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e))
00051 static inline __m128 btSimdDot3( __m128 vec0, __m128 vec1 )
00052 {
00053 __m128 result = _mm_mul_ps( vec0, vec1);
00054 return _mm_add_ps( btVecSplat( result, 0 ), _mm_add_ps( btVecSplat( result, 1 ), btVecSplat( result, 2 ) ) );
00055 }
00056 #endif//USE_SIMD
00057
00058
00059 void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
00060 {
00061 #ifdef USE_SIMD
00062 __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
00063 __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
00064 __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
00065 __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)));
00066 __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
00067 __m128 deltaVel2Dotn = _mm_sub_ps(btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),btSimdDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128));
00068 deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
00069 deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
00070 btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
00071 btSimdScalar resultLowerLess,resultUpperLess;
00072 resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
00073 resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
00074 __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
00075 deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
00076 c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
00077 __m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp);
00078 deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) );
00079 c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) );
00080 __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128);
00081 __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128);
00082 __m128 impulseMagnitude = deltaImpulse;
00083 body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
00084 body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
00085 body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
00086 body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
00087 #else
00088 resolveSingleConstraintRowGeneric(body1,body2,c);
00089 #endif
00090 }
00091
00092
00093 void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
00094 {
00095 btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
00096 const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
00097 const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
00098
00099
00100 deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
00101 deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
00102
00103 const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
00104 if (sum < c.m_lowerLimit)
00105 {
00106 deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
00107 c.m_appliedImpulse = c.m_lowerLimit;
00108 }
00109 else if (sum > c.m_upperLimit)
00110 {
00111 deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
00112 c.m_appliedImpulse = c.m_upperLimit;
00113 }
00114 else
00115 {
00116 c.m_appliedImpulse = sum;
00117 }
00118
00119 body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
00120 body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
00121 }
00122
00123 void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
00124 {
00125 #ifdef USE_SIMD
00126 __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
00127 __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
00128 __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
00129 __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)));
00130 __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
00131 __m128 deltaVel2Dotn = _mm_sub_ps(btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),btSimdDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128));
00132 deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
00133 deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
00134 btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
00135 btSimdScalar resultLowerLess,resultUpperLess;
00136 resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
00137 resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
00138 __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
00139 deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
00140 c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
00141 __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128);
00142 __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128);
00143 __m128 impulseMagnitude = deltaImpulse;
00144 body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
00145 body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
00146 body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
00147 body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
00148 #else
00149 resolveSingleConstraintRowLowerLimit(body1,body2,c);
00150 #endif
00151 }
00152
00153
00154 void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
00155 {
00156 btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
00157 const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
00158 const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
00159
00160 deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
00161 deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
00162 const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
00163 if (sum < c.m_lowerLimit)
00164 {
00165 deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
00166 c.m_appliedImpulse = c.m_lowerLimit;
00167 }
00168 else
00169 {
00170 c.m_appliedImpulse = sum;
00171 }
00172 body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
00173 body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
00174 }
00175
00176
00177 void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly(
00178 btSolverBody& body1,
00179 btSolverBody& body2,
00180 const btSolverConstraint& c)
00181 {
00182 if (c.m_rhsPenetration)
00183 {
00184 gNumSplitImpulseRecoveries++;
00185 btScalar deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm;
00186 const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity());
00187 const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity());
00188
00189 deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv;
00190 deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv;
00191 const btScalar sum = btScalar(c.m_appliedPushImpulse) + deltaImpulse;
00192 if (sum < c.m_lowerLimit)
00193 {
00194 deltaImpulse = c.m_lowerLimit-c.m_appliedPushImpulse;
00195 c.m_appliedPushImpulse = c.m_lowerLimit;
00196 }
00197 else
00198 {
00199 c.m_appliedPushImpulse = sum;
00200 }
00201 body1.internalApplyPushImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
00202 body2.internalApplyPushImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
00203 }
00204 }
00205
00206 void btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
00207 {
00208 #ifdef USE_SIMD
00209 if (!c.m_rhsPenetration)
00210 return;
00211
00212 gNumSplitImpulseRecoveries++;
00213
00214 __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedPushImpulse);
00215 __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
00216 __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit);
00217 __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhsPenetration), _mm_mul_ps(_mm_set1_ps(c.m_appliedPushImpulse),_mm_set1_ps(c.m_cfm)));
00218 __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal.mVec128,body1.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128));
00219 __m128 deltaVel2Dotn = _mm_sub_ps(btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128),btSimdDot3((c.m_contactNormal).mVec128,body2.internalGetPushVelocity().mVec128));
00220 deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
00221 deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
00222 btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
00223 btSimdScalar resultLowerLess,resultUpperLess;
00224 resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
00225 resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
00226 __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
00227 deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
00228 c.m_appliedPushImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
00229 __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128);
00230 __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128);
00231 __m128 impulseMagnitude = deltaImpulse;
00232 body1.internalGetPushVelocity().mVec128 = _mm_add_ps(body1.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
00233 body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
00234 body2.internalGetPushVelocity().mVec128 = _mm_sub_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
00235 body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
00236 #else
00237 resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c);
00238 #endif
00239 }
00240
00241
00242
00243 unsigned long btSequentialImpulseConstraintSolver::btRand2()
00244 {
00245 m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
00246 return m_btSeed2;
00247 }
00248
00249
00250
00251
00252 int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
00253 {
00254
00255 const unsigned long un = static_cast<unsigned long>(n);
00256 unsigned long r = btRand2();
00257
00258
00259
00260 if (un <= 0x00010000UL) {
00261 r ^= (r >> 16);
00262 if (un <= 0x00000100UL) {
00263 r ^= (r >> 8);
00264 if (un <= 0x00000010UL) {
00265 r ^= (r >> 4);
00266 if (un <= 0x00000004UL) {
00267 r ^= (r >> 2);
00268 if (un <= 0x00000002UL) {
00269 r ^= (r >> 1);
00270 }
00271 }
00272 }
00273 }
00274 }
00275
00276 return (int) (r % un);
00277 }
00278
00279
00280
00281 void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
00282 {
00283
00284 btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0;
00285
00286 solverBody->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
00287 solverBody->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
00288 solverBody->internalGetPushVelocity().setValue(0.f,0.f,0.f);
00289 solverBody->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
00290
00291 if (rb)
00292 {
00293 solverBody->m_worldTransform = rb->getWorldTransform();
00294 solverBody->internalSetInvMass(btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor());
00295 solverBody->m_originalBody = rb;
00296 solverBody->m_angularFactor = rb->getAngularFactor();
00297 solverBody->m_linearFactor = rb->getLinearFactor();
00298 solverBody->m_linearVelocity = rb->getLinearVelocity();
00299 solverBody->m_angularVelocity = rb->getAngularVelocity();
00300 } else
00301 {
00302 solverBody->m_worldTransform.setIdentity();
00303 solverBody->internalSetInvMass(btVector3(0,0,0));
00304 solverBody->m_originalBody = 0;
00305 solverBody->m_angularFactor.setValue(1,1,1);
00306 solverBody->m_linearFactor.setValue(1,1,1);
00307 solverBody->m_linearVelocity.setValue(0,0,0);
00308 solverBody->m_angularVelocity.setValue(0,0,0);
00309 }
00310
00311
00312 }
00313
00314
00315
00316
00317
00318
00319 btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution)
00320 {
00321 btScalar rest = restitution * -rel_vel;
00322 return rest;
00323 }
00324
00325
00326
00327 static void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode);
00328 static void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode)
00329 {
00330
00331
00332 if (colObj && colObj->hasAnisotropicFriction(frictionMode))
00333 {
00334
00335 btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis();
00336 const btVector3& friction_scaling = colObj->getAnisotropicFriction();
00337
00338 loc_lateral *= friction_scaling;
00339
00340 frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral;
00341 }
00342
00343 }
00344
00345
00346
00347
00348 void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
00349 {
00350
00351
00352 solverConstraint.m_contactNormal = normalAxis;
00353 btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
00354 btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
00355
00356 btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody;
00357 btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
00358
00359 solverConstraint.m_solverBodyIdA = solverBodyIdA;
00360 solverConstraint.m_solverBodyIdB = solverBodyIdB;
00361
00362 solverConstraint.m_friction = cp.m_combinedFriction;
00363 solverConstraint.m_originalContactPoint = 0;
00364
00365 solverConstraint.m_appliedImpulse = 0.f;
00366 solverConstraint.m_appliedPushImpulse = 0.f;
00367
00368 {
00369 btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
00370 solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
00371 solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0);
00372 }
00373 {
00374 btVector3 ftorqueAxis1 = rel_pos2.cross(-solverConstraint.m_contactNormal);
00375 solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
00376 solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0);
00377 }
00378
00379 {
00380 btVector3 vec;
00381 btScalar denom0 = 0.f;
00382 btScalar denom1 = 0.f;
00383 if (body0)
00384 {
00385 vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
00386 denom0 = body0->getInvMass() + normalAxis.dot(vec);
00387 }
00388 if (body1)
00389 {
00390 vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
00391 denom1 = body1->getInvMass() + normalAxis.dot(vec);
00392 }
00393 btScalar denom = relaxation/(denom0+denom1);
00394 solverConstraint.m_jacDiagABInv = denom;
00395 }
00396
00397 {
00398
00399
00400 btScalar rel_vel;
00401 btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?solverBodyA.m_linearVelocity:btVector3(0,0,0))
00402 + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0));
00403 btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?solverBodyB.m_linearVelocity:btVector3(0,0,0))
00404 + solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0));
00405
00406 rel_vel = vel1Dotn+vel2Dotn;
00407
00408
00409
00410 btSimdScalar velocityError = desiredVelocity - rel_vel;
00411 btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
00412 solverConstraint.m_rhs = velocityImpulse;
00413 solverConstraint.m_cfm = cfmSlip;
00414 solverConstraint.m_lowerLimit = 0;
00415 solverConstraint.m_upperLimit = 1e10f;
00416
00417 }
00418 }
00419
00420 btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
00421 {
00422 btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
00423 solverConstraint.m_frictionIndex = frictionIndex;
00424 setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
00425 colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
00426 return solverConstraint;
00427 }
00428
00429
00430 void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis1,int solverBodyIdA,int solverBodyIdB,
00431 btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
00432 btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
00433 btScalar desiredVelocity, btScalar cfmSlip)
00434
00435 {
00436 btVector3 normalAxis(0,0,0);
00437
00438
00439 solverConstraint.m_contactNormal = normalAxis;
00440 btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
00441 btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
00442
00443 btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody;
00444 btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
00445
00446 solverConstraint.m_solverBodyIdA = solverBodyIdA;
00447 solverConstraint.m_solverBodyIdB = solverBodyIdB;
00448
00449 solverConstraint.m_friction = cp.m_combinedRollingFriction;
00450 solverConstraint.m_originalContactPoint = 0;
00451
00452 solverConstraint.m_appliedImpulse = 0.f;
00453 solverConstraint.m_appliedPushImpulse = 0.f;
00454
00455 {
00456 btVector3 ftorqueAxis1 = -normalAxis1;
00457 solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
00458 solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0);
00459 }
00460 {
00461 btVector3 ftorqueAxis1 = normalAxis1;
00462 solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
00463 solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0);
00464 }
00465
00466
00467 {
00468 btVector3 iMJaA = body0?body0->getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal:btVector3(0,0,0);
00469 btVector3 iMJaB = body1?body1->getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal:btVector3(0,0,0);
00470 btScalar sum = 0;
00471 sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
00472 sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
00473 solverConstraint.m_jacDiagABInv = btScalar(1.)/sum;
00474 }
00475
00476 {
00477
00478
00479 btScalar rel_vel;
00480 btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?solverBodyA.m_linearVelocity:btVector3(0,0,0))
00481 + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0));
00482 btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?solverBodyB.m_linearVelocity:btVector3(0,0,0))
00483 + solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0));
00484
00485 rel_vel = vel1Dotn+vel2Dotn;
00486
00487
00488
00489 btSimdScalar velocityError = desiredVelocity - rel_vel;
00490 btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
00491 solverConstraint.m_rhs = velocityImpulse;
00492 solverConstraint.m_cfm = cfmSlip;
00493 solverConstraint.m_lowerLimit = 0;
00494 solverConstraint.m_upperLimit = 1e10f;
00495
00496 }
00497 }
00498
00499
00500
00501
00502
00503
00504
00505
00506 btSolverConstraint& btSequentialImpulseConstraintSolver::addRollingFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
00507 {
00508 btSolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing();
00509 solverConstraint.m_frictionIndex = frictionIndex;
00510 setupRollingFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
00511 colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
00512 return solverConstraint;
00513 }
00514
00515
00516 int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body)
00517 {
00518
00519 int solverBodyIdA = -1;
00520
00521 if (body.getCompanionId() >= 0)
00522 {
00523
00524 solverBodyIdA = body.getCompanionId();
00525 btAssert(solverBodyIdA < m_tmpSolverBodyPool.size());
00526 } else
00527 {
00528 btRigidBody* rb = btRigidBody::upcast(&body);
00529
00530 if (rb && (rb->getInvMass() || rb->isKinematicObject()))
00531 {
00532 solverBodyIdA = m_tmpSolverBodyPool.size();
00533 btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
00534 initSolverBody(&solverBody,&body);
00535 body.setCompanionId(solverBodyIdA);
00536 } else
00537 {
00538 return 0;
00539 }
00540 }
00541
00542 return solverBodyIdA;
00543
00544 }
00545 #include <stdio.h>
00546
00547
00548 void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint,
00549 int solverBodyIdA, int solverBodyIdB,
00550 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
00551 btVector3& vel, btScalar& rel_vel, btScalar& relaxation,
00552 btVector3& rel_pos1, btVector3& rel_pos2)
00553 {
00554
00555 const btVector3& pos1 = cp.getPositionWorldOnA();
00556 const btVector3& pos2 = cp.getPositionWorldOnB();
00557
00558 btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
00559 btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
00560
00561 btRigidBody* rb0 = bodyA->m_originalBody;
00562 btRigidBody* rb1 = bodyB->m_originalBody;
00563
00564
00565
00566 rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
00567 rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
00568
00569 relaxation = 1.f;
00570
00571 btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
00572 solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
00573 btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
00574 solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
00575
00576 {
00577 #ifdef COMPUTE_IMPULSE_DENOM
00578 btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
00579 btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
00580 #else
00581 btVector3 vec;
00582 btScalar denom0 = 0.f;
00583 btScalar denom1 = 0.f;
00584 if (rb0)
00585 {
00586 vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
00587 denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
00588 }
00589 if (rb1)
00590 {
00591 vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
00592 denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
00593 }
00594 #endif //COMPUTE_IMPULSE_DENOM
00595
00596 btScalar denom = relaxation/(denom0+denom1);
00597 solverConstraint.m_jacDiagABInv = denom;
00598 }
00599
00600 solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
00601 solverConstraint.m_relpos1CrossNormal = torqueAxis0;
00602 solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
00603
00604 btScalar restitution = 0.f;
00605 btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
00606
00607 {
00608 btVector3 vel1,vel2;
00609
00610 vel1 = rb0? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
00611 vel2 = rb1? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
00612
00613
00614 vel = vel1 - vel2;
00615 rel_vel = cp.m_normalWorldOnB.dot(vel);
00616
00617
00618
00619 solverConstraint.m_friction = cp.m_combinedFriction;
00620
00621
00622 restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution);
00623 if (restitution <= btScalar(0.))
00624 {
00625 restitution = 0.f;
00626 };
00627 }
00628
00629
00631 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
00632 {
00633 solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
00634 if (rb0)
00635 bodyA->internalApplyImpulse(solverConstraint.m_contactNormal*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
00636 if (rb1)
00637 bodyB->internalApplyImpulse(solverConstraint.m_contactNormal*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
00638 } else
00639 {
00640 solverConstraint.m_appliedImpulse = 0.f;
00641 }
00642
00643 solverConstraint.m_appliedPushImpulse = 0.f;
00644
00645 {
00646 btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?bodyA->m_linearVelocity:btVector3(0,0,0))
00647 + solverConstraint.m_relpos1CrossNormal.dot(rb0?bodyA->m_angularVelocity:btVector3(0,0,0));
00648 btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1?bodyB->m_linearVelocity:btVector3(0,0,0))
00649 + solverConstraint.m_relpos2CrossNormal.dot(rb1?bodyB->m_angularVelocity:btVector3(0,0,0));
00650 btScalar rel_vel = vel1Dotn+vel2Dotn;
00651
00652 btScalar positionalError = 0.f;
00653 btScalar velocityError = restitution - rel_vel;
00654
00655
00656 btScalar erp = infoGlobal.m_erp2;
00657 if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
00658 {
00659 erp = infoGlobal.m_erp;
00660 }
00661
00662 if (penetration>0)
00663 {
00664 positionalError = 0;
00665
00666 velocityError -= penetration / infoGlobal.m_timeStep;
00667 } else
00668 {
00669 positionalError = -penetration * erp/infoGlobal.m_timeStep;
00670 }
00671
00672 btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
00673 btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
00674
00675 if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
00676 {
00677
00678 solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
00679 solverConstraint.m_rhsPenetration = 0.f;
00680
00681 } else
00682 {
00683
00684 solverConstraint.m_rhs = velocityImpulse;
00685 solverConstraint.m_rhsPenetration = penetrationImpulse;
00686 }
00687 solverConstraint.m_cfm = 0.f;
00688 solverConstraint.m_lowerLimit = 0;
00689 solverConstraint.m_upperLimit = 1e10f;
00690 }
00691
00692
00693
00694
00695 }
00696
00697
00698
00699 void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint,
00700 int solverBodyIdA, int solverBodyIdB,
00701 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal)
00702 {
00703
00704 btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
00705 btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
00706
00707 btRigidBody* rb0 = bodyA->m_originalBody;
00708 btRigidBody* rb1 = bodyB->m_originalBody;
00709
00710 {
00711 btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
00712 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
00713 {
00714 frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
00715 if (rb0)
00716 bodyA->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
00717 if (rb1)
00718 bodyB->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse);
00719 } else
00720 {
00721 frictionConstraint1.m_appliedImpulse = 0.f;
00722 }
00723 }
00724
00725 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
00726 {
00727 btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
00728 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
00729 {
00730 frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
00731 if (rb0)
00732 bodyA->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
00733 if (rb1)
00734 bodyB->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse);
00735 } else
00736 {
00737 frictionConstraint2.m_appliedImpulse = 0.f;
00738 }
00739 }
00740 }
00741
00742
00743
00744
00745 void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
00746 {
00747 btCollisionObject* colObj0=0,*colObj1=0;
00748
00749 colObj0 = (btCollisionObject*)manifold->getBody0();
00750 colObj1 = (btCollisionObject*)manifold->getBody1();
00751
00752 int solverBodyIdA = getOrInitSolverBody(*colObj0);
00753 int solverBodyIdB = getOrInitSolverBody(*colObj1);
00754
00755
00756
00757
00758 btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA];
00759 btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB];
00760
00761
00762
00764 if (!solverBodyA || (!solverBodyA->m_originalBody && (!solverBodyB || !solverBodyB->m_originalBody)))
00765 return;
00766
00767 int rollingFriction=1;
00768 for (int j=0;j<manifold->getNumContacts();j++)
00769 {
00770
00771 btManifoldPoint& cp = manifold->getContactPoint(j);
00772
00773 if (cp.getDistance() <= manifold->getContactProcessingThreshold())
00774 {
00775 btVector3 rel_pos1;
00776 btVector3 rel_pos2;
00777 btScalar relaxation;
00778 btScalar rel_vel;
00779 btVector3 vel;
00780
00781 int frictionIndex = m_tmpSolverContactConstraintPool.size();
00782 btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing();
00783
00784
00785 solverConstraint.m_solverBodyIdA = solverBodyIdA;
00786 solverConstraint.m_solverBodyIdB = solverBodyIdB;
00787
00788 solverConstraint.m_originalContactPoint = &cp;
00789
00790 setupContactConstraint(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2);
00791
00792
00793
00794
00796
00797 solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
00798
00799 btVector3 angVelA,angVelB;
00800 solverBodyA->getAngularVelocity(angVelA);
00801 solverBodyB->getAngularVelocity(angVelB);
00802 btVector3 relAngVel = angVelB-angVelA;
00803
00804 if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
00805 {
00806
00807 rollingFriction--;
00808 if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
00809 {
00810 relAngVel.normalize();
00811 applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
00812 applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
00813 if (relAngVel.length()>0.001)
00814 addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
00815
00816 } else
00817 {
00818 addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
00819 btVector3 axis0,axis1;
00820 btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
00821 applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
00822 applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
00823 applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
00824 applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
00825 if (axis0.length()>0.001)
00826 addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
00827 if (axis1.length()>0.001)
00828 addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
00829
00830 }
00831 }
00832
00837
00848 if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
00849 {
00850 cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
00851 btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
00852 if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
00853 {
00854 cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel);
00855 if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
00856 {
00857 cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
00858 cp.m_lateralFrictionDir2.normalize();
00859 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
00860 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
00861 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
00862
00863 }
00864
00865 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
00866 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
00867 addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
00868
00869 } else
00870 {
00871 btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
00872
00873 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
00874 {
00875 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
00876 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
00877 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
00878 }
00879
00880 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
00881 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
00882 addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
00883
00884 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
00885 {
00886 cp.m_lateralFrictionInitialized = true;
00887 }
00888 }
00889
00890 } else
00891 {
00892 addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1);
00893
00894 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
00895 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2);
00896
00897 setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
00898 }
00899
00900
00901
00902
00903 }
00904 }
00905 }
00906
00907 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
00908 {
00909 BT_PROFILE("solveGroupCacheFriendlySetup");
00910 (void)stackAlloc;
00911 (void)debugDrawer;
00912
00913 m_maxOverrideNumSolverIterations = 0;
00914
00915 #ifdef BT_DEBUG
00916
00917 for (int i=0;i<numConstraints;i++)
00918 {
00919 btTypedConstraint* constraint = constraints[i];
00920 if (constraint->isEnabled())
00921 {
00922 if (!constraint->getRigidBodyA().isStaticOrKinematicObject())
00923 {
00924 bool found=false;
00925 for (int b=0;b<numBodies;b++)
00926 {
00927
00928 if (&constraint->getRigidBodyA()==bodies[b])
00929 {
00930 found = true;
00931 break;
00932 }
00933 }
00934 btAssert(found);
00935 }
00936 if (!constraint->getRigidBodyB().isStaticOrKinematicObject())
00937 {
00938 bool found=false;
00939 for (int b=0;b<numBodies;b++)
00940 {
00941 if (&constraint->getRigidBodyB()==bodies[b])
00942 {
00943 found = true;
00944 break;
00945 }
00946 }
00947 btAssert(found);
00948 }
00949 }
00950 }
00951
00952 for (int i=0;i<numManifolds;i++)
00953 {
00954 if (!manifoldPtr[i]->getBody0()->isStaticOrKinematicObject())
00955 {
00956 bool found=false;
00957 for (int b=0;b<numBodies;b++)
00958 {
00959
00960 if (manifoldPtr[i]->getBody0()==bodies[b])
00961 {
00962 found = true;
00963 break;
00964 }
00965 }
00966 btAssert(found);
00967 }
00968 if (!manifoldPtr[i]->getBody1()->isStaticOrKinematicObject())
00969 {
00970 bool found=false;
00971 for (int b=0;b<numBodies;b++)
00972 {
00973 if (manifoldPtr[i]->getBody1()==bodies[b])
00974 {
00975 found = true;
00976 break;
00977 }
00978 }
00979 btAssert(found);
00980 }
00981 }
00982 #endif //BT_DEBUG
00983
00984
00985 for (int i = 0; i < numBodies; i++)
00986 {
00987 bodies[i]->setCompanionId(-1);
00988 }
00989
00990
00991 m_tmpSolverBodyPool.reserve(numBodies+1);
00992 m_tmpSolverBodyPool.resize(0);
00993
00994 btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
00995 initSolverBody(&fixedBody,0);
00996
00997
00998
00999 for (int i=0;i<numBodies;i++)
01000 {
01001 int bodyId = getOrInitSolverBody(*bodies[i]);
01002 btRigidBody* body = btRigidBody::upcast(bodies[i]);
01003 if (body && body->getInvMass())
01004 {
01005 btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
01006 btVector3 gyroForce (0,0,0);
01007 if (body->getFlags()&BT_ENABLE_GYROPSCOPIC_FORCE)
01008 {
01009 gyroForce = body->computeGyroscopicForce(infoGlobal.m_maxGyroscopicForce);
01010 }
01011 solverBody.m_linearVelocity += body->getTotalForce()*body->getInvMass()*infoGlobal.m_timeStep;
01012 solverBody.m_angularVelocity += (body->getTotalTorque()-gyroForce)*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep;
01013 }
01014 }
01015
01016 if (1)
01017 {
01018 int j;
01019 for (j=0;j<numConstraints;j++)
01020 {
01021 btTypedConstraint* constraint = constraints[j];
01022 constraint->buildJacobian();
01023 constraint->internalSetAppliedImpulse(0.0f);
01024 }
01025 }
01026
01027
01028
01029
01030 {
01031 {
01032
01033 int totalNumRows = 0;
01034 int i;
01035
01036 m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints);
01037
01038 for (i=0;i<numConstraints;i++)
01039 {
01040 btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
01041 btJointFeedback* fb = constraints[i]->getJointFeedback();
01042 if (fb)
01043 {
01044 fb->m_appliedForceBodyA.setZero();
01045 fb->m_appliedTorqueBodyA.setZero();
01046 fb->m_appliedForceBodyB.setZero();
01047 fb->m_appliedTorqueBodyB.setZero();
01048 }
01049
01050 if (constraints[i]->isEnabled())
01051 {
01052 }
01053 if (constraints[i]->isEnabled())
01054 {
01055 constraints[i]->getInfo1(&info1);
01056 } else
01057 {
01058 info1.m_numConstraintRows = 0;
01059 info1.nub = 0;
01060 }
01061 totalNumRows += info1.m_numConstraintRows;
01062 }
01063 m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows);
01064
01065
01067 int currentRow = 0;
01068
01069 for (i=0;i<numConstraints;i++)
01070 {
01071 const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
01072
01073 if (info1.m_numConstraintRows)
01074 {
01075 btAssert(currentRow<totalNumRows);
01076
01077 btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
01078 btTypedConstraint* constraint = constraints[i];
01079 btRigidBody& rbA = constraint->getRigidBodyA();
01080 btRigidBody& rbB = constraint->getRigidBodyB();
01081
01082 int solverBodyIdA = getOrInitSolverBody(rbA);
01083 int solverBodyIdB = getOrInitSolverBody(rbB);
01084
01085 btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
01086 btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
01087
01088
01089
01090
01091 int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations;
01092 if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations)
01093 m_maxOverrideNumSolverIterations = overrideNumSolverIterations;
01094
01095
01096 int j;
01097 for ( j=0;j<info1.m_numConstraintRows;j++)
01098 {
01099 memset(¤tConstraintRow[j],0,sizeof(btSolverConstraint));
01100 currentConstraintRow[j].m_lowerLimit = -SIMD_INFINITY;
01101 currentConstraintRow[j].m_upperLimit = SIMD_INFINITY;
01102 currentConstraintRow[j].m_appliedImpulse = 0.f;
01103 currentConstraintRow[j].m_appliedPushImpulse = 0.f;
01104 currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
01105 currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
01106 currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations;
01107 }
01108
01109 bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
01110 bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
01111 bodyAPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
01112 bodyAPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
01113 bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
01114 bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
01115 bodyBPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
01116 bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
01117
01118
01119 btTypedConstraint::btConstraintInfo2 info2;
01120 info2.fps = 1.f/infoGlobal.m_timeStep;
01121 info2.erp = infoGlobal.m_erp;
01122 info2.m_J1linearAxis = currentConstraintRow->m_contactNormal;
01123 info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
01124 info2.m_J2linearAxis = 0;
01125 info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
01126 info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);
01128 btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
01129 info2.m_constraintError = ¤tConstraintRow->m_rhs;
01130 currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
01131 info2.m_damping = infoGlobal.m_damping;
01132 info2.cfm = ¤tConstraintRow->m_cfm;
01133 info2.m_lowerLimit = ¤tConstraintRow->m_lowerLimit;
01134 info2.m_upperLimit = ¤tConstraintRow->m_upperLimit;
01135 info2.m_numIterations = infoGlobal.m_numIterations;
01136 constraints[i]->getInfo2(&info2);
01137
01139 for ( j=0;j<info1.m_numConstraintRows;j++)
01140 {
01141 btSolverConstraint& solverConstraint = currentConstraintRow[j];
01142
01143 if (solverConstraint.m_upperLimit>=constraints[i]->getBreakingImpulseThreshold())
01144 {
01145 solverConstraint.m_upperLimit = constraints[i]->getBreakingImpulseThreshold();
01146 }
01147
01148 if (solverConstraint.m_lowerLimit<=-constraints[i]->getBreakingImpulseThreshold())
01149 {
01150 solverConstraint.m_lowerLimit = -constraints[i]->getBreakingImpulseThreshold();
01151 }
01152
01153 solverConstraint.m_originalContactPoint = constraint;
01154
01155 {
01156 const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
01157 solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor();
01158 }
01159 {
01160 const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
01161 solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor();
01162 }
01163
01164 {
01165 btVector3 iMJlA = solverConstraint.m_contactNormal*rbA.getInvMass();
01166 btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;
01167 btVector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();
01168 btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;
01169
01170 btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal);
01171 sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
01172 sum += iMJlB.dot(solverConstraint.m_contactNormal);
01173 sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
01174 btScalar fsum = btFabs(sum);
01175 btAssert(fsum > SIMD_EPSILON);
01176 solverConstraint.m_jacDiagABInv = fsum>SIMD_EPSILON?btScalar(1.)/sum : 0.f;
01177 }
01178
01179
01182 {
01183 btScalar rel_vel;
01184 btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity());
01185 btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity());
01186
01187 rel_vel = vel1Dotn+vel2Dotn;
01188
01189 btScalar restitution = 0.f;
01190 btScalar positionalError = solverConstraint.m_rhs;
01191 btScalar velocityError = restitution - rel_vel * info2.m_damping;
01192 btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
01193 btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
01194 solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
01195 solverConstraint.m_appliedImpulse = 0.f;
01196
01197 }
01198 }
01199 }
01200 currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows;
01201 }
01202 }
01203
01204 {
01205 int i;
01206 btPersistentManifold* manifold = 0;
01207
01208
01209
01210 for (i=0;i<numManifolds;i++)
01211 {
01212 manifold = manifoldPtr[i];
01213 convertContact(manifold,infoGlobal);
01214 }
01215 }
01216 }
01217
01218
01219
01220
01221 int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
01222 int numConstraintPool = m_tmpSolverContactConstraintPool.size();
01223 int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
01224
01226 m_orderNonContactConstraintPool.resizeNoInitialize(numNonContactPool);
01227 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
01228 m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool*2);
01229 else
01230 m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool);
01231
01232 m_orderFrictionConstraintPool.resizeNoInitialize(numFrictionPool);
01233 {
01234 int i;
01235 for (i=0;i<numNonContactPool;i++)
01236 {
01237 m_orderNonContactConstraintPool[i] = i;
01238 }
01239 for (i=0;i<numConstraintPool;i++)
01240 {
01241 m_orderTmpConstraintPool[i] = i;
01242 }
01243 for (i=0;i<numFrictionPool;i++)
01244 {
01245 m_orderFrictionConstraintPool[i] = i;
01246 }
01247 }
01248
01249 return 0.f;
01250
01251 }
01252
01253
01254 btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** ,int ,btPersistentManifold** , int ,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* ,btStackAlloc* )
01255 {
01256
01257 int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
01258 int numConstraintPool = m_tmpSolverContactConstraintPool.size();
01259 int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
01260
01261 if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
01262 {
01263 if (1)
01264 {
01265
01266 for (int j=0; j<numNonContactPool; ++j) {
01267 int tmp = m_orderNonContactConstraintPool[j];
01268 int swapi = btRandInt2(j+1);
01269 m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi];
01270 m_orderNonContactConstraintPool[swapi] = tmp;
01271 }
01272
01273
01274 if (iteration< infoGlobal.m_numIterations)
01275 {
01276 for (int j=0; j<numConstraintPool; ++j) {
01277 int tmp = m_orderTmpConstraintPool[j];
01278 int swapi = btRandInt2(j+1);
01279 m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
01280 m_orderTmpConstraintPool[swapi] = tmp;
01281 }
01282
01283 for (int j=0; j<numFrictionPool; ++j) {
01284 int tmp = m_orderFrictionConstraintPool[j];
01285 int swapi = btRandInt2(j+1);
01286 m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
01287 m_orderFrictionConstraintPool[swapi] = tmp;
01288 }
01289 }
01290 }
01291 }
01292
01293 if (infoGlobal.m_solverMode & SOLVER_SIMD)
01294 {
01296 for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
01297 {
01298 btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
01299 if (iteration < constraint.m_overrideNumSolverIterations)
01300 resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
01301 }
01302
01303 if (iteration< infoGlobal.m_numIterations)
01304 {
01305 for (int j=0;j<numConstraints;j++)
01306 {
01307 if (constraints[j]->isEnabled())
01308 {
01309 int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
01310 int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
01311 btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
01312 btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
01313 constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
01314 }
01315 }
01316
01318 if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
01319 {
01320 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
01321 int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1;
01322
01323 for (int c=0;c<numPoolConstraints;c++)
01324 {
01325 btScalar totalImpulse =0;
01326
01327 {
01328 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
01329 resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
01330 totalImpulse = solveManifold.m_appliedImpulse;
01331 }
01332 bool applyFriction = true;
01333 if (applyFriction)
01334 {
01335 {
01336
01337 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier]];
01338
01339 if (totalImpulse>btScalar(0))
01340 {
01341 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
01342 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
01343
01344 resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
01345 }
01346 }
01347
01348 if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
01349 {
01350
01351 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]];
01352
01353 if (totalImpulse>btScalar(0))
01354 {
01355 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
01356 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
01357
01358 resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
01359 }
01360 }
01361 }
01362 }
01363
01364 }
01365 else
01366 {
01367
01368 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
01369 int j;
01370
01371 for (j=0;j<numPoolConstraints;j++)
01372 {
01373 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
01374 resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
01375
01376 }
01377
01378
01379
01381
01382 int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
01383 for (j=0;j<numFrictionPoolConstraints;j++)
01384 {
01385 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
01386 btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
01387
01388 if (totalImpulse>btScalar(0))
01389 {
01390 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
01391 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
01392
01393 resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
01394 }
01395 }
01396
01397
01398 int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
01399 for (j=0;j<numRollingFrictionPoolConstraints;j++)
01400 {
01401
01402 btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
01403 btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
01404 if (totalImpulse>btScalar(0))
01405 {
01406 btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
01407 if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
01408 rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
01409
01410 rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
01411 rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
01412
01413 resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
01414 }
01415 }
01416
01417
01418 }
01419 }
01420 } else
01421 {
01422
01424 for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
01425 {
01426 btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
01427 if (iteration < constraint.m_overrideNumSolverIterations)
01428 resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
01429 }
01430
01431 if (iteration< infoGlobal.m_numIterations)
01432 {
01433 for (int j=0;j<numConstraints;j++)
01434 {
01435 if (constraints[j]->isEnabled())
01436 {
01437 int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
01438 int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
01439 btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
01440 btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
01441 constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
01442 }
01443 }
01445 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
01446 for (int j=0;j<numPoolConstraints;j++)
01447 {
01448 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
01449 resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
01450 }
01452 int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
01453 for (int j=0;j<numFrictionPoolConstraints;j++)
01454 {
01455 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
01456 btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
01457
01458 if (totalImpulse>btScalar(0))
01459 {
01460 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
01461 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
01462
01463 resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
01464 }
01465 }
01466
01467 int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
01468 for (int j=0;j<numRollingFrictionPoolConstraints;j++)
01469 {
01470 btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
01471 btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
01472 if (totalImpulse>btScalar(0))
01473 {
01474 btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
01475 if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
01476 rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
01477
01478 rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
01479 rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
01480
01481 resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
01482 }
01483 }
01484 }
01485 }
01486 return 0.f;
01487 }
01488
01489
01490 void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
01491 {
01492 int iteration;
01493 if (infoGlobal.m_splitImpulse)
01494 {
01495 if (infoGlobal.m_solverMode & SOLVER_SIMD)
01496 {
01497 for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
01498 {
01499 {
01500 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
01501 int j;
01502 for (j=0;j<numPoolConstraints;j++)
01503 {
01504 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
01505
01506 resolveSplitPenetrationSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
01507 }
01508 }
01509 }
01510 }
01511 else
01512 {
01513 for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
01514 {
01515 {
01516 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
01517 int j;
01518 for (j=0;j<numPoolConstraints;j++)
01519 {
01520 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
01521
01522 resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
01523 }
01524 }
01525 }
01526 }
01527 }
01528 }
01529
01530 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
01531 {
01532 BT_PROFILE("solveGroupCacheFriendlyIterations");
01533
01534 {
01536 solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
01537
01538 int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
01539
01540 for ( int iteration = 0 ; iteration< maxIterations ; iteration++)
01541
01542 {
01543 solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
01544 }
01545
01546 }
01547 return 0.f;
01548 }
01549
01550 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
01551 {
01552 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
01553 int i,j;
01554
01555 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
01556 {
01557 for (j=0;j<numPoolConstraints;j++)
01558 {
01559 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
01560 btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
01561 btAssert(pt);
01562 pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
01563
01564
01565 pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
01566
01567 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
01568 {
01569 pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
01570 }
01571
01572 }
01573 }
01574
01575 numPoolConstraints = m_tmpSolverNonContactConstraintPool.size();
01576 for (j=0;j<numPoolConstraints;j++)
01577 {
01578 const btSolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j];
01579 btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint;
01580 btJointFeedback* fb = constr->getJointFeedback();
01581 if (fb)
01582 {
01583 fb->m_appliedForceBodyA += solverConstr.m_contactNormal*solverConstr.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep;
01584 fb->m_appliedForceBodyB += -solverConstr.m_contactNormal*solverConstr.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep;
01585 fb->m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep;
01586 fb->m_appliedTorqueBodyB += -solverConstr.m_relpos1CrossNormal* constr->getRigidBodyB().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep;
01587
01588 }
01589
01590 constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse);
01591 if (btFabs(solverConstr.m_appliedImpulse)>=constr->getBreakingImpulseThreshold())
01592 {
01593 constr->setEnabled(false);
01594 }
01595 }
01596
01597
01598
01599 for ( i=0;i<m_tmpSolverBodyPool.size();i++)
01600 {
01601 btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
01602 if (body)
01603 {
01604 if (infoGlobal.m_splitImpulse)
01605 m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp);
01606 else
01607 m_tmpSolverBodyPool[i].writebackVelocity();
01608
01609 m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity);
01610 m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity);
01611 if (infoGlobal.m_splitImpulse)
01612 m_tmpSolverBodyPool[i].m_originalBody->setWorldTransform(m_tmpSolverBodyPool[i].m_worldTransform);
01613
01614 m_tmpSolverBodyPool[i].m_originalBody->setCompanionId(-1);
01615 }
01616 }
01617
01618 m_tmpSolverContactConstraintPool.resizeNoInitialize(0);
01619 m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0);
01620 m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0);
01621 m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0);
01622
01623 m_tmpSolverBodyPool.resizeNoInitialize(0);
01624 return 0.f;
01625 }
01626
01627
01628
01630 btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* )
01631 {
01632
01633 BT_PROFILE("solveGroup");
01634
01635
01636 solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
01637
01638 solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
01639
01640 solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
01641
01642 return 0.f;
01643 }
01644
01645 void btSequentialImpulseConstraintSolver::reset()
01646 {
01647 m_btSeed2 = 0;
01648 }
01649
01650