btSequentialImpulseConstraintSolver.cpp

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00015 
00016 //#define COMPUTE_IMPULSE_DENOM 1
00017 //It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
00018 
00019 #include "btSequentialImpulseConstraintSolver.h"
00020 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
00021 
00022 #include "LinearMath/btIDebugDraw.h"
00023 //#include "btJacobianEntry.h"
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 //#include "btSolverBody.h"
00030 //#include "btSolverConstraint.h"
00031 #include "LinearMath/btAlignedObjectArray.h"
00032 #include <string.h> //for memset
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 // Project Gauss Seidel or the equivalent Sequential Impulse
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 // Project Gauss Seidel or the equivalent Sequential Impulse
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 //      const btScalar delta_rel_vel    =       deltaVel1Dotn-deltaVel2Dotn;
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 // Project Gauss Seidel or the equivalent Sequential Impulse
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 //See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
00252 int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
00253 {
00254         // seems good; xor-fold and modulus
00255         const unsigned long un = static_cast<unsigned long>(n);
00256         unsigned long r = btRand2();
00257 
00258         // note: probably more aggressive than it needs to be -- might be
00259         //       able to get away without one or two of the innermost branches.
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                 // transform to local coordinates
00335                 btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis();
00336                 const btVector3& friction_scaling = colObj->getAnisotropicFriction();
00337                 //apply anisotropic friction
00338                 loc_lateral *= friction_scaling;
00339                 // ... and transform it back to global coordinates
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 //              btScalar positionalError = 0.f;
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 //              btScalar positionalError = 0.f;
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                 //body has already been converted
00524                 solverBodyIdA = body.getCompanionId();
00525         btAssert(solverBodyIdA < m_tmpSolverBodyPool.size());
00526         } else
00527         {
00528                 btRigidBody* rb = btRigidBody::upcast(&body);
00529                 //convert both active and kinematic objects (for their velocity)
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;//assume first one is a fixed solver body
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 //                      btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); 
00565 //                      btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
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         //                      btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
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;// * damping;
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                                                 //combine position and velocity into rhs
00678                                                 solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
00679                                                 solverConstraint.m_rhsPenetration = 0.f;
00680 
00681                                         } else
00682                                         {
00683                                                 //split position and velocity into rhs and m_rhsPenetration
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 //      btRigidBody* bodyA = btRigidBody::upcast(colObj0);
00756 //      btRigidBody* bodyB = btRigidBody::upcast(colObj1);
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 //                      btRigidBody* rb0 = btRigidBody::upcast(colObj0);
00784 //                      btRigidBody* rb1 = btRigidBody::upcast(colObj1);
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 //                      const btVector3& pos1 = cp.getPositionWorldOnA();
00793 //                      const btVector3& pos2 = cp.getPositionWorldOnB();
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                                 //only a single rollingFriction per manifold
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          //make sure that dynamic bodies exist for all (enabled) constraints
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     //make sure that dynamic bodies exist for all contact manifolds
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         //convert all bodies
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         //btRigidBody* rb0=0,*rb1=0;
01028 
01029         //if (1)
01030         {
01031                 {
01032 
01033                         int totalNumRows = 0;
01034                         int i;
01035                         
01036                         m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints);
01037                         //calculate the total number of contraint rows
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(&currentConstraintRow[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);//check this
01128                             btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
01129                                         info2.m_constraintError = &currentConstraintRow->m_rhs;
01130                                         currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
01131                                         info2.m_damping = infoGlobal.m_damping;
01132                                         info2.cfm = &currentConstraintRow->m_cfm;
01133                                         info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
01134                                         info2.m_upperLimit = &currentConstraintRow->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();//sign of normal?
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;//already filled in by getConstraintInfo2
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 //                      btCollisionObject* colObj0=0,*colObj1=0;
01208 
01209 
01210                         for (i=0;i<numManifolds;i++)
01211                         {
01212                                 manifold = manifoldPtr[i];
01213                                 convertContact(manifold,infoGlobal);
01214                         }
01215                 }
01216         }
01217 
01218 //      btContactSolverInfo info = infoGlobal;
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** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
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)                  // uncomment this for a bit less random ((iteration & 7) == 0)
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                         //contact/friction constraints are not solved more than 
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//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
01366                         {
01367                                 //solve the friction constraints after all contact constraints, don't interleave them
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                 //non-SIMD version
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                 //for ( int iteration = maxIterations-1  ; iteration >= 0;iteration--)
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                 //      float f = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
01564                         //      printf("pt->m_appliedImpulseLateral1 = %f\n", f);
01565                         pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
01566                         //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
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                         //do a callback here?
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* /*dispatcher*/)
01631 {
01632 
01633         BT_PROFILE("solveGroup");
01634         //you need to provide at least some bodies
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