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
  22 #include "LinearMath/btIDebugDraw.h"
  23 //#include "btJacobianEntry.h"
  24 #include "LinearMath/btMinMax.h"
  25 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
  26 #include <new>
  27 #include "LinearMath/btStackAlloc.h"
  28 #include "LinearMath/btQuickprof.h"
  29 //#include "btSolverBody.h"
  30 //#include "btSolverConstraint.h"
  31 #include "LinearMath/btAlignedObjectArray.h"
  32 #include <string.h> //for memset
  33
  34 int             gNumSplitImpulseRecoveries = 0;
  35
  36 #include "BulletDynamics/Dynamics/btRigidBody.h"
  37
  38 btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
  39 :m_btSeed2(0)
  40 {
  41
  42 }
  43
  44 btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
  45 {
  46 }
  47
  48 #ifdef USE_SIMD
  49 #include <emmintrin.h>
  50 #define btVecSplat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e))
  51 static inline __m128 btSimdDot3( __m128 vec0, __m128 vec1 )
  52 {
  53         __m128 result = _mm_mul_ps( vec0, vec1);
  54         return _mm_add_ps( btVecSplat( result, 0 ), _mm_add_ps( btVecSplat( result, 1 ), btVecSplat( result, 2 ) ) );
  55 }
  56 #endif//USE_SIMD
  57
  58 // Project Gauss Seidel or the equivalent Sequential Impulse
  59 void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
  60 {
  61 #ifdef USE_SIMD
  62         __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
  63         __m128  lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
  64         __m128  upperLimit1 = _mm_set1_ps(c.m_upperLimit);
  65         __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)));
  66         __m128 deltaVel1Dotn    =       _mm_add_ps(btSimdDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
  67         __m128 deltaVel2Dotn    =       _mm_sub_ps(btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),btSimdDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128));
  68         deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
  69         deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
  70         btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
  71         btSimdScalar resultLowerLess,resultUpperLess;
  72         resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
  73         resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
  74         __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
  75         deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
  76         c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
  77         __m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp);
  78         deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) );
  79         c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) );
  80         __m128  linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128);
  81         __m128  linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128);
  82         __m128 impulseMagnitude = deltaImpulse;
  83         body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
  84         body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
  85         body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
  86         body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
  87 #else
  88         resolveSingleConstraintRowGeneric(body1,body2,c);
  89 #endif
  90 }
  91
  92 // Project Gauss Seidel or the equivalent Sequential Impulse
  93  void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
  94 {
  95         btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
  96         const btScalar deltaVel1Dotn    =       c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity())   + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
  97         const btScalar deltaVel2Dotn    =       -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
  98
  99 //      const btScalar delta_rel_vel    =       deltaVel1Dotn-deltaVel2Dotn;
 100         deltaImpulse    -=      deltaVel1Dotn*c.m_jacDiagABInv;
 101         deltaImpulse    -=      deltaVel2Dotn*c.m_jacDiagABInv;
 102
 103         const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
 104         if (sum < c.m_lowerLimit)
 105         {
 106                 deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
 107                 c.m_appliedImpulse = c.m_lowerLimit;
 108         }
 109         else if (sum > c.m_upperLimit)
 110         {
 111                 deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
 112                 c.m_appliedImpulse = c.m_upperLimit;
 113         }
 114         else
 115         {
 116                 c.m_appliedImpulse = sum;
 117         }
 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(btSolverBody& body1,btSolverBody& 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(btSolverBody& body1,btSolverBody& 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         btSolverBody& body1,
 179         btSolverBody& 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(btSolverBody& body1,btSolverBody& 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
 281 void    btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
 282 {
 283
 284         btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0;
 285
 286         solverBody->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
 287         solverBody->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
 288         solverBody->internalGetPushVelocity().setValue(0.f,0.f,0.f);
 289         solverBody->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
 290
 291         if (rb)
 292         {
 293                 solverBody->m_worldTransform = rb->getWorldTransform();
 294                 solverBody->internalSetInvMass(btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor());
 295                 solverBody->m_originalBody = rb;
 296                 solverBody->m_angularFactor = rb->getAngularFactor();
 297                 solverBody->m_linearFactor = rb->getLinearFactor();
 298                 solverBody->m_linearVelocity = rb->getLinearVelocity();
 299                 solverBody->m_angularVelocity = rb->getAngularVelocity();
 300         } else
 301         {
 302                 solverBody->m_worldTransform.setIdentity();
 303                 solverBody->internalSetInvMass(btVector3(0,0,0));
 304                 solverBody->m_originalBody = 0;
 305                 solverBody->m_angularFactor.setValue(1,1,1);
 306                 solverBody->m_linearFactor.setValue(1,1,1);
 307                 solverBody->m_linearVelocity.setValue(0,0,0);
 308                 solverBody->m_angularVelocity.setValue(0,0,0);
 309         }
 310
 311
 312 }
 313
 314
 315
 316
 317
 318
 319 btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution)
 320 {
 321         btScalar rest = restitution * -rel_vel;
 322         return rest;
 323 }
 324
 325
 326
 327 static void     applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode);
 328 static void     applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode)
 329 {
 330
 331
 332         if (colObj && colObj->hasAnisotropicFriction(frictionMode))
 333         {
 334                 // transform to local coordinates
 335                 btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis();
 336                 const btVector3& friction_scaling = colObj->getAnisotropicFriction();
 337                 //apply anisotropic friction
 338                 loc_lateral *= friction_scaling;
 339                 // ... and transform it back to global coordinates
 340                 frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral;
 341         }
 342
 343 }
 344
 345
 346
 347
 348 void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis,int  solverBodyIdA,int solverBodyIdB,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
 349 {
 350
 351
 352         solverConstraint.m_contactNormal = normalAxis;
 353         btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
 354         btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
 355
 356         btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody;
 357         btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
 358
 359         solverConstraint.m_solverBodyIdA = solverBodyIdA;
 360         solverConstraint.m_solverBodyIdB = solverBodyIdB;
 361
 362         solverConstraint.m_friction = cp.m_combinedFriction;
 363         solverConstraint.m_originalContactPoint = 0;
 364
 365         solverConstraint.m_appliedImpulse = 0.f;
 366         solverConstraint.m_appliedPushImpulse = 0.f;
 367
 368         {
 369                 btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
 370                 solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
 371                 solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0);
 372         }
 373         {
 374                 btVector3 ftorqueAxis1 = rel_pos2.cross(-solverConstraint.m_contactNormal);
 375                 solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
 376                 solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0);
 377         }
 378
 379         {
 380                 btVector3 vec;
 381                 btScalar denom0 = 0.f;
 382                 btScalar denom1 = 0.f;
 383                 if (body0)
 384                 {
 385                         vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
 386                         denom0 = body0->getInvMass() + normalAxis.dot(vec);
 387                 }
 388                 if (body1)
 389                 {
 390                         vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
 391                         denom1 = body1->getInvMass() + normalAxis.dot(vec);
 392                 }
 393                 btScalar denom = relaxation/(denom0+denom1);
 394                 solverConstraint.m_jacDiagABInv = denom;
 395         }
 396
 397         {
 398
 399
 400                 btScalar rel_vel;
 401                 btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?solverBodyA.m_linearVelocity:btVector3(0,0,0))
 402                         + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0));
 403                 btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?solverBodyB.m_linearVelocity:btVector3(0,0,0))
 404                         + solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0));
 405
 406                 rel_vel = vel1Dotn+vel2Dotn;
 407
 408 //              btScalar positionalError = 0.f;
 409
 410                 btSimdScalar velocityError =  desiredVelocity - rel_vel;
 411                 btSimdScalar    velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
 412                 solverConstraint.m_rhs = velocityImpulse;
 413                 solverConstraint.m_cfm = cfmSlip;
 414                 solverConstraint.m_lowerLimit = 0;
 415                 solverConstraint.m_upperLimit = 1e10f;
 416
 417         }
 418 }
 419
 420 btSolverConstraint&     btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
 421 {
 422         btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
 423         solverConstraint.m_frictionIndex = frictionIndex;
 424         setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
 425                                                         colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
 426         return solverConstraint;
 427 }
 428
 429
 430 void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint(       btSolverConstraint& solverConstraint, const btVector3& normalAxis1,int solverBodyIdA,int  solverBodyIdB,
 431                                                                         btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
 432                                                                         btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
 433                                                                         btScalar desiredVelocity, btScalar cfmSlip)
 434
 435 {
 436         btVector3 normalAxis(0,0,0);
 437
 438
 439         solverConstraint.m_contactNormal = normalAxis;
 440         btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
 441         btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
 442
 443         btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody;
 444         btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
 445
 446         solverConstraint.m_solverBodyIdA = solverBodyIdA;
 447         solverConstraint.m_solverBodyIdB = solverBodyIdB;
 448
 449         solverConstraint.m_friction = cp.m_combinedRollingFriction;
 450         solverConstraint.m_originalContactPoint = 0;
 451
 452         solverConstraint.m_appliedImpulse = 0.f;
 453         solverConstraint.m_appliedPushImpulse = 0.f;
 454
 455         {
 456                 btVector3 ftorqueAxis1 = -normalAxis1;
 457                 solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
 458                 solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0);
 459         }
 460         {
 461                 btVector3 ftorqueAxis1 = normalAxis1;
 462                 solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
 463                 solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0);
 464         }
 465
 466
 467         {
 468                 btVector3 iMJaA = body0?body0->getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal:btVector3(0,0,0);
 469                 btVector3 iMJaB = body1?body1->getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal:btVector3(0,0,0);
 470                 btScalar sum = 0;
 471                 sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
 472                 sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
 473                 solverConstraint.m_jacDiagABInv = btScalar(1.)/sum;
 474         }
 475
 476         {
 477
 478
 479                 btScalar rel_vel;
 480                 btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?solverBodyA.m_linearVelocity:btVector3(0,0,0))
 481                         + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0));
 482                 btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?solverBodyB.m_linearVelocity:btVector3(0,0,0))
 483                         + solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0));
 484
 485                 rel_vel = vel1Dotn+vel2Dotn;
 486
 487 //              btScalar positionalError = 0.f;
 488
 489                 btSimdScalar velocityError =  desiredVelocity - rel_vel;
 490                 btSimdScalar    velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
 491                 solverConstraint.m_rhs = velocityImpulse;
 492                 solverConstraint.m_cfm = cfmSlip;
 493                 solverConstraint.m_lowerLimit = 0;
 494                 solverConstraint.m_upperLimit = 1e10f;
 495
 496         }
 497 }
 498
 499
 500
 501
 502
 503
 504
 505
 506 btSolverConstraint&     btSequentialImpulseConstraintSolver::addRollingFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
 507 {
 508         btSolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing();
 509         solverConstraint.m_frictionIndex = frictionIndex;
 510         setupRollingFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
 511                                                         colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
 512         return solverConstraint;
 513 }
 514
 515
 516 int     btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body)
 517 {
 518
 519         int solverBodyIdA = -1;
 520
 521         if (body.getCompanionId() >= 0)
 522         {
 523                 //body has already been converted
 524                 solverBodyIdA = body.getCompanionId();
 525         btAssert(solverBodyIdA < m_tmpSolverBodyPool.size());
 526         } else
 527         {
 528                 btRigidBody* rb = btRigidBody::upcast(&body);
 529                 //convert both active and kinematic objects (for their velocity)
 530                 if (rb && (rb->getInvMass() || rb->isKinematicObject()))
 531                 {
 532                         solverBodyIdA = m_tmpSolverBodyPool.size();
 533                         btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
 534                         initSolverBody(&solverBody,&body);
 535                         body.setCompanionId(solverBodyIdA);
 536                 } else
 537                 {
 538                         return 0;//assume first one is a fixed solver body
 539                 }
 540         }
 541
 542         return solverBodyIdA;
 543
 544 }
 545 #include <stdio.h>
 546
 547
 548 void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint,
 549                                                                                                                                  int solverBodyIdA, int solverBodyIdB,
 550                                                                                                                                  btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
 551                                                                                                                                  btVector3& vel, btScalar& rel_vel, btScalar& relaxation,
 552                                                                                                                                  btVector3& rel_pos1, btVector3& rel_pos2)
 553 {
 554
 555                         const btVector3& pos1 = cp.getPositionWorldOnA();
 556                         const btVector3& pos2 = cp.getPositionWorldOnB();
 557
 558                         btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
 559                         btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
 560
 561                         btRigidBody* rb0 = bodyA->m_originalBody;
 562                         btRigidBody* rb1 = bodyB->m_originalBody;
 563
 564 //                      btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
 565 //                      btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
 566                         rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
 567                         rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
 568
 569                         relaxation = 1.f;
 570
 571                         btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
 572                         solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
 573                         btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);
 574                         solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
 575
 576                                 {
 577 #ifdef COMPUTE_IMPULSE_DENOM
 578                                         btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
 579                                         btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
 580 #else
 581                                         btVector3 vec;
 582                                         btScalar denom0 = 0.f;
 583                                         btScalar denom1 = 0.f;
 584                                         if (rb0)
 585                                         {
 586                                                 vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
 587                                                 denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
 588                                         }
 589                                         if (rb1)
 590                                         {
 591                                                 vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
 592                                                 denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
 593                                         }
 594 #endif //COMPUTE_IMPULSE_DENOM
 595
 596                                         btScalar denom = relaxation/(denom0+denom1);
 597                                         solverConstraint.m_jacDiagABInv = denom;
 598                                 }
 599
 600                                 solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
 601                                 solverConstraint.m_relpos1CrossNormal = torqueAxis0;
 602                                 solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
 603
 604                                 btScalar restitution = 0.f;
 605                                 btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
 606
 607                                 {
 608                                         btVector3 vel1,vel2;
 609
 610                                         vel1 = rb0? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
 611                                         vel2 = rb1? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
 612
 613         //                      btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
 614                                         vel  = vel1 - vel2;
 615                                         rel_vel = cp.m_normalWorldOnB.dot(vel);
 616
 617
 618
 619                                         solverConstraint.m_friction = cp.m_combinedFriction;
 620
 621
 622                                         restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
 623                                         if (restitution <= btScalar(0.))
 624                                         {
 625                                                 restitution = 0.f;
 626                                         };
 627                                 }
 628
 629
 630                                 ///warm starting (or zero if disabled)
 631                                 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
 632                                 {
 633                                         solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
 634                                         if (rb0)
 635                                                 bodyA->internalApplyImpulse(solverConstraint.m_contactNormal*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
 636                                         if (rb1)
 637                                                 bodyB->internalApplyImpulse(solverConstraint.m_contactNormal*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
 638                                 } else
 639                                 {
 640                                         solverConstraint.m_appliedImpulse = 0.f;
 641                                 }
 642
 643                                 solverConstraint.m_appliedPushImpulse = 0.f;
 644
 645                                 {
 646                                         btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?bodyA->m_linearVelocity:btVector3(0,0,0))
 647                                                 + solverConstraint.m_relpos1CrossNormal.dot(rb0?bodyA->m_angularVelocity:btVector3(0,0,0));
 648                                         btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1?bodyB->m_linearVelocity:btVector3(0,0,0))
 649                                                 + solverConstraint.m_relpos2CrossNormal.dot(rb1?bodyB->m_angularVelocity:btVector3(0,0,0));
 650                                         btScalar rel_vel = vel1Dotn+vel2Dotn;
 651
 652                                         btScalar positionalError = 0.f;
 653                                         btScalar        velocityError = restitution - rel_vel;// * damping;
 654
 655
 656                                         btScalar erp = infoGlobal.m_erp2;
 657                                         if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
 658                                         {
 659                                                 erp = infoGlobal.m_erp;
 660                                         }
 661
 662                                         if (penetration>0)
 663                                         {
 664                                                 positionalError = 0;
 665
 666                                                 velocityError -= penetration / infoGlobal.m_timeStep;
 667                                         } else
 668                                         {
 669                                                 positionalError = -penetration * erp/infoGlobal.m_timeStep;
 670                                         }
 671
 672                                         btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
 673                                         btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
 674
 675                                         if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
 676                                         {
 677                                                 //combine position and velocity into rhs
 678                                                 solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
 679                                                 solverConstraint.m_rhsPenetration = 0.f;
 680
 681                                         } else
 682                                         {
 683                                                 //split position and velocity into rhs and m_rhsPenetration
 684                                                 solverConstraint.m_rhs = velocityImpulse;
 685                                                 solverConstraint.m_rhsPenetration = penetrationImpulse;
 686                                         }
 687                                         solverConstraint.m_cfm = 0.f;
 688                                         solverConstraint.m_lowerLimit = 0;
 689                                         solverConstraint.m_upperLimit = 1e10f;
 690                                 }
 691
 692
 693
 694
 695 }
 696
 697
 698
 699 void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint,
 700                                                                                                                                                 int solverBodyIdA, int solverBodyIdB,
 701                                                                                                                                  btManifoldPoint& cp, const btContactSolverInfo& infoGlobal)
 702 {
 703
 704         btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA];
 705         btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB];
 706
 707         btRigidBody* rb0 = bodyA->m_originalBody;
 708         btRigidBody* rb1 = bodyB->m_originalBody;
 709
 710         {
 711                 btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
 712                 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
 713                 {
 714                         frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
 715                         if (rb0)
 716                                 bodyA->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
 717                         if (rb1)
 718                                 bodyB->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse);
 719                 } else
 720                 {
 721                         frictionConstraint1.m_appliedImpulse = 0.f;
 722                 }
 723         }
 724
 725         if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 726         {
 727                 btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
 728                 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
 729                 {
 730                         frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2  * infoGlobal.m_warmstartingFactor;
 731                         if (rb0)
 732                                 bodyA->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
 733                         if (rb1)
 734                                 bodyB->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse);
 735                 } else
 736                 {
 737                         frictionConstraint2.m_appliedImpulse = 0.f;
 738                 }
 739         }
 740 }
 741
 742
 743
 744
 745 void    btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
 746 {
 747         btCollisionObject* colObj0=0,*colObj1=0;
 748
 749         colObj0 = (btCollisionObject*)manifold->getBody0();
 750         colObj1 = (btCollisionObject*)manifold->getBody1();
 751
 752         int solverBodyIdA = getOrInitSolverBody(*colObj0);
 753         int solverBodyIdB = getOrInitSolverBody(*colObj1);
 754
 755 //      btRigidBody* bodyA = btRigidBody::upcast(colObj0);
 756 //      btRigidBody* bodyB = btRigidBody::upcast(colObj1);
 757
 758         btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA];
 759         btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB];
 760
 761
 762
 763         ///avoid collision response between two static objects
 764         if (!solverBodyA || (!solverBodyA->m_originalBody && (!solverBodyB || !solverBodyB->m_originalBody)))
 765                 return;
 766
 767         int rollingFriction=1;
 768         for (int j=0;j<manifold->getNumContacts();j++)
 769         {
 770
 771                 btManifoldPoint& cp = manifold->getContactPoint(j);
 772
 773                 if (cp.getDistance() <= manifold->getContactProcessingThreshold())
 774                 {
 775                         btVector3 rel_pos1;
 776                         btVector3 rel_pos2;
 777                         btScalar relaxation;
 778                         btScalar rel_vel;
 779                         btVector3 vel;
 780
 781                         int frictionIndex = m_tmpSolverContactConstraintPool.size();
 782                         btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing();
 783 //                      btRigidBody* rb0 = btRigidBody::upcast(colObj0);
 784 //                      btRigidBody* rb1 = btRigidBody::upcast(colObj1);
 785                         solverConstraint.m_solverBodyIdA = solverBodyIdA;
 786                         solverConstraint.m_solverBodyIdB = solverBodyIdB;
 787
 788                         solverConstraint.m_originalContactPoint = &cp;
 789
 790                         setupContactConstraint(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2);
 791
 792 //                      const btVector3& pos1 = cp.getPositionWorldOnA();
 793 //                      const btVector3& pos2 = cp.getPositionWorldOnB();
 794
 795                         /////setup the friction constraints
 796
 797                         solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
 798
 799                         btVector3 angVelA,angVelB;
 800                         solverBodyA->getAngularVelocity(angVelA);
 801                         solverBodyB->getAngularVelocity(angVelB);
 802                         btVector3 relAngVel = angVelB-angVelA;
 803
 804                         if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
 805                         {
 806                                 //only a single rollingFriction per manifold
 807                                 rollingFriction--;
 808                                 if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
 809                                 {
 810                                         relAngVel.normalize();
 811                                         applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
 812                                         applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
 813                                         if (relAngVel.length()>0.001)
 814                                                 addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
 815
 816                                 } else
 817                                 {
 818                                         addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
 819                                         btVector3 axis0,axis1;
 820                                         btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
 821                                         applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
 822                                         applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
 823                                         applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
 824                                         applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
 825                                         if (axis0.length()>0.001)
 826                                                 addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
 827                                         if (axis1.length()>0.001)
 828                                                 addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
 829
 830                                 }
 831                         }
 832
 833                         ///Bullet has several options to set the friction directions
 834                         ///By default, each contact has only a single friction direction that is recomputed automatically very frame
 835                         ///based on the relative linear velocity.
 836                         ///If the relative velocity it zero, it will automatically compute a friction direction.
 837
 838                         ///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS.
 839                         ///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction.
 840                         ///
 841                         ///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity.
 842                         ///
 843                         ///The user can manually override the friction directions for certain contacts using a contact callback,
 844                         ///and set the cp.m_lateralFrictionInitialized to true
 845                         ///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2)
 846                         ///this will give a conveyor belt effect
 847                         ///
 848                         if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
 849                         {
 850                                 cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
 851                                 btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
 852                                 if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
 853                                 {
 854                                         cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel);
 855                                         if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 856                                         {
 857                                                 cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
 858                                                 cp.m_lateralFrictionDir2.normalize();//??
 859                                                 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
 860                                                 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
 861                                                 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
 862
 863                                         }
 864
 865                                         applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
 866                                         applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
 867                                         addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
 868
 869                                 } else
 870                                 {
 871                                         btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
 872
 873                                         if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 874                                         {
 875                                                 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
 876                                                 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
 877                                                 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
 878                                         }
 879
 880                                         applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
 881                                         applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
 882                                         addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
 883
 884                                         if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
 885                                         {
 886                                                 cp.m_lateralFrictionInitialized = true;
 887                                         }
 888                                 }
 889
 890                         } else
 891                         {
 892                                 addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1);
 893
 894                                 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 895                                         addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2);
 896
 897                                 setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
 898                         }
 899
 900
 901
 902
 903                 }
 904         }
 905 }
 906
 907 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
 908 {
 909         BT_PROFILE("solveGroupCacheFriendlySetup");
 910         (void)stackAlloc;
 911         (void)debugDrawer;
 912
 913         m_maxOverrideNumSolverIterations = 0;
 914
 915 #ifdef BT_DEBUG
 916          //make sure that dynamic bodies exist for all (enabled) constraints
 917         for (int i=0;i<numConstraints;i++)
 918         {
 919                 btTypedConstraint* constraint = constraints[i];
 920                 if (constraint->isEnabled())
 921                 {
 922                         if (!constraint->getRigidBodyA().isStaticOrKinematicObject())
 923                         {
 924                                 bool found=false;
 925                                 for (int b=0;b<numBodies;b++)
 926                                 {
 927
 928                                         if (&constraint->getRigidBodyA()==bodies[b])
 929                                         {
 930                                                 found = true;
 931                                                 break;
 932                                         }
 933                                 }
 934                                 btAssert(found);
 935                         }
 936                         if (!constraint->getRigidBodyB().isStaticOrKinematicObject())
 937                         {
 938                                 bool found=false;
 939                                 for (int b=0;b<numBodies;b++)
 940                                 {
 941                                         if (&constraint->getRigidBodyB()==bodies[b])
 942                                         {
 943                                                 found = true;
 944                                                 break;
 945                                         }
 946                                 }
 947                                 btAssert(found);
 948                         }
 949                 }
 950         }
 951     //make sure that dynamic bodies exist for all contact manifolds
 952     for (int i=0;i<numManifolds;i++)
 953     {
 954         if (!manifoldPtr[i]->getBody0()->isStaticOrKinematicObject())
 955         {
 956             bool found=false;
 957             for (int b=0;b<numBodies;b++)
 958             {
 959
 960                 if (manifoldPtr[i]->getBody0()==bodies[b])
 961                 {
 962                     found = true;
 963                     break;
 964                 }
 965             }
 966             btAssert(found);
 967         }
 968         if (!manifoldPtr[i]->getBody1()->isStaticOrKinematicObject())
 969         {
 970             bool found=false;
 971             for (int b=0;b<numBodies;b++)
 972             {
 973                 if (manifoldPtr[i]->getBody1()==bodies[b])
 974                 {
 975                     found = true;
 976                     break;
 977                 }
 978             }
 979             btAssert(found);
 980         }
 981     }
 982 #endif //BT_DEBUG
 983
 984
 985         for (int i = 0; i < numBodies; i++)
 986         {
 987                 bodies[i]->setCompanionId(-1);
 988         }
 989
 990
 991         m_tmpSolverBodyPool.reserve(numBodies+1);
 992         m_tmpSolverBodyPool.resize(0);
 993
 994         btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
 995     initSolverBody(&fixedBody,0);
 996
 997         //convert all bodies
 998
 999         for (int i=0;i<numBodies;i++)
1000         {
1001                 int bodyId = getOrInitSolverBody(*bodies[i]);
1002                 btRigidBody* body = btRigidBody::upcast(bodies[i]);
1003                 if (body && body->getInvMass())
1004                 {
1005                         btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
1006                         btVector3 gyroForce (0,0,0);
1007                         if (body->getFlags()&BT_ENABLE_GYROPSCOPIC_FORCE)
1008                         {
1009                                 gyroForce = body->computeGyroscopicForce(infoGlobal.m_maxGyroscopicForce);
1010                         }
1011                         solverBody.m_linearVelocity += body->getTotalForce()*body->getInvMass()*infoGlobal.m_timeStep;
1012                         solverBody.m_angularVelocity += (body->getTotalTorque()-gyroForce)*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep;
1013                 }
1014         }
1015
1016         if (1)
1017         {
1018                 int j;
1019                 for (j=0;j<numConstraints;j++)
1020                 {
1021                         btTypedConstraint* constraint = constraints[j];
1022                         constraint->buildJacobian();
1023                         constraint->internalSetAppliedImpulse(0.0f);
1024                 }
1025         }
1026
1027         //btRigidBody* rb0=0,*rb1=0;
1028
1029         //if (1)
1030         {
1031                 {
1032
1033                         int totalNumRows = 0;
1034                         int i;
1035
1036                         m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints);
1037                         //calculate the total number of contraint rows
1038                         for (i=0;i<numConstraints;i++)
1039                         {
1040                                 btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
1041                                 btJointFeedback* fb = constraints[i]->getJointFeedback();
1042                                 if (fb)
1043                                 {
1044                                         fb->m_appliedForceBodyA.setZero();
1045                                         fb->m_appliedTorqueBodyA.setZero();
1046                                         fb->m_appliedForceBodyB.setZero();
1047                                         fb->m_appliedTorqueBodyB.setZero();
1048                                 }
1049
1050                                 if (constraints[i]->isEnabled())
1051                                 {
1052                                 }
1053                                 if (constraints[i]->isEnabled())
1054                                 {
1055                                         constraints[i]->getInfo1(&info1);
1056                                 } else
1057                                 {
1058                                         info1.m_numConstraintRows = 0;
1059                                         info1.nub = 0;
1060                                 }
1061                                 totalNumRows += info1.m_numConstraintRows;
1062                         }
1063                         m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows);
1064
1065
1066                         ///setup the btSolverConstraints
1067                         int currentRow = 0;
1068
1069                         for (i=0;i<numConstraints;i++)
1070                         {
1071                                 const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
1072
1073                                 if (info1.m_numConstraintRows)
1074                                 {
1075                                         btAssert(currentRow<totalNumRows);
1076
1077                                         btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
1078                                         btTypedConstraint* constraint = constraints[i];
1079                                         btRigidBody& rbA = constraint->getRigidBodyA();
1080                                         btRigidBody& rbB = constraint->getRigidBodyB();
1081
1082                     int solverBodyIdA = getOrInitSolverBody(rbA);
1083                     int solverBodyIdB = getOrInitSolverBody(rbB);
1084
1085                     btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
1086                     btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
1087
1088
1089
1090
1091                                         int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations;
1092                                         if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations)
1093                                                 m_maxOverrideNumSolverIterations = overrideNumSolverIterations;
1094
1095
1096                                         int j;
1097                                         for ( j=0;j<info1.m_numConstraintRows;j++)
1098                                         {
1099                                                 memset(&currentConstraintRow[j],0,sizeof(btSolverConstraint));
1100                                                 currentConstraintRow[j].m_lowerLimit = -SIMD_INFINITY;
1101                                                 currentConstraintRow[j].m_upperLimit = SIMD_INFINITY;
1102                                                 currentConstraintRow[j].m_appliedImpulse = 0.f;
1103                                                 currentConstraintRow[j].m_appliedPushImpulse = 0.f;
1104                                                 currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
1105                                                 currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
1106                                                 currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations;
1107                                         }
1108
1109                                         bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
1110                                         bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
1111                                         bodyAPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
1112                                         bodyAPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
1113                                         bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
1114                                         bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
1115                                         bodyBPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f);
1116                                         bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
1117
1118
1119                                         btTypedConstraint::btConstraintInfo2 info2;
1120                                         info2.fps = 1.f/infoGlobal.m_timeStep;
1121                                         info2.erp = infoGlobal.m_erp;
1122                                         info2.m_J1linearAxis = currentConstraintRow->m_contactNormal;
1123                                         info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
1124                                         info2.m_J2linearAxis = 0;
1125                                         info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
1126                                         info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this
1127                                         ///the size of btSolverConstraint needs be a multiple of btScalar
1128                             btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
1129                                         info2.m_constraintError = &currentConstraintRow->m_rhs;
1130                                         currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
1131                                         info2.m_damping = infoGlobal.m_damping;
1132                                         info2.cfm = &currentConstraintRow->m_cfm;
1133                                         info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
1134                                         info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
1135                                         info2.m_numIterations = infoGlobal.m_numIterations;
1136                                         constraints[i]->getInfo2(&info2);
1137
1138                                         ///finalize the constraint setup
1139                                         for ( j=0;j<info1.m_numConstraintRows;j++)
1140                                         {
1141                                                 btSolverConstraint& solverConstraint = currentConstraintRow[j];
1142
1143                                                 if (solverConstraint.m_upperLimit>=constraints[i]->getBreakingImpulseThreshold())
1144                                                 {
1145                                                         solverConstraint.m_upperLimit = constraints[i]->getBreakingImpulseThreshold();
1146                                                 }
1147
1148                                                 if (solverConstraint.m_lowerLimit<=-constraints[i]->getBreakingImpulseThreshold())
1149                                                 {
1150                                                         solverConstraint.m_lowerLimit = -constraints[i]->getBreakingImpulseThreshold();
1151                                                 }
1152
1153                                                 solverConstraint.m_originalContactPoint = constraint;
1154
1155                                                 {
1156                                                         const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
1157                                                         solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor();
1158                                                 }
1159                                                 {
1160                                                         const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
1161                                                         solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor();
1162                                                 }
1163
1164                                                 {
1165                                                         btVector3 iMJlA = solverConstraint.m_contactNormal*rbA.getInvMass();
1166                                                         btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;
1167                                                         btVector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal?
1168                                                         btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;
1169
1170                                                         btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal);
1171                                                         sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
1172                                                         sum += iMJlB.dot(solverConstraint.m_contactNormal);
1173                                                         sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
1174                                                         btScalar fsum = btFabs(sum);
1175                                                         btAssert(fsum > SIMD_EPSILON);
1176                                                         solverConstraint.m_jacDiagABInv = fsum>SIMD_EPSILON?btScalar(1.)/sum : 0.f;
1177                                                 }
1178
1179
1180                                                 ///fix rhs
1181                                                 ///todo: add force/torque accelerators
1182                                                 {
1183                                                         btScalar rel_vel;
1184                                                         btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity());
1185                                                         btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity());
1186
1187                                                         rel_vel = vel1Dotn+vel2Dotn;
1188
1189                                                         btScalar restitution = 0.f;
1190                                                         btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
1191                                                         btScalar        velocityError = restitution - rel_vel * info2.m_damping;
1192                                                         btScalar        penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
1193                                                         btScalar        velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
1194                                                         solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
1195                                                         solverConstraint.m_appliedImpulse = 0.f;
1196
1197                                                 }
1198                                         }
1199                                 }
1200                                 currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows;
1201                         }
1202                 }
1203
1204                 {
1205                         int i;
1206                         btPersistentManifold* manifold = 0;
1207 //                      btCollisionObject* colObj0=0,*colObj1=0;
1208
1209
1210                         for (i=0;i<numManifolds;i++)
1211                         {
1212                                 manifold = manifoldPtr[i];
1213                                 convertContact(manifold,infoGlobal);
1214                         }
1215                 }
1216         }
1217
1218 //      btContactSolverInfo info = infoGlobal;
1219
1220
1221         int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
1222         int numConstraintPool = m_tmpSolverContactConstraintPool.size();
1223         int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
1224
1225         ///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
1226         m_orderNonContactConstraintPool.resizeNoInitialize(numNonContactPool);
1227         if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1228                 m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool*2);
1229         else
1230                 m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool);
1231
1232         m_orderFrictionConstraintPool.resizeNoInitialize(numFrictionPool);
1233         {
1234                 int i;
1235                 for (i=0;i<numNonContactPool;i++)
1236                 {
1237                         m_orderNonContactConstraintPool[i] = i;
1238                 }
1239                 for (i=0;i<numConstraintPool;i++)
1240                 {
1241                         m_orderTmpConstraintPool[i] = i;
1242                 }
1243                 for (i=0;i<numFrictionPool;i++)
1244                 {
1245                         m_orderFrictionConstraintPool[i] = i;
1246                 }
1247         }
1248
1249         return 0.f;
1250
1251 }
1252
1253
1254 btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
1255 {
1256
1257         int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
1258         int numConstraintPool = m_tmpSolverContactConstraintPool.size();
1259         int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
1260
1261         if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
1262         {
1263                 if (1)                  // uncomment this for a bit less random ((iteration & 7) == 0)
1264                 {
1265
1266                         for (int j=0; j<numNonContactPool; ++j) {
1267                                 int tmp = m_orderNonContactConstraintPool[j];
1268                                 int swapi = btRandInt2(j+1);
1269                                 m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi];
1270                                 m_orderNonContactConstraintPool[swapi] = tmp;
1271                         }
1272
1273                         //contact/friction constraints are not solved more than
1274                         if (iteration< infoGlobal.m_numIterations)
1275                         {
1276                                 for (int j=0; j<numConstraintPool; ++j) {
1277                                         int tmp = m_orderTmpConstraintPool[j];
1278                                         int swapi = btRandInt2(j+1);
1279                                         m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
1280                                         m_orderTmpConstraintPool[swapi] = tmp;
1281                                 }
1282
1283                                 for (int j=0; j<numFrictionPool; ++j) {
1284                                         int tmp = m_orderFrictionConstraintPool[j];
1285                                         int swapi = btRandInt2(j+1);
1286                                         m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
1287                                         m_orderFrictionConstraintPool[swapi] = tmp;
1288                                 }
1289                         }
1290                 }
1291         }
1292
1293         if (infoGlobal.m_solverMode & SOLVER_SIMD)
1294         {
1295                 ///solve all joint constraints, using SIMD, if available
1296                 for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
1297                 {
1298                         btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
1299                         if (iteration < constraint.m_overrideNumSolverIterations)
1300                                 resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
1301                 }
1302
1303                 if (iteration< infoGlobal.m_numIterations)
1304                 {
1305                         for (int j=0;j<numConstraints;j++)
1306                         {
1307                 if (constraints[j]->isEnabled())
1308                 {
1309                     int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
1310                     int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
1311                     btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
1312                     btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
1313                     constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
1314                 }
1315                         }
1316
1317                         ///solve all contact constraints using SIMD, if available
1318                         if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
1319                         {
1320                                 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1321                                 int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1;
1322
1323                                 for (int c=0;c<numPoolConstraints;c++)
1324                                 {
1325                                         btScalar totalImpulse =0;
1326
1327                                         {
1328                                                 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
1329                                                 resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1330                                                 totalImpulse = solveManifold.m_appliedImpulse;
1331                                         }
1332                                         bool applyFriction = true;
1333                                         if (applyFriction)
1334                                         {
1335                                                 {
1336
1337                                                         btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier]];
1338
1339                                                         if (totalImpulse>btScalar(0))
1340                                                         {
1341                                                                 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
1342                                                                 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
1343
1344                                                                 resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1345                                                         }
1346                                                 }
1347
1348                                                 if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
1349                                                 {
1350
1351                                                         btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]];
1352
1353                                                         if (totalImpulse>btScalar(0))
1354                                                         {
1355                                                                 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
1356                                                                 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
1357
1358                                                                 resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1359                                                         }
1360                                                 }
1361                                         }
1362                                 }
1363
1364                         }
1365                         else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
1366                         {
1367                                 //solve the friction constraints after all contact constraints, don't interleave them
1368                                 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1369                                 int j;
1370
1371                                 for (j=0;j<numPoolConstraints;j++)
1372                                 {
1373                                         const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
1374                                         resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1375
1376                                 }
1377
1378
1379
1380                                 ///solve all friction constraints, using SIMD, if available
1381
1382                                 int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
1383                                 for (j=0;j<numFrictionPoolConstraints;j++)
1384                                 {
1385                                         btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
1386                                         btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1387
1388                                         if (totalImpulse>btScalar(0))
1389                                         {
1390                                                 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
1391                                                 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
1392
1393                                                 resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1394                                         }
1395                                 }
1396
1397
1398                                 int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
1399                                 for (j=0;j<numRollingFrictionPoolConstraints;j++)
1400                                 {
1401
1402                                         btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
1403                                         btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
1404                                         if (totalImpulse>btScalar(0))
1405                                         {
1406                                                 btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
1407                                                 if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
1408                                                         rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
1409
1410                                                 rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
1411                                                 rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
1412
1413                                                 resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
1414                                         }
1415                                 }
1416
1417
1418                         }
1419                 }
1420         } else
1421         {
1422                 //non-SIMD version
1423                 ///solve all joint constraints
1424                 for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
1425                 {
1426                         btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
1427                         if (iteration < constraint.m_overrideNumSolverIterations)
1428                                 resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
1429                 }
1430
1431                 if (iteration< infoGlobal.m_numIterations)
1432                 {
1433                         for (int j=0;j<numConstraints;j++)
1434                         {
1435                 if (constraints[j]->isEnabled())
1436                 {
1437                     int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
1438                     int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
1439                     btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
1440                     btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
1441                     constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
1442                 }
1443                         }
1444                         ///solve all contact constraints
1445                         int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1446                         for (int j=0;j<numPoolConstraints;j++)
1447                         {
1448                                 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
1449                                 resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1450                         }
1451                         ///solve all friction constraints
1452                         int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
1453                         for (int j=0;j<numFrictionPoolConstraints;j++)
1454                         {
1455                                 btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
1456                                 btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1457
1458                                 if (totalImpulse>btScalar(0))
1459                                 {
1460                                         solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
1461                                         solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
1462
1463                                         resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1464                                 }
1465                         }
1466
1467                         int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
1468                         for (int j=0;j<numRollingFrictionPoolConstraints;j++)
1469                         {
1470                                 btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
1471                                 btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
1472                                 if (totalImpulse>btScalar(0))
1473                                 {
1474                                         btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
1475                                         if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
1476                                                 rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
1477
1478                                         rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
1479                                         rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
1480
1481                                         resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
1482                                 }
1483                         }
1484                 }
1485         }
1486         return 0.f;
1487 }
1488
1489
1490 void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
1491 {
1492         int iteration;
1493         if (infoGlobal.m_splitImpulse)
1494         {
1495                 if (infoGlobal.m_solverMode & SOLVER_SIMD)
1496                 {
1497                         for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
1498                         {
1499                                 {
1500                                         int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1501                                         int j;
1502                                         for (j=0;j<numPoolConstraints;j++)
1503                                         {
1504                                                 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
1505
1506                                                 resolveSplitPenetrationSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1507                                         }
1508                                 }
1509                         }
1510                 }
1511                 else
1512                 {
1513                         for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
1514                         {
1515                                 {
1516                                         int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1517                                         int j;
1518                                         for (j=0;j<numPoolConstraints;j++)
1519                                         {
1520                                                 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
1521
1522                                                 resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
1523                                         }
1524                                 }
1525                         }
1526                 }
1527         }
1528 }
1529
1530 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
1531 {
1532         BT_PROFILE("solveGroupCacheFriendlyIterations");
1533
1534         {
1535                 ///this is a special step to resolve penetrations (just for contacts)
1536                 solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
1537
1538                 int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
1539
1540                 for ( int iteration = 0 ; iteration< maxIterations ; iteration++)
1541                 //for ( int iteration = maxIterations-1  ; iteration >= 0;iteration--)
1542                 {
1543                         solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
1544                 }
1545
1546         }
1547         return 0.f;
1548 }
1549
1550 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
1551 {
1552         int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
1553         int i,j;
1554
1555         if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
1556         {
1557                 for (j=0;j<numPoolConstraints;j++)
1558                 {
1559                         const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
1560                         btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
1561                         btAssert(pt);
1562                         pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
1563                 //      float f = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1564                         //      printf("pt->m_appliedImpulseLateral1 = %f\n", f);
1565                         pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
1566                         //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
1567                         if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
1568                         {
1569                                 pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
1570                         }
1571                         //do a callback here?
1572                 }
1573         }
1574
1575         numPoolConstraints = m_tmpSolverNonContactConstraintPool.size();
1576         for (j=0;j<numPoolConstraints;j++)
1577         {
1578                 const btSolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j];
1579                 btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint;
1580                 btJointFeedback* fb = constr->getJointFeedback();
1581                 if (fb)
1582                 {
1583                         fb->m_appliedForceBodyA += solverConstr.m_contactNormal*solverConstr.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep;
1584                         fb->m_appliedForceBodyB += -solverConstr.m_contactNormal*solverConstr.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep;
1585                         fb->m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep;
1586                         fb->m_appliedTorqueBodyB += -solverConstr.m_relpos1CrossNormal* constr->getRigidBodyB().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep;
1587
1588                 }
1589
1590                 constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse);
1591                 if (btFabs(solverConstr.m_appliedImpulse)>=constr->getBreakingImpulseThreshold())
1592                 {
1593                         constr->setEnabled(false);
1594                 }
1595         }
1596
1597
1598
1599         for ( i=0;i<m_tmpSolverBodyPool.size();i++)
1600         {
1601                 btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
1602                 if (body)
1603                 {
1604                         if (infoGlobal.m_splitImpulse)
1605                                 m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp);
1606                         else
1607                                 m_tmpSolverBodyPool[i].writebackVelocity();
1608
1609                         m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity);
1610                         m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity);
1611                         if (infoGlobal.m_splitImpulse)
1612                                 m_tmpSolverBodyPool[i].m_originalBody->setWorldTransform(m_tmpSolverBodyPool[i].m_worldTransform);
1613
1614                         m_tmpSolverBodyPool[i].m_originalBody->setCompanionId(-1);
1615                 }
1616         }
1617
1618         m_tmpSolverContactConstraintPool.resizeNoInitialize(0);
1619         m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0);
1620         m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0);
1621         m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0);
1622
1623         m_tmpSolverBodyPool.resizeNoInitialize(0);
1624         return 0.f;
1625 }
1626
1627
1628
1629 /// btSequentialImpulseConstraintSolver Sequentially applies impulses
1630 btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/)
1631 {
1632
1633         BT_PROFILE("solveGroup");
1634         //you need to provide at least some bodies
1635
1636         solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
1637
1638         solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
1639
1640         solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
1641
1642         return 0.f;
1643 }
1644
1645 void    btSequentialImpulseConstraintSolver::reset()
1646 {
1647         m_btSeed2 = 0;
1648 }
1649
1650