Blender V2.61 - r43446

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 #include "BulletDynamics/Dynamics/btRigidBody.h"
00022 #include "btContactConstraint.h"
00023 #include "btSolve2LinearConstraint.h"
00024 #include "btContactSolverInfo.h"
00025 #include "LinearMath/btIDebugDraw.h"
00026 #include "btJacobianEntry.h"
00027 #include "LinearMath/btMinMax.h"
00028 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
00029 #include <new>
00030 #include "LinearMath/btStackAlloc.h"
00031 #include "LinearMath/btQuickprof.h"
00032 #include "btSolverBody.h"
00033 #include "btSolverConstraint.h"
00034 #include "LinearMath/btAlignedObjectArray.h"
00035 #include <string.h> //for memset
00036 
00037 int     gNumSplitImpulseRecoveries = 0;
00038 
00039 btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
00040 :m_btSeed2(0)
00041 {
00042 
00043 }
00044 
00045 btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
00046 {
00047 }
00048 
00049 #ifdef USE_SIMD
00050 #include <emmintrin.h>
00051 #define btVecSplat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e))
00052 static inline __m128 btSimdDot3( __m128 vec0, __m128 vec1 )
00053 {
00054     __m128 result = _mm_mul_ps( vec0, vec1);
00055     return _mm_add_ps( btVecSplat( result, 0 ), _mm_add_ps( btVecSplat( result, 1 ), btVecSplat( result, 2 ) ) );
00056 }
00057 #endif//USE_SIMD
00058 
00059 // Project Gauss Seidel or the equivalent Sequential Impulse
00060 void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
00061 {
00062 #ifdef USE_SIMD
00063     __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
00064     __m128  lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
00065     __m128  upperLimit1 = _mm_set1_ps(c.m_upperLimit);
00066     __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)));
00067     __m128 deltaVel1Dotn    =   _mm_add_ps(btSimdDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
00068     __m128 deltaVel2Dotn    =   _mm_sub_ps(btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),btSimdDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128));
00069     deltaImpulse    =   _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
00070     deltaImpulse    =   _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
00071     btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
00072     btSimdScalar resultLowerLess,resultUpperLess;
00073     resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
00074     resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
00075     __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
00076     deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
00077     c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
00078     __m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp);
00079     deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) );
00080     c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) );
00081     __m128  linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128);
00082     __m128  linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128);
00083     __m128 impulseMagnitude = deltaImpulse;
00084     body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
00085     body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
00086     body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
00087     body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
00088 #else
00089     resolveSingleConstraintRowGeneric(body1,body2,c);
00090 #endif
00091 }
00092 
00093 // Project Gauss Seidel or the equivalent Sequential Impulse
00094  void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
00095 {
00096     btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
00097     const btScalar deltaVel1Dotn    =   c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity())   + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
00098     const btScalar deltaVel2Dotn    =   -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
00099 
00100 //  const btScalar delta_rel_vel    =   deltaVel1Dotn-deltaVel2Dotn;
00101     deltaImpulse    -=  deltaVel1Dotn*c.m_jacDiagABInv;
00102     deltaImpulse    -=  deltaVel2Dotn*c.m_jacDiagABInv;
00103 
00104     const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
00105     if (sum < c.m_lowerLimit)
00106     {
00107         deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
00108         c.m_appliedImpulse = c.m_lowerLimit;
00109     }
00110     else if (sum > c.m_upperLimit) 
00111     {
00112         deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
00113         c.m_appliedImpulse = c.m_upperLimit;
00114     }
00115     else
00116     {
00117         c.m_appliedImpulse = sum;
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(btRigidBody& body1,btRigidBody& 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(btRigidBody& body1,btRigidBody& 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         btRigidBody& body1,
00179         btRigidBody& 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(btRigidBody& body1,btRigidBody& 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_appliedImpulse = _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 #if 0
00281 void    btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
00282 {
00283     btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0;
00284 
00285     solverBody->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
00286     solverBody->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
00287     solverBody->internalGetPushVelocity().setValue(0.f,0.f,0.f);
00288     solverBody->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
00289 
00290     if (rb)
00291     {
00292         solverBody->internalGetInvMass() = btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor();
00293         solverBody->m_originalBody = rb;
00294         solverBody->m_angularFactor = rb->getAngularFactor();
00295     } else
00296     {
00297         solverBody->internalGetInvMass().setValue(0,0,0);
00298         solverBody->m_originalBody = 0;
00299         solverBody->m_angularFactor.setValue(1,1,1);
00300     }
00301 }
00302 #endif
00303 
00304 
00305 
00306 
00307 
00308 btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution)
00309 {
00310     btScalar rest = restitution * -rel_vel;
00311     return rest;
00312 }
00313 
00314 
00315 
00316 void    applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection);
00317 void    applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection)
00318 {
00319     if (colObj && colObj->hasAnisotropicFriction())
00320     {
00321         // transform to local coordinates
00322         btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis();
00323         const btVector3& friction_scaling = colObj->getAnisotropicFriction();
00324         //apply anisotropic friction
00325         loc_lateral *= friction_scaling;
00326         // ... and transform it back to global coordinates
00327         frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral;
00328     }
00329 }
00330 
00331 
00332 void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
00333 {
00334 
00335 
00336     btRigidBody* body0=btRigidBody::upcast(colObj0);
00337     btRigidBody* body1=btRigidBody::upcast(colObj1);
00338 
00339     solverConstraint.m_contactNormal = normalAxis;
00340 
00341     solverConstraint.m_solverBodyA = body0 ? body0 : &getFixedBody();
00342     solverConstraint.m_solverBodyB = body1 ? body1 : &getFixedBody();
00343 
00344     solverConstraint.m_friction = cp.m_combinedFriction;
00345     solverConstraint.m_originalContactPoint = 0;
00346 
00347     solverConstraint.m_appliedImpulse = 0.f;
00348     solverConstraint.m_appliedPushImpulse = 0.f;
00349 
00350     {
00351         btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
00352         solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
00353         solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0);
00354     }
00355     {
00356         btVector3 ftorqueAxis1 = rel_pos2.cross(-solverConstraint.m_contactNormal);
00357         solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
00358         solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0);
00359     }
00360 
00361 #ifdef COMPUTE_IMPULSE_DENOM
00362     btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
00363     btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
00364 #else
00365     btVector3 vec;
00366     btScalar denom0 = 0.f;
00367     btScalar denom1 = 0.f;
00368     if (body0)
00369     {
00370         vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
00371         denom0 = body0->getInvMass() + normalAxis.dot(vec);
00372     }
00373     if (body1)
00374     {
00375         vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
00376         denom1 = body1->getInvMass() + normalAxis.dot(vec);
00377     }
00378 
00379 
00380 #endif //COMPUTE_IMPULSE_DENOM
00381     btScalar denom = relaxation/(denom0+denom1);
00382     solverConstraint.m_jacDiagABInv = denom;
00383 
00384 #ifdef _USE_JACOBIAN
00385     solverConstraint.m_jac =  btJacobianEntry (
00386         rel_pos1,rel_pos2,solverConstraint.m_contactNormal,
00387         body0->getInvInertiaDiagLocal(),
00388         body0->getInvMass(),
00389         body1->getInvInertiaDiagLocal(),
00390         body1->getInvMass());
00391 #endif //_USE_JACOBIAN
00392 
00393 
00394     {
00395         btScalar rel_vel;
00396         btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?body0->getLinearVelocity():btVector3(0,0,0)) 
00397             + solverConstraint.m_relpos1CrossNormal.dot(body0?body0->getAngularVelocity():btVector3(0,0,0));
00398         btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?body1->getLinearVelocity():btVector3(0,0,0)) 
00399             + solverConstraint.m_relpos2CrossNormal.dot(body1?body1->getAngularVelocity():btVector3(0,0,0));
00400 
00401         rel_vel = vel1Dotn+vel2Dotn;
00402 
00403 //      btScalar positionalError = 0.f;
00404 
00405         btSimdScalar velocityError =  desiredVelocity - rel_vel;
00406         btSimdScalar    velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
00407         solverConstraint.m_rhs = velocityImpulse;
00408         solverConstraint.m_cfm = cfmSlip;
00409         solverConstraint.m_lowerLimit = 0;
00410         solverConstraint.m_upperLimit = 1e10f;
00411     }
00412 }
00413 
00414 
00415 
00416 btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
00417 {
00418     btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
00419     solverConstraint.m_frictionIndex = frictionIndex;
00420     setupFrictionConstraint(solverConstraint, normalAxis, solverBodyA, solverBodyB, cp, rel_pos1, rel_pos2, 
00421                             colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
00422     return solverConstraint;
00423 }
00424 
00425 int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body)
00426 {
00427 #if 0
00428     int solverBodyIdA = -1;
00429 
00430     if (body.getCompanionId() >= 0)
00431     {
00432         //body has already been converted
00433         solverBodyIdA = body.getCompanionId();
00434     } else
00435     {
00436         btRigidBody* rb = btRigidBody::upcast(&body);
00437         if (rb && rb->getInvMass())
00438         {
00439             solverBodyIdA = m_tmpSolverBodyPool.size();
00440             btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
00441             initSolverBody(&solverBody,&body);
00442             body.setCompanionId(solverBodyIdA);
00443         } else
00444         {
00445             return 0;//assume first one is a fixed solver body
00446         }
00447     }
00448     return solverBodyIdA;
00449 #endif
00450     return 0;
00451 }
00452 #include <stdio.h>
00453 
00454 
00455 void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint, 
00456                                                                  btCollisionObject* colObj0, btCollisionObject* colObj1,
00457                                                                  btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
00458                                                                  btVector3& vel, btScalar& rel_vel, btScalar& relaxation,
00459                                                                  btVector3& rel_pos1, btVector3& rel_pos2)
00460 {
00461             btRigidBody* rb0 = btRigidBody::upcast(colObj0);
00462             btRigidBody* rb1 = btRigidBody::upcast(colObj1);
00463 
00464             const btVector3& pos1 = cp.getPositionWorldOnA();
00465             const btVector3& pos2 = cp.getPositionWorldOnB();
00466 
00467 //          btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); 
00468 //          btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
00469             rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); 
00470             rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
00471 
00472             relaxation = 1.f;
00473 
00474             btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
00475             solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
00476             btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);        
00477             solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
00478 
00479                 {
00480 #ifdef COMPUTE_IMPULSE_DENOM
00481                     btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
00482                     btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
00483 #else                           
00484                     btVector3 vec;
00485                     btScalar denom0 = 0.f;
00486                     btScalar denom1 = 0.f;
00487                     if (rb0)
00488                     {
00489                         vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
00490                         denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
00491                     }
00492                     if (rb1)
00493                     {
00494                         vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
00495                         denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
00496                     }
00497 #endif //COMPUTE_IMPULSE_DENOM      
00498 
00499                     btScalar denom = relaxation/(denom0+denom1);
00500                     solverConstraint.m_jacDiagABInv = denom;
00501                 }
00502 
00503                 solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
00504                 solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB);
00505                 solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(-cp.m_normalWorldOnB);
00506 
00507 
00508 
00509 
00510             btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
00511             btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
00512             vel  = vel1 - vel2;
00513             rel_vel = cp.m_normalWorldOnB.dot(vel);
00514 
00515                 btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
00516 
00517 
00518                 solverConstraint.m_friction = cp.m_combinedFriction;
00519 
00520                 btScalar restitution = 0.f;
00521                 
00522                 if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold)
00523                 {
00524                     restitution = 0.f;
00525                 } else
00526                 {
00527                     restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
00528                     if (restitution <= btScalar(0.))
00529                     {
00530                         restitution = 0.f;
00531                     };
00532                 }
00533 
00534 
00536                 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
00537                 {
00538                     solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
00539                     if (rb0)
00540                         rb0->internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
00541                     if (rb1)
00542                         rb1->internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
00543                 } else
00544                 {
00545                     solverConstraint.m_appliedImpulse = 0.f;
00546                 }
00547 
00548                 solverConstraint.m_appliedPushImpulse = 0.f;
00549 
00550                 {
00551                     btScalar rel_vel;
00552                     btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?rb0->getLinearVelocity():btVector3(0,0,0)) 
00553                         + solverConstraint.m_relpos1CrossNormal.dot(rb0?rb0->getAngularVelocity():btVector3(0,0,0));
00554                     btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1?rb1->getLinearVelocity():btVector3(0,0,0)) 
00555                         + solverConstraint.m_relpos2CrossNormal.dot(rb1?rb1->getAngularVelocity():btVector3(0,0,0));
00556 
00557                     rel_vel = vel1Dotn+vel2Dotn;
00558 
00559                     btScalar positionalError = 0.f;
00560                     btScalar    velocityError = restitution - rel_vel;// * damping;
00561 
00562                     if (penetration>0)
00563                     {
00564                         positionalError = 0;
00565                         velocityError -= penetration / infoGlobal.m_timeStep;
00566                     } else
00567                     {
00568                         positionalError = -penetration * infoGlobal.m_erp/infoGlobal.m_timeStep;
00569                     }
00570 
00571                     btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
00572                     btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
00573                     if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
00574                     {
00575                         //combine position and velocity into rhs
00576                         solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
00577                         solverConstraint.m_rhsPenetration = 0.f;
00578                     } else
00579                     {
00580                         //split position and velocity into rhs and m_rhsPenetration
00581                         solverConstraint.m_rhs = velocityImpulse;
00582                         solverConstraint.m_rhsPenetration = penetrationImpulse;
00583                     }
00584                     solverConstraint.m_cfm = 0.f;
00585                     solverConstraint.m_lowerLimit = 0;
00586                     solverConstraint.m_upperLimit = 1e10f;
00587                 }
00588 
00589 
00590 
00591 
00592 }
00593 
00594 
00595 
00596 void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, 
00597                                                                         btRigidBody* rb0, btRigidBody* rb1, 
00598                                                                  btManifoldPoint& cp, const btContactSolverInfo& infoGlobal)
00599 {
00600                     if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
00601                     {
00602                         {
00603                             btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
00604                             if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
00605                             {
00606                                 frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
00607                                 if (rb0)
00608                                     rb0->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
00609                                 if (rb1)
00610                                     rb1->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse);
00611                             } else
00612                             {
00613                                 frictionConstraint1.m_appliedImpulse = 0.f;
00614                             }
00615                         }
00616 
00617                         if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
00618                         {
00619                             btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
00620                             if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
00621                             {
00622                                 frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
00623                                 if (rb0)
00624                                     rb0->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
00625                                 if (rb1)
00626                                     rb1->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse);
00627                             } else
00628                             {
00629                                 frictionConstraint2.m_appliedImpulse = 0.f;
00630                             }
00631                         }
00632                     } else
00633                     {
00634                         btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
00635                         frictionConstraint1.m_appliedImpulse = 0.f;
00636                         if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
00637                         {
00638                             btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
00639                             frictionConstraint2.m_appliedImpulse = 0.f;
00640                         }
00641                     }
00642 }
00643 
00644 
00645 
00646 
00647 void    btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
00648 {
00649     btCollisionObject* colObj0=0,*colObj1=0;
00650 
00651     colObj0 = (btCollisionObject*)manifold->getBody0();
00652     colObj1 = (btCollisionObject*)manifold->getBody1();
00653 
00654 
00655     btRigidBody* solverBodyA = btRigidBody::upcast(colObj0);
00656     btRigidBody* solverBodyB = btRigidBody::upcast(colObj1);
00657 
00659     if ((!solverBodyA || !solverBodyA->getInvMass()) && (!solverBodyB || !solverBodyB->getInvMass()))
00660         return;
00661 
00662     for (int j=0;j<manifold->getNumContacts();j++)
00663     {
00664 
00665         btManifoldPoint& cp = manifold->getContactPoint(j);
00666 
00667         if (cp.getDistance() <= manifold->getContactProcessingThreshold())
00668         {
00669             btVector3 rel_pos1;
00670             btVector3 rel_pos2;
00671             btScalar relaxation;
00672             btScalar rel_vel;
00673             btVector3 vel;
00674 
00675             int frictionIndex = m_tmpSolverContactConstraintPool.size();
00676             btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing();
00677             btRigidBody* rb0 = btRigidBody::upcast(colObj0);
00678             btRigidBody* rb1 = btRigidBody::upcast(colObj1);
00679             solverConstraint.m_solverBodyA = rb0? rb0 : &getFixedBody();
00680             solverConstraint.m_solverBodyB = rb1? rb1 : &getFixedBody();
00681             solverConstraint.m_originalContactPoint = &cp;
00682 
00683             setupContactConstraint(solverConstraint, colObj0, colObj1, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2);
00684 
00685 //          const btVector3& pos1 = cp.getPositionWorldOnA();
00686 //          const btVector3& pos2 = cp.getPositionWorldOnB();
00687 
00689 
00690             solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
00691 
00692             if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
00693             {
00694                 cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
00695                 btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
00696                 if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
00697                 {
00698                     cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
00699                     if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
00700                     {
00701                         cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
00702                         cp.m_lateralFrictionDir2.normalize();//??
00703                         applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
00704                         applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
00705                         addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
00706                     }
00707 
00708                     applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
00709                     applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
00710                     addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
00711                     cp.m_lateralFrictionInitialized = true;
00712                 } else
00713                 {
00714                     //re-calculate friction direction every frame, todo: check if this is really needed
00715                     btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
00716                     if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
00717                     {
00718                         applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
00719                         applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
00720                         addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
00721                     }
00722 
00723                     applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
00724                     applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
00725                     addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
00726 
00727                     cp.m_lateralFrictionInitialized = true;
00728                 }
00729 
00730             } else
00731             {
00732                 addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1);
00733                 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
00734                     addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2);
00735             }
00736             
00737             setFrictionConstraintImpulse( solverConstraint, rb0, rb1, cp, infoGlobal);
00738 
00739         }
00740     }
00741 }
00742 
00743 
00744 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
00745 {
00746     BT_PROFILE("solveGroupCacheFriendlySetup");
00747     (void)stackAlloc;
00748     (void)debugDrawer;
00749 
00750 
00751     if (!(numConstraints + numManifolds))
00752     {
00753         //      printf("empty\n");
00754         return 0.f;
00755     }
00756 
00757     if (infoGlobal.m_splitImpulse)
00758     {
00759         for (int i = 0; i < numBodies; i++)
00760         {
00761             btRigidBody* body = btRigidBody::upcast(bodies[i]);
00762             if (body)
00763             {   
00764                 body->internalGetDeltaLinearVelocity().setZero();
00765                 body->internalGetDeltaAngularVelocity().setZero();
00766                 body->internalGetPushVelocity().setZero();
00767                 body->internalGetTurnVelocity().setZero();
00768             }
00769         }
00770     }
00771     else
00772     {
00773         for (int i = 0; i < numBodies; i++)
00774         {
00775             btRigidBody* body = btRigidBody::upcast(bodies[i]);
00776             if (body)
00777             {   
00778                 body->internalGetDeltaLinearVelocity().setZero();
00779                 body->internalGetDeltaAngularVelocity().setZero();
00780             }
00781         }
00782     }
00783 
00784     if (1)
00785     {
00786         int j;
00787         for (j=0;j<numConstraints;j++)
00788         {
00789             btTypedConstraint* constraint = constraints[j];
00790             constraint->buildJacobian();
00791         }
00792     }
00793     //btRigidBody* rb0=0,*rb1=0;
00794 
00795     //if (1)
00796     {
00797         {
00798 
00799             int totalNumRows = 0;
00800             int i;
00801             
00802             m_tmpConstraintSizesPool.resize(numConstraints);
00803             //calculate the total number of contraint rows
00804             for (i=0;i<numConstraints;i++)
00805             {
00806                 btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
00807                 constraints[i]->getInfo1(&info1);
00808                 totalNumRows += info1.m_numConstraintRows;
00809             }
00810             m_tmpSolverNonContactConstraintPool.resize(totalNumRows);
00811 
00812             
00814             int currentRow = 0;
00815 
00816             for (i=0;i<numConstraints;i++)
00817             {
00818                 const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
00819                 
00820                 if (info1.m_numConstraintRows)
00821                 {
00822                     btAssert(currentRow<totalNumRows);
00823 
00824                     btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
00825                     btTypedConstraint* constraint = constraints[i];
00826 
00827 
00828 
00829                     btRigidBody& rbA = constraint->getRigidBodyA();
00830                     btRigidBody& rbB = constraint->getRigidBodyB();
00831 
00832                     
00833                     int j;
00834                     for ( j=0;j<info1.m_numConstraintRows;j++)
00835                     {
00836                         memset(&currentConstraintRow[j],0,sizeof(btSolverConstraint));
00837                         currentConstraintRow[j].m_lowerLimit = -FLT_MAX;
00838                         currentConstraintRow[j].m_upperLimit = FLT_MAX;
00839                         currentConstraintRow[j].m_appliedImpulse = 0.f;
00840                         currentConstraintRow[j].m_appliedPushImpulse = 0.f;
00841                         currentConstraintRow[j].m_solverBodyA = &rbA;
00842                         currentConstraintRow[j].m_solverBodyB = &rbB;
00843                     }
00844 
00845                     rbA.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
00846                     rbA.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
00847                     rbB.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
00848                     rbB.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
00849 
00850 
00851 
00852                     btTypedConstraint::btConstraintInfo2 info2;
00853                     info2.fps = 1.f/infoGlobal.m_timeStep;
00854                     info2.erp = infoGlobal.m_erp;
00855                     info2.m_J1linearAxis = currentConstraintRow->m_contactNormal;
00856                     info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
00857                     info2.m_J2linearAxis = 0;
00858                     info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
00859                     info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this
00861                     btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
00862                     info2.m_constraintError = &currentConstraintRow->m_rhs;
00863                     currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
00864                     info2.m_damping = infoGlobal.m_damping;
00865                     info2.cfm = &currentConstraintRow->m_cfm;
00866                     info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
00867                     info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
00868                     info2.m_numIterations = infoGlobal.m_numIterations;
00869                     constraints[i]->getInfo2(&info2);
00870 
00872                     for ( j=0;j<info1.m_numConstraintRows;j++)
00873                     {
00874                         btSolverConstraint& solverConstraint = currentConstraintRow[j];
00875                         solverConstraint.m_originalContactPoint = constraint;
00876 
00877                         {
00878                             const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
00879                             solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor();
00880                         }
00881                         {
00882                             const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
00883                             solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor();
00884                         }
00885 
00886                         {
00887                             btVector3 iMJlA = solverConstraint.m_contactNormal*rbA.getInvMass();
00888                             btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;
00889                             btVector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal?
00890                             btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;
00891 
00892                             btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal);
00893                             sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
00894                             sum += iMJlB.dot(solverConstraint.m_contactNormal);
00895                             sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
00896 
00897                             solverConstraint.m_jacDiagABInv = btScalar(1.)/sum;
00898                         }
00899 
00900 
00903                         {
00904                             btScalar rel_vel;
00905                             btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity());
00906                             btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity());
00907 
00908                             rel_vel = vel1Dotn+vel2Dotn;
00909 
00910                             btScalar restitution = 0.f;
00911                             btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
00912                             btScalar    velocityError = restitution - rel_vel * info2.m_damping;
00913                             btScalar    penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
00914                             btScalar    velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
00915                             solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
00916                             solverConstraint.m_appliedImpulse = 0.f;
00917 
00918                         }
00919                     }
00920                 }
00921                 currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows;
00922             }
00923         }
00924 
00925         {
00926             int i;
00927             btPersistentManifold* manifold = 0;
00928 //          btCollisionObject* colObj0=0,*colObj1=0;
00929 
00930 
00931             for (i=0;i<numManifolds;i++)
00932             {
00933                 manifold = manifoldPtr[i];
00934                 convertContact(manifold,infoGlobal);
00935             }
00936         }
00937     }
00938 
00939     btContactSolverInfo info = infoGlobal;
00940 
00941 
00942 
00943     int numConstraintPool = m_tmpSolverContactConstraintPool.size();
00944     int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
00945 
00947     m_orderTmpConstraintPool.resize(numConstraintPool);
00948     m_orderFrictionConstraintPool.resize(numFrictionPool);
00949     {
00950         int i;
00951         for (i=0;i<numConstraintPool;i++)
00952         {
00953             m_orderTmpConstraintPool[i] = i;
00954         }
00955         for (i=0;i<numFrictionPool;i++)
00956         {
00957             m_orderFrictionConstraintPool[i] = i;
00958         }
00959     }
00960 
00961     return 0.f;
00962 
00963 }
00964 
00965 btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
00966 {
00967 
00968     int numConstraintPool = m_tmpSolverContactConstraintPool.size();
00969     int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
00970 
00971     int j;
00972 
00973     if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
00974     {
00975         if ((iteration & 7) == 0) {
00976             for (j=0; j<numConstraintPool; ++j) {
00977                 int tmp = m_orderTmpConstraintPool[j];
00978                 int swapi = btRandInt2(j+1);
00979                 m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
00980                 m_orderTmpConstraintPool[swapi] = tmp;
00981             }
00982 
00983             for (j=0; j<numFrictionPool; ++j) {
00984                 int tmp = m_orderFrictionConstraintPool[j];
00985                 int swapi = btRandInt2(j+1);
00986                 m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
00987                 m_orderFrictionConstraintPool[swapi] = tmp;
00988             }
00989         }
00990     }
00991 
00992     if (infoGlobal.m_solverMode & SOLVER_SIMD)
00993     {
00995         for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
00996         {
00997             btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
00998             resolveSingleConstraintRowGenericSIMD(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint);
00999         }
01000 
01001         for (j=0;j<numConstraints;j++)
01002         {
01003             constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
01004         }
01005 
01007         int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
01008         for (j=0;j<numPoolConstraints;j++)
01009         {
01010             const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
01011             resolveSingleConstraintRowLowerLimitSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
01012 
01013         }
01015         int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
01016         for (j=0;j<numFrictionPoolConstraints;j++)
01017         {
01018             btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
01019             btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
01020 
01021             if (totalImpulse>btScalar(0))
01022             {
01023                 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
01024                 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
01025 
01026                 resolveSingleConstraintRowGenericSIMD(*solveManifold.m_solverBodyA, *solveManifold.m_solverBodyB,solveManifold);
01027             }
01028         }
01029     } else
01030     {
01031 
01033         for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
01034         {
01035             btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
01036             resolveSingleConstraintRowGeneric(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint);
01037         }
01038 
01039         for (j=0;j<numConstraints;j++)
01040         {
01041             constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
01042         }
01044         int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
01045         for (j=0;j<numPoolConstraints;j++)
01046         {
01047             const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
01048             resolveSingleConstraintRowLowerLimit(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
01049         }
01051         int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
01052         for (j=0;j<numFrictionPoolConstraints;j++)
01053         {
01054             btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
01055             btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
01056 
01057             if (totalImpulse>btScalar(0))
01058             {
01059                 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
01060                 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
01061 
01062                 resolveSingleConstraintRowGeneric(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
01063             }
01064         }
01065     }
01066     return 0.f;
01067 }
01068 
01069 
01070 void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
01071 {
01072     int iteration;
01073     if (infoGlobal.m_splitImpulse)
01074     {
01075         if (infoGlobal.m_solverMode & SOLVER_SIMD)
01076         {
01077             for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
01078             {
01079                 {
01080                     int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
01081                     int j;
01082                     for (j=0;j<numPoolConstraints;j++)
01083                     {
01084                         const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
01085 
01086                         resolveSplitPenetrationSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
01087                     }
01088                 }
01089             }
01090         }
01091         else
01092         {
01093             for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
01094             {
01095                 {
01096                     int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
01097                     int j;
01098                     for (j=0;j<numPoolConstraints;j++)
01099                     {
01100                         const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
01101 
01102                         resolveSplitPenetrationImpulseCacheFriendly(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
01103                     }
01104                 }
01105             }
01106         }
01107     }
01108 }
01109 
01110 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
01111 {
01112     BT_PROFILE("solveGroupCacheFriendlyIterations");
01113 
01114     
01115     //should traverse the contacts random order...
01116     int iteration;
01117     {
01118         for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
01119         {           
01120             solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
01121         }
01122         
01123         solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
01124     }
01125     return 0.f;
01126 }
01127 
01128 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies ,int numBodies,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** /*constraints*/,int /* numConstraints*/,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
01129 {
01130     int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
01131     int i,j;
01132 
01133     for (j=0;j<numPoolConstraints;j++)
01134     {
01135 
01136         const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
01137         btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
01138         btAssert(pt);
01139         pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
01140         if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
01141         {
01142             pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
01143             pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
01144         }
01145 
01146         //do a callback here?
01147     }
01148 
01149     numPoolConstraints = m_tmpSolverNonContactConstraintPool.size();
01150     for (j=0;j<numPoolConstraints;j++)
01151     {
01152         const btSolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j];
01153         btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint;
01154         btScalar sum = constr->internalGetAppliedImpulse();
01155         sum += solverConstr.m_appliedImpulse;
01156         constr->internalSetAppliedImpulse(sum);
01157     }
01158 
01159 
01160     if (infoGlobal.m_splitImpulse)
01161     {       
01162         for ( i=0;i<numBodies;i++)
01163         {
01164             btRigidBody* body = btRigidBody::upcast(bodies[i]);
01165             if (body)
01166                 body->internalWritebackVelocity(infoGlobal.m_timeStep);
01167         }
01168     } else
01169     {
01170         for ( i=0;i<numBodies;i++)
01171         {
01172             btRigidBody* body = btRigidBody::upcast(bodies[i]);
01173             if (body)
01174                 body->internalWritebackVelocity();
01175         }
01176     }
01177 
01178 
01179     m_tmpSolverContactConstraintPool.resize(0);
01180     m_tmpSolverNonContactConstraintPool.resize(0);
01181     m_tmpSolverContactFrictionConstraintPool.resize(0);
01182 
01183     return 0.f;
01184 }
01185 
01186 
01187 
01189 btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/)
01190 {
01191 
01192     BT_PROFILE("solveGroup");
01193     //you need to provide at least some bodies
01194     btAssert(bodies);
01195     btAssert(numBodies);
01196 
01197     solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
01198 
01199     solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
01200 
01201     solveGroupCacheFriendlyFinish(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
01202     
01203     return 0.f;
01204 }
01205 
01206 void    btSequentialImpulseConstraintSolver::reset()
01207 {
01208     m_btSeed2 = 0;
01209 }
01210 
01211 btRigidBody& btSequentialImpulseConstraintSolver::getFixedBody()
01212 {
01213     static btRigidBody s_fixed(0, 0,0);
01214     s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
01215     return s_fixed;
01216 }
01217