src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 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.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15
  16 //#define COMPUTE_IMPULSE_DENOM 1
  17 //It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
  18
  19 #include "btSequentialImpulseConstraintSolver.h"
  20 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
  21 #include "BulletDynamics/Dynamics/btRigidBody.h"
  22 #include "btContactConstraint.h"
  23 #include "btSolve2LinearConstraint.h"
  24 #include "btContactSolverInfo.h"
  25 #include "LinearMath/btIDebugDraw.h"
  26 #include "btJacobianEntry.h"
  27 #include "LinearMath/btMinMax.h"
  28 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
  29 #include <new>
  30 #include "LinearMath/btStackAlloc.h"
  31 #include "LinearMath/btQuickprof.h"
  32 #include "btSolverBody.h"
  33 #include "btSolverConstraint.h"
  34 #include "LinearMath/btAlignedObjectArray.h"
  35 #include <string.h> //for memset
  36
  37 int             gNumSplitImpulseRecoveries = 0;
  38
  39 btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
  40 :m_btSeed2(0)
  41 {
  42
  43 }
  44
  45 btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
  46 {
  47 }
  48
  49 #ifdef USE_SIMD
  50 #include <emmintrin.h>
  51 #define btVecSplat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e))
  52 static inline __m128 btSimdDot3( __m128 vec0, __m128 vec1 )
  53 {
  54         __m128 result = _mm_mul_ps( vec0, vec1);
  55         return _mm_add_ps( btVecSplat( result, 0 ), _mm_add_ps( btVecSplat( result, 1 ), btVecSplat( result, 2 ) ) );
  56 }
  57 #endif//USE_SIMD
  58
  59 // Project Gauss Seidel or the equivalent Sequential Impulse
  60 void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
  61 {
  62 #ifdef USE_SIMD
  63         __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
  64         __m128  lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
  65         __m128  upperLimit1 = _mm_set1_ps(c.m_upperLimit);
  66         __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)));
  67         __m128 deltaVel1Dotn    =       _mm_add_ps(btSimdDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
  68         __m128 deltaVel2Dotn    =       _mm_sub_ps(btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),btSimdDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128));
  69         deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
  70         deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
  71         btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
  72         btSimdScalar resultLowerLess,resultUpperLess;
  73         resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
  74         resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
  75         __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
  76         deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
  77         c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
  78         __m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp);
  79         deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) );
  80         c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) );
  81         __m128  linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128);
  82         __m128  linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128);
  83         __m128 impulseMagnitude = deltaImpulse;
  84         body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
  85         body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
  86         body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
  87         body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
  88 #else
  89         resolveSingleConstraintRowGeneric(body1,body2,c);
  90 #endif
  91 }
  92
  93 // Project Gauss Seidel or the equivalent Sequential Impulse
  94  void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
  95 {
  96         btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
  97         const btScalar deltaVel1Dotn    =       c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity())   + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
  98         const btScalar deltaVel2Dotn    =       -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
  99
 100 //      const btScalar delta_rel_vel    =       deltaVel1Dotn-deltaVel2Dotn;
 101         deltaImpulse    -=      deltaVel1Dotn*c.m_jacDiagABInv;
 102         deltaImpulse    -=      deltaVel2Dotn*c.m_jacDiagABInv;
 103
 104         const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
 105         if (sum < c.m_lowerLimit)
 106         {
 107                 deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
 108                 c.m_appliedImpulse = c.m_lowerLimit;
 109         }
 110         else if (sum > c.m_upperLimit)
 111         {
 112                 deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
 113                 c.m_appliedImpulse = c.m_upperLimit;
 114         }
 115         else
 116         {
 117                 c.m_appliedImpulse = sum;
 118         }
 119                 body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
 120                 body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
 121 }
 122
 123  void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
 124 {
 125 #ifdef USE_SIMD
 126         __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
 127         __m128  lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
 128         __m128  upperLimit1 = _mm_set1_ps(c.m_upperLimit);
 129         __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)));
 130         __m128 deltaVel1Dotn    =       _mm_add_ps(btSimdDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
 131         __m128 deltaVel2Dotn    =       _mm_sub_ps(btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),btSimdDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128));
 132         deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
 133         deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
 134         btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
 135         btSimdScalar resultLowerLess,resultUpperLess;
 136         resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
 137         resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
 138         __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
 139         deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
 140         c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
 141         __m128  linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128);
 142         __m128  linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128);
 143         __m128 impulseMagnitude = deltaImpulse;
 144         body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
 145         body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
 146         body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
 147         body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
 148 #else
 149         resolveSingleConstraintRowLowerLimit(body1,body2,c);
 150 #endif
 151 }
 152
 153 // Project Gauss Seidel or the equivalent Sequential Impulse
 154  void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
 155 {
 156         btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
 157         const btScalar deltaVel1Dotn    =       c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity())   + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
 158         const btScalar deltaVel2Dotn    =       -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
 159
 160         deltaImpulse    -=      deltaVel1Dotn*c.m_jacDiagABInv;
 161         deltaImpulse    -=      deltaVel2Dotn*c.m_jacDiagABInv;
 162         const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
 163         if (sum < c.m_lowerLimit)
 164         {
 165                 deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
 166                 c.m_appliedImpulse = c.m_lowerLimit;
 167         }
 168         else
 169         {
 170                 c.m_appliedImpulse = sum;
 171         }
 172         body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
 173         body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
 174 }
 175
 176
 177 void    btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly(
 178         btRigidBody& body1,
 179         btRigidBody& body2,
 180         const btSolverConstraint& c)
 181 {
 182                 if (c.m_rhsPenetration)
 183         {
 184                         gNumSplitImpulseRecoveries++;
 185                         btScalar deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm;
 186                         const btScalar deltaVel1Dotn    =       c.m_contactNormal.dot(body1.internalGetPushVelocity())  + c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity());
 187                         const btScalar deltaVel2Dotn    =       -c.m_contactNormal.dot(body2.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity());
 188
 189                         deltaImpulse    -=      deltaVel1Dotn*c.m_jacDiagABInv;
 190                         deltaImpulse    -=      deltaVel2Dotn*c.m_jacDiagABInv;
 191                         const btScalar sum = btScalar(c.m_appliedPushImpulse) + deltaImpulse;
 192                         if (sum < c.m_lowerLimit)
 193                         {
 194                                 deltaImpulse = c.m_lowerLimit-c.m_appliedPushImpulse;
 195                                 c.m_appliedPushImpulse = c.m_lowerLimit;
 196                         }
 197                         else
 198                         {
 199                                 c.m_appliedPushImpulse = sum;
 200                         }
 201                         body1.internalApplyPushImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
 202                         body2.internalApplyPushImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
 203         }
 204 }
 205
 206  void btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
 207 {
 208 #ifdef USE_SIMD
 209         if (!c.m_rhsPenetration)
 210                 return;
 211
 212         gNumSplitImpulseRecoveries++;
 213
 214         __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedPushImpulse);
 215         __m128  lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
 216         __m128  upperLimit1 = _mm_set1_ps(c.m_upperLimit);
 217         __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)));
 218         __m128 deltaVel1Dotn    =       _mm_add_ps(btSimdDot3(c.m_contactNormal.mVec128,body1.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128));
 219         __m128 deltaVel2Dotn    =       _mm_sub_ps(btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128),btSimdDot3((c.m_contactNormal).mVec128,body2.internalGetPushVelocity().mVec128));
 220         deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
 221         deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
 222         btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
 223         btSimdScalar resultLowerLess,resultUpperLess;
 224         resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
 225         resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
 226         __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
 227         deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
 228         c.m_appliedPushImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
 229         __m128  linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128);
 230         __m128  linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128);
 231         __m128 impulseMagnitude = deltaImpulse;
 232         body1.internalGetPushVelocity().mVec128 = _mm_add_ps(body1.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
 233         body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
 234         body2.internalGetPushVelocity().mVec128 = _mm_sub_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
 235         body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
 236 #else
 237         resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c);
 238 #endif
 239 }
 240
 241
 242
 243 unsigned long btSequentialImpulseConstraintSolver::btRand2()
 244 {
 245         m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
 246         return m_btSeed2;
 247 }
 248
 249
 250
 251 //See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
 252 int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
 253 {
 254         // seems good; xor-fold and modulus
 255         const unsigned long un = static_cast<unsigned long>(n);
 256         unsigned long r = btRand2();
 257
 258         // note: probably more aggressive than it needs to be -- might be
 259         //       able to get away without one or two of the innermost branches.
 260         if (un <= 0x00010000UL) {
 261                 r ^= (r >> 16);
 262                 if (un <= 0x00000100UL) {
 263                         r ^= (r >> 8);
 264                         if (un <= 0x00000010UL) {
 265                                 r ^= (r >> 4);
 266                                 if (un <= 0x00000004UL) {
 267                                         r ^= (r >> 2);
 268                                         if (un <= 0x00000002UL) {
 269                                                 r ^= (r >> 1);
 270                                         }
 271                                 }
 272                         }
 273                 }
 274         }
 275
 276         return (int) (r % un);
 277 }
 278
 279
 280 #if 0
 281 void    btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
 282 {
 283         btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0;
 284
 285         solverBody->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
 286         solverBody->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
 287         solverBody->internalGetPushVelocity().setValue(0.f,0.f,0.f);
 288         solverBody->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
 289
 290         if (rb)
 291         {
 292                 solverBody->internalGetInvMass() = btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor();
 293                 solverBody->m_originalBody = rb;
 294                 solverBody->m_angularFactor = rb->getAngularFactor();
 295         } else
 296         {
 297                 solverBody->internalGetInvMass().setValue(0,0,0);
 298                 solverBody->m_originalBody = 0;
 299                 solverBody->m_angularFactor.setValue(1,1,1);
 300         }
 301 }
 302 #endif
 303
 304
 305
 306
 307
 308 btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution)
 309 {
 310         btScalar rest = restitution * -rel_vel;
 311         return rest;
 312 }
 313
 314
 315
 316 void    applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection);
 317 void    applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection)
 318 {
 319         if (colObj && colObj->hasAnisotropicFriction())
 320         {
 321                 // transform to local coordinates
 322                 btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis();
 323                 const btVector3& friction_scaling = colObj->getAnisotropicFriction();
 324                 //apply anisotropic friction
 325                 loc_lateral *= friction_scaling;
 326                 // ... and transform it back to global coordinates
 327                 frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral;
 328         }
 329 }
 330
 331
 332 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)
 333 {
 334
 335
 336         btRigidBody* body0=btRigidBody::upcast(colObj0);
 337         btRigidBody* body1=btRigidBody::upcast(colObj1);
 338
 339         solverConstraint.m_contactNormal = normalAxis;
 340
 341         solverConstraint.m_solverBodyA = body0 ? body0 : &getFixedBody();
 342         solverConstraint.m_solverBodyB = body1 ? body1 : &getFixedBody();
 343
 344         solverConstraint.m_friction = cp.m_combinedFriction;
 345         solverConstraint.m_originalContactPoint = 0;
 346
 347         solverConstraint.m_appliedImpulse = 0.f;
 348         solverConstraint.m_appliedPushImpulse = 0.f;
 349
 350         {
 351                 btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
 352                 solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
 353                 solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0);
 354         }
 355         {
 356                 btVector3 ftorqueAxis1 = rel_pos2.cross(-solverConstraint.m_contactNormal);
 357                 solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
 358                 solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0);
 359         }
 360
 361 #ifdef COMPUTE_IMPULSE_DENOM
 362         btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
 363         btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
 364 #else
 365         btVector3 vec;
 366         btScalar denom0 = 0.f;
 367         btScalar denom1 = 0.f;
 368         if (body0)
 369         {
 370                 vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
 371                 denom0 = body0->getInvMass() + normalAxis.dot(vec);
 372         }
 373         if (body1)
 374         {
 375                 vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
 376                 denom1 = body1->getInvMass() + normalAxis.dot(vec);
 377         }
 378
 379
 380 #endif //COMPUTE_IMPULSE_DENOM
 381         btScalar denom = relaxation/(denom0+denom1);
 382         solverConstraint.m_jacDiagABInv = denom;
 383
 384 #ifdef _USE_JACOBIAN
 385         solverConstraint.m_jac =  btJacobianEntry (
 386                 rel_pos1,rel_pos2,solverConstraint.m_contactNormal,
 387                 body0->getInvInertiaDiagLocal(),
 388                 body0->getInvMass(),
 389                 body1->getInvInertiaDiagLocal(),
 390                 body1->getInvMass());
 391 #endif //_USE_JACOBIAN
 392
 393
 394         {
 395                 btScalar rel_vel;
 396                 btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?body0->getLinearVelocity():btVector3(0,0,0))
 397                         + solverConstraint.m_relpos1CrossNormal.dot(body0?body0->getAngularVelocity():btVector3(0,0,0));
 398                 btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?body1->getLinearVelocity():btVector3(0,0,0))
 399                         + solverConstraint.m_relpos2CrossNormal.dot(body1?body1->getAngularVelocity():btVector3(0,0,0));
 400
 401                 rel_vel = vel1Dotn+vel2Dotn;
 402
 403 //              btScalar positionalError = 0.f;
 404
 405                 btSimdScalar velocityError =  desiredVelocity - rel_vel;
 406                 btSimdScalar    velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
 407                 solverConstraint.m_rhs = velocityImpulse;
 408                 solverConstraint.m_cfm = cfmSlip;
 409                 solverConstraint.m_lowerLimit = 0;
 410                 solverConstraint.m_upperLimit = 1e10f;
 411         }
 412 }
 413
 414
 415
 416 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)
 417 {
 418         btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
 419         solverConstraint.m_frictionIndex = frictionIndex;
 420         setupFrictionConstraint(solverConstraint, normalAxis, solverBodyA, solverBodyB, cp, rel_pos1, rel_pos2,
 421                                                         colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
 422         return solverConstraint;
 423 }
 424
 425 int     btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body)
 426 {
 427 #if 0
 428         int solverBodyIdA = -1;
 429
 430         if (body.getCompanionId() >= 0)
 431         {
 432                 //body has already been converted
 433                 solverBodyIdA = body.getCompanionId();
 434         } else
 435         {
 436                 btRigidBody* rb = btRigidBody::upcast(&body);
 437                 if (rb && rb->getInvMass())
 438                 {
 439                         solverBodyIdA = m_tmpSolverBodyPool.size();
 440                         btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
 441                         initSolverBody(&solverBody,&body);
 442                         body.setCompanionId(solverBodyIdA);
 443                 } else
 444                 {
 445                         return 0;//assume first one is a fixed solver body
 446                 }
 447         }
 448         return solverBodyIdA;
 449 #endif
 450         return 0;
 451 }
 452 #include <stdio.h>
 453
 454
 455 void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint,
 456                                                                                                                                  btCollisionObject* colObj0, btCollisionObject* colObj1,
 457                                                                                                                                  btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
 458                                                                                                                                  btVector3& vel, btScalar& rel_vel, btScalar& relaxation,
 459                                                                                                                                  btVector3& rel_pos1, btVector3& rel_pos2)
 460 {
 461                         btRigidBody* rb0 = btRigidBody::upcast(colObj0);
 462                         btRigidBody* rb1 = btRigidBody::upcast(colObj1);
 463
 464                         const btVector3& pos1 = cp.getPositionWorldOnA();
 465                         const btVector3& pos2 = cp.getPositionWorldOnB();
 466
 467 //                      btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
 468 //                      btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
 469                         rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
 470                         rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
 471
 472                         relaxation = 1.f;
 473
 474                         btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
 475                         solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
 476                         btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
 477                         solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
 478
 479                                 {
 480 #ifdef COMPUTE_IMPULSE_DENOM
 481                                         btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
 482                                         btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
 483 #else
 484                                         btVector3 vec;
 485                                         btScalar denom0 = 0.f;
 486                                         btScalar denom1 = 0.f;
 487                                         if (rb0)
 488                                         {
 489                                                 vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
 490                                                 denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
 491                                         }
 492                                         if (rb1)
 493                                         {
 494                                                 vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
 495                                                 denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
 496                                         }
 497 #endif //COMPUTE_IMPULSE_DENOM
 498
 499                                         btScalar denom = relaxation/(denom0+denom1);
 500                                         solverConstraint.m_jacDiagABInv = denom;
 501                                 }
 502
 503                                 solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
 504                                 solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB);
 505                                 solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(-cp.m_normalWorldOnB);
 506
 507
 508
 509
 510                         btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
 511                         btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
 512                         vel  = vel1 - vel2;
 513                         rel_vel = cp.m_normalWorldOnB.dot(vel);
 514
 515                                 btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
 516
 517
 518                                 solverConstraint.m_friction = cp.m_combinedFriction;
 519
 520                                 btScalar restitution = 0.f;
 521
 522                                 if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold)
 523                                 {
 524                                         restitution = 0.f;
 525                                 } else
 526                                 {
 527                                         restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
 528                                         if (restitution <= btScalar(0.))
 529                                         {
 530                                                 restitution = 0.f;
 531                                         };
 532                                 }
 533
 534
 535                                 ///warm starting (or zero if disabled)
 536                                 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
 537                                 {
 538                                         solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
 539                                         if (rb0)
 540                                                 rb0->internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
 541                                         if (rb1)
 542                                                 rb1->internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
 543                                 } else
 544                                 {
 545                                         solverConstraint.m_appliedImpulse = 0.f;
 546                                 }
 547
 548                                 solverConstraint.m_appliedPushImpulse = 0.f;
 549
 550                                 {
 551                                         btScalar rel_vel;
 552                                         btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?rb0->getLinearVelocity():btVector3(0,0,0))
 553                                                 + solverConstraint.m_relpos1CrossNormal.dot(rb0?rb0->getAngularVelocity():btVector3(0,0,0));
 554                                         btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1?rb1->getLinearVelocity():btVector3(0,0,0))
 555                                                 + solverConstraint.m_relpos2CrossNormal.dot(rb1?rb1->getAngularVelocity():btVector3(0,0,0));
 556
 557                                         rel_vel = vel1Dotn+vel2Dotn;
 558
 559                                         btScalar positionalError = 0.f;
 560                                         btScalar        velocityError = restitution - rel_vel;// * damping;
 561
 562                                         if (penetration>0)
 563                                         {
 564                                                 positionalError = 0;
 565                                                 velocityError -= penetration / infoGlobal.m_timeStep;
 566                                         } else
 567                                         {
 568                                                 positionalError = -penetration * infoGlobal.m_erp/infoGlobal.m_timeStep;
 569                                         }
 570
 571                                         btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
 572                                         btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
 573                                         if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
 574                                         {
 575                                                 //combine position and velocity into rhs
 576                                                 solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
 577                                                 solverConstraint.m_rhsPenetration = 0.f;
 578                                         } else
 579                                         {
 580                                                 //split position and velocity into rhs and m_rhsPenetration
 581                                                 solverConstraint.m_rhs = velocityImpulse;
 582                                                 solverConstraint.m_rhsPenetration = penetrationImpulse;
 583                                         }
 584                                         solverConstraint.m_cfm = 0.f;
 585                                         solverConstraint.m_lowerLimit = 0;
 586                                         solverConstraint.m_upperLimit = 1e10f;
 587                                 }
 588
 589
 590
 591
 592 }
 593
 594
 595
 596 void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint,
 597                                                                                                                                                 btRigidBody* rb0, btRigidBody* rb1,
 598                                                                                                                                  btManifoldPoint& cp, const btContactSolverInfo& infoGlobal)
 599 {
 600                                         if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
 601                                         {
 602                                                 {
 603                                                         btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
 604                                                         if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
 605                                                         {
 606                                                                 frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
 607                                                                 if (rb0)
 608                                                                         rb0->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
 609                                                                 if (rb1)
 610                                                                         rb1->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse);
 611                                                         } else
 612                                                         {
 613                                                                 frictionConstraint1.m_appliedImpulse = 0.f;
 614                                                         }
 615                                                 }
 616
 617                                                 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 618                                                 {
 619                                                         btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
 620                                                         if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
 621                                                         {
 622                                                                 frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
 623                                                                 if (rb0)
 624                                                                         rb0->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
 625                                                                 if (rb1)
 626                                                                         rb1->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse);
 627                                                         } else
 628                                                         {
 629                                                                 frictionConstraint2.m_appliedImpulse = 0.f;
 630                                                         }
 631                                                 }
 632                                         } else
 633                                         {
 634                                                 btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
 635                                                 frictionConstraint1.m_appliedImpulse = 0.f;
 636                                                 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 637                                                 {
 638                                                         btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
 639                                                         frictionConstraint2.m_appliedImpulse = 0.f;
 640                                                 }
 641                                         }
 642 }
 643
 644
 645
 646
 647 void    btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
 648 {
 649         btCollisionObject* colObj0=0,*colObj1=0;
 650
 651         colObj0 = (btCollisionObject*)manifold->getBody0();
 652         colObj1 = (btCollisionObject*)manifold->getBody1();
 653
 654
 655         btRigidBody* solverBodyA = btRigidBody::upcast(colObj0);
 656         btRigidBody* solverBodyB = btRigidBody::upcast(colObj1);
 657
 658         ///avoid collision response between two static objects
 659         if ((!solverBodyA || !solverBodyA->getInvMass()) && (!solverBodyB || !solverBodyB->getInvMass()))
 660                 return;
 661
 662         for (int j=0;j<manifold->getNumContacts();j++)
 663         {
 664
 665                 btManifoldPoint& cp = manifold->getContactPoint(j);
 666
 667                 if (cp.getDistance() <= manifold->getContactProcessingThreshold())
 668                 {
 669                         btVector3 rel_pos1;
 670                         btVector3 rel_pos2;
 671                         btScalar relaxation;
 672                         btScalar rel_vel;
 673                         btVector3 vel;
 674
 675                         int frictionIndex = m_tmpSolverContactConstraintPool.size();
 676                         btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing();
 677                         btRigidBody* rb0 = btRigidBody::upcast(colObj0);
 678                         btRigidBody* rb1 = btRigidBody::upcast(colObj1);
 679                         solverConstraint.m_solverBodyA = rb0? rb0 : &getFixedBody();
 680                         solverConstraint.m_solverBodyB = rb1? rb1 : &getFixedBody();
 681                         solverConstraint.m_originalContactPoint = &cp;
 682
 683                         setupContactConstraint(solverConstraint, colObj0, colObj1, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2);
 684
 685 //                      const btVector3& pos1 = cp.getPositionWorldOnA();
 686 //                      const btVector3& pos2 = cp.getPositionWorldOnB();
 687
 688                         /////setup the friction constraints
 689
 690                         solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
 691
 692                         if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
 693                         {
 694                                 cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
 695                                 btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
 696                                 if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
 697                                 {
 698                                         cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
 699                                         if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 700                                         {
 701                                                 cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
 702                                                 cp.m_lateralFrictionDir2.normalize();//??
 703                                                 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
 704                                                 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
 705                                                 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
 706                                         }
 707
 708                                         applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
 709                                         applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
 710                                         addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
 711                                         cp.m_lateralFrictionInitialized = true;
 712                                 } else
 713                                 {
 714                                         //re-calculate friction direction every frame, todo: check if this is really needed
 715                                         btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
 716                                         if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 717                                         {
 718                                                 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
 719                                                 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
 720                                                 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
 721                                         }
 722
 723                                         applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
 724                                         applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
 725                                         addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
 726
 727                                         cp.m_lateralFrictionInitialized = true;
 728                                 }
 729
 730                         } else
 731                         {
 732                                 addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1);
 733                                 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 734                                         addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2);
 735                         }
 736
 737                         setFrictionConstraintImpulse( solverConstraint, rb0, rb1, cp, infoGlobal);
 738
 739                 }
 740         }
 741 }
 742
 743 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
 744 {
 745         BT_PROFILE("solveGroupCacheFriendlySetup");
 746         (void)stackAlloc;
 747         (void)debugDrawer;
 748
 749         m_maxOverrideNumSolverIterations = 0;
 750
 751         if (!(numConstraints + numManifolds))
 752         {
 753                 //              printf("empty\n");
 754                 return 0.f;
 755         }
 756
 757         if (infoGlobal.m_splitImpulse)
 758         {
 759                 for (int i = 0; i < numBodies; i++)
 760                 {
 761                         btRigidBody* body = btRigidBody::upcast(bodies[i]);
 762                         if (body)
 763                         {
 764                                 body->internalGetDeltaLinearVelocity().setZero();
 765                                 body->internalGetDeltaAngularVelocity().setZero();
 766                                 body->internalGetPushVelocity().setZero();
 767                                 body->internalGetTurnVelocity().setZero();
 768                         }
 769                 }
 770         }
 771         else
 772         {
 773                 for (int i = 0; i < numBodies; i++)
 774                 {
 775                         btRigidBody* body = btRigidBody::upcast(bodies[i]);
 776                         if (body)
 777                         {
 778                                 body->internalGetDeltaLinearVelocity().setZero();
 779                                 body->internalGetDeltaAngularVelocity().setZero();
 780                         }
 781                 }
 782         }
 783
 784         if (1)
 785         {
 786                 int j;
 787                 for (j=0;j<numConstraints;j++)
 788                 {
 789                         btTypedConstraint* constraint = constraints[j];
 790                         constraint->buildJacobian();
 791                         constraint->internalSetAppliedImpulse(0.0f);
 792                 }
 793         }
 794         //btRigidBody* rb0=0,*rb1=0;
 795
 796         //if (1)
 797         {
 798                 {
 799
 800                         int totalNumRows = 0;
 801                         int i;
 802
 803                         m_tmpConstraintSizesPool.resize(numConstraints);
 804                         //calculate the total number of contraint rows
 805                         for (i=0;i<numConstraints;i++)
 806                         {
 807                                 btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
 808                                 if (constraints[i]->isEnabled())
 809                                 {
 810                                         constraints[i]->getInfo1(&info1);
 811                                 } else
 812                                 {
 813                                         info1.m_numConstraintRows = 0;
 814                                         info1.nub = 0;
 815                                 }
 816                                 totalNumRows += info1.m_numConstraintRows;
 817                         }
 818                         m_tmpSolverNonContactConstraintPool.resize(totalNumRows);
 819
 820
 821                         ///setup the btSolverConstraints
 822                         int currentRow = 0;
 823
 824                         for (i=0;i<numConstraints;i++)
 825                         {
 826                                 const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
 827
 828                                 if (info1.m_numConstraintRows)
 829                                 {
 830                                         btAssert(currentRow<totalNumRows);
 831
 832                                         btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
 833                                         btTypedConstraint* constraint = constraints[i];
 834                                         btRigidBody& rbA = constraint->getRigidBodyA();
 835                                         btRigidBody& rbB = constraint->getRigidBodyB();
 836
 837
 838                                         int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations;
 839                                         if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations)
 840                                                 m_maxOverrideNumSolverIterations = overrideNumSolverIterations;
 841
 842
 843                                         int j;
 844                                         for ( j=0;j<info1.m_numConstraintRows;j++)
 845                                         {
 846                                                 memset(&currentConstraintRow[j],0,sizeof(btSolverConstraint));
 847                                                 currentConstraintRow[j].m_lowerLimit = -SIMD_INFINITY;
 848                                                 currentConstraintRow[j].m_upperLimit = SIMD_INFINITY;
 849                                                 currentConstraintRow[j].m_appliedImpulse = 0.f;
 850                                                 currentConstraintRow[j].m_appliedPushImpulse = 0.f;
 851                                                 currentConstraintRow[j].m_solverBodyA = &rbA;
 852                                                 currentConstraintRow[j].m_solverBodyB = &rbB;
 853                                                 currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations;
 854                                         }
 855
 856                                         rbA.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
 857                                         rbA.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
 858                                         rbB.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
 859                                         rbB.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
 860
 861
 862
 863                                         btTypedConstraint::btConstraintInfo2 info2;
 864                                         info2.fps = 1.f/infoGlobal.m_timeStep;
 865                                         info2.erp = infoGlobal.m_erp;
 866                                         info2.m_J1linearAxis = currentConstraintRow->m_contactNormal;
 867                                         info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
 868                                         info2.m_J2linearAxis = 0;
 869                                         info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
 870                                         info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this
 871                                         ///the size of btSolverConstraint needs be a multiple of btScalar
 872                                         btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
 873                                         info2.m_constraintError = &currentConstraintRow->m_rhs;
 874                                         currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
 875                                         info2.m_damping = infoGlobal.m_damping;
 876                                         info2.cfm = &currentConstraintRow->m_cfm;
 877                                         info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
 878                                         info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
 879                                         info2.m_numIterations = infoGlobal.m_numIterations;
 880                                         constraints[i]->getInfo2(&info2);
 881
 882                                         ///finalize the constraint setup
 883                                         for ( j=0;j<info1.m_numConstraintRows;j++)
 884                                         {
 885                                                 btSolverConstraint& solverConstraint = currentConstraintRow[j];
 886
 887                                                 if (solverConstraint.m_upperLimit>=constraints[i]->getBreakingImpulseThreshold())
 888                                                 {
 889                                                         solverConstraint.m_upperLimit = constraints[i]->getBreakingImpulseThreshold();
 890                                                 }
 891
 892                                                 if (solverConstraint.m_lowerLimit<=-constraints[i]->getBreakingImpulseThreshold())
 893                                                 {
 894                                                         solverConstraint.m_lowerLimit = -constraints[i]->getBreakingImpulseThreshold();
 895                                                 }
 896
 897                                                 solverConstraint.m_originalContactPoint = constraint;
 898
 899                                                 {
 900                                                         const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
 901                                                         solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor();
 902                                                 }
 903                                                 {
 904                                                         const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
 905                                                         solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor();
 906                                                 }
 907
 908                                                 {
 909                                                         btVector3 iMJlA = solverConstraint.m_contactNormal*rbA.getInvMass();
 910                                                         btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;
 911                                                         btVector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal?
 912                                                         btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;
 913
 914                                                         btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal);
 915                                                         sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
 916                                                         sum += iMJlB.dot(solverConstraint.m_contactNormal);
 917                                                         sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
 918
 919                                                         solverConstraint.m_jacDiagABInv = btScalar(1.)/sum;
 920                                                 }
 921
 922
 923                                                 ///fix rhs
 924                                                 ///todo: add force/torque accelerators
 925                                                 {
 926                                                         btScalar rel_vel;
 927                                                         btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity());
 928                                                         btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity());
 929
 930                                                         rel_vel = vel1Dotn+vel2Dotn;
 931
 932                                                         btScalar restitution = 0.f;
 933                                                         btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
 934                                                         btScalar        velocityError = restitution - rel_vel * info2.m_damping;
 935                                                         btScalar        penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
 936                                                         btScalar        velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
 937                                                         solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
 938                                                         solverConstraint.m_appliedImpulse = 0.f;
 939
 940                                                 }
 941                                         }
 942                                 }
 943                                 currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows;
 944                         }
 945                 }
 946
 947                 {
 948                         int i;
 949                         btPersistentManifold* manifold = 0;
 950 //                      btCollisionObject* colObj0=0,*colObj1=0;
 951
 952
 953                         for (i=0;i<numManifolds;i++)
 954                         {
 955                                 manifold = manifoldPtr[i];
 956                                 convertContact(manifold,infoGlobal);
 957                         }
 958                 }
 959         }
 960
 961         btContactSolverInfo info = infoGlobal;
 962
 963
 964         int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
 965         int numConstraintPool = m_tmpSolverContactConstraintPool.size();
 966         int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
 967
 968         ///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
 969         m_orderNonContactConstraintPool.resize(numNonContactPool);
 970         m_orderTmpConstraintPool.resize(numConstraintPool);
 971         m_orderFrictionConstraintPool.resize(numFrictionPool);
 972         {
 973                 int i;
 974                 for (i=0;i<numNonContactPool;i++)
 975                 {
 976                         m_orderNonContactConstraintPool[i] = i;
 977                 }
 978                 for (i=0;i<numConstraintPool;i++)
 979                 {
 980                         m_orderTmpConstraintPool[i] = i;
 981                 }
 982                 for (i=0;i<numFrictionPool;i++)
 983                 {
 984                         m_orderFrictionConstraintPool[i] = i;
 985                 }
 986         }
 987
 988         return 0.f;
 989
 990 }
 991
 992 btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
 993 {
 994
 995         int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
 996         int numConstraintPool = m_tmpSolverContactConstraintPool.size();
 997         int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
 998
 999         int j;
1000
1001         if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
1002         {
1003                 if ((iteration & 7) == 0) {
1004                         for (j=0; j<numNonContactPool; ++j) {
1005                                 int tmp = m_orderNonContactConstraintPool[j];
1006                                 int swapi = btRandInt2(j+1);
1007                                 m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi];
1008                                 m_orderNonContactConstraintPool[swapi] = tmp;
1009                         }
1010
1011                         //contact/friction constraints are not solved more than
1012                         if (iteration< infoGlobal.m_numIterations)
1013                         {
1014                                 for (j=0; j<numConstraintPool; ++j) {
1015                                         int tmp = m_orderTmpConstraintPool[j];
1016                                         int swapi = btRandInt2(j+1);
1017                                         m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
1018                                         m_orderTmpConstraintPool[swapi] = tmp;
1019                                 }
1020
1021                                 for (j=0; j<numFrictionPool; ++j) {
1022                                         int tmp = m_orderFrictionConstraintPool[j];
1023                                         int swapi = btRandInt2(j+1);
1024                                         m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
1025                                         m_orderFrictionConstraintPool[swapi] = tmp;
1026                                 }
1027                         }
1028                 }
1029         }
1030
1031         if (infoGlobal.m_solverMode & SOLVER_SIMD)
1032         {
1033                 ///solve all joint constraints, using SIMD, if available
1034                 for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
1035                 {
1036                         btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
1037                         if (iteration < constraint.m_overrideNumSolverIterations)
1038                                 resolveSingleConstraintRowGenericSIMD(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint);
1039                 }
1040
1041                 if (iteration< infoGlobal.m_numIterations)
1042                 {
1043                         for (j=0;j<numConstraints;j++)
1044                         {
1045                                 constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
1046                         }
1047
1048                         ///solve all contact constraints using SIMD, if available
1049                         int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1050                         for (j=0;j<numPoolConstraints;j++)
1051                         {
1052                                 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
1053                                 resolveSingleConstraintRowLowerLimitSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
1054
1055                         }
1056
1057                         ///solve all friction constraints, using SIMD, if available
1058                         int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
1059                         for (j=0;j<numFrictionPoolConstraints;j++)
1060                         {
1061                                 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
1062                                 btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1063
1064                                 if (totalImpulse>btScalar(0))
1065                                 {
1066                                         solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
1067                                         solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
1068
1069                                         resolveSingleConstraintRowGenericSIMD(*solveManifold.m_solverBodyA,     *solveManifold.m_solverBodyB,solveManifold);
1070                                 }
1071                         }
1072                 }
1073         } else
1074         {
1075
1076                 ///solve all joint constraints
1077                 for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
1078                 {
1079                         btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
1080                         if (iteration < constraint.m_overrideNumSolverIterations)
1081                                 resolveSingleConstraintRowGeneric(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint);
1082                 }
1083
1084                 if (iteration< infoGlobal.m_numIterations)
1085                 {
1086                         for (j=0;j<numConstraints;j++)
1087                         {
1088                                 constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
1089                         }
1090                         ///solve all contact constraints
1091                         int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1092                         for (j=0;j<numPoolConstraints;j++)
1093                         {
1094                                 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
1095                                 resolveSingleConstraintRowLowerLimit(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
1096                         }
1097                         ///solve all friction constraints
1098                         int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
1099                         for (j=0;j<numFrictionPoolConstraints;j++)
1100                         {
1101                                 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
1102                                 btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1103
1104                                 if (totalImpulse>btScalar(0))
1105                                 {
1106                                         solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
1107                                         solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
1108
1109                                         resolveSingleConstraintRowGeneric(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
1110                                 }
1111                         }
1112                 }
1113         }
1114         return 0.f;
1115 }
1116
1117
1118 void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
1119 {
1120         int iteration;
1121         if (infoGlobal.m_splitImpulse)
1122         {
1123                 if (infoGlobal.m_solverMode & SOLVER_SIMD)
1124                 {
1125                         for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
1126                         {
1127                                 {
1128                                         int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1129                                         int j;
1130                                         for (j=0;j<numPoolConstraints;j++)
1131                                         {
1132                                                 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
1133
1134                                                 resolveSplitPenetrationSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
1135                                         }
1136                                 }
1137                         }
1138                 }
1139                 else
1140                 {
1141                         for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
1142                         {
1143                                 {
1144                                         int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1145                                         int j;
1146                                         for (j=0;j<numPoolConstraints;j++)
1147                                         {
1148                                                 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
1149
1150                                                 resolveSplitPenetrationImpulseCacheFriendly(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
1151                                         }
1152                                 }
1153                         }
1154                 }
1155         }
1156 }
1157
1158 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
1159 {
1160         BT_PROFILE("solveGroupCacheFriendlyIterations");
1161
1162         {
1163                 ///this is a special step to resolve penetrations (just for contacts)
1164                 solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
1165
1166                 int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
1167
1168                 for ( int iteration = 0 ; iteration< maxIterations ; iteration++)
1169                 //for ( int iteration = maxIterations-1  ; iteration >= 0;iteration--)
1170                 {
1171                         solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
1172                 }
1173
1174         }
1175         return 0.f;
1176 }
1177
1178 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies ,int numBodies,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** /*constraints*/,int /* numConstraints*/,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
1179 {
1180         int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1181         int i,j;
1182
1183         for (j=0;j<numPoolConstraints;j++)
1184         {
1185
1186                 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
1187                 btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
1188                 btAssert(pt);
1189                 pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
1190                 if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
1191                 {
1192                         pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1193                         pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
1194                 }
1195
1196                 //do a callback here?
1197         }
1198
1199         numPoolConstraints = m_tmpSolverNonContactConstraintPool.size();
1200         for (j=0;j<numPoolConstraints;j++)
1201         {
1202                 const btSolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j];
1203                 btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint;
1204                 constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse);
1205                 if (btFabs(solverConstr.m_appliedImpulse)>=constr->getBreakingImpulseThreshold())
1206                 {
1207                         constr->setEnabled(false);
1208                 }
1209         }
1210
1211
1212         if (infoGlobal.m_splitImpulse)
1213         {
1214                 for ( i=0;i<numBodies;i++)
1215                 {
1216                         btRigidBody* body = btRigidBody::upcast(bodies[i]);
1217                         if (body)
1218                                 body->internalWritebackVelocity(infoGlobal.m_timeStep);
1219                 }
1220         } else
1221         {
1222                 for ( i=0;i<numBodies;i++)
1223                 {
1224                         btRigidBody* body = btRigidBody::upcast(bodies[i]);
1225                         if (body)
1226                                 body->internalWritebackVelocity();
1227                 }
1228         }
1229
1230
1231         m_tmpSolverContactConstraintPool.resize(0);
1232         m_tmpSolverNonContactConstraintPool.resize(0);
1233         m_tmpSolverContactFrictionConstraintPool.resize(0);
1234
1235         return 0.f;
1236 }
1237
1238
1239
1240 /// btSequentialImpulseConstraintSolver Sequentially applies impulses
1241 btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/)
1242 {
1243
1244         BT_PROFILE("solveGroup");
1245         //you need to provide at least some bodies
1246         btAssert(bodies);
1247         btAssert(numBodies);
1248
1249         solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
1250
1251         solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
1252
1253         solveGroupCacheFriendlyFinish(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
1254
1255         return 0.f;
1256 }
1257
1258 void    btSequentialImpulseConstraintSolver::reset()
1259 {
1260         m_btSeed2 = 0;
1261 }
1262
1263 btRigidBody& btSequentialImpulseConstraintSolver::getFixedBody()
1264 {
1265         static btRigidBody s_fixed(0, 0,0);
1266         s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
1267         return s_fixed;
1268 }
1269