Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

Ignore:
Timestamp:
Apr 28, 2011, 7:15:14 AM (13 years ago)
Author:
rgrieder
Message:

Merged kicklib2 branch back to trunk (includes former branches ois_update, mac_osx and kicklib).

Notes for updating

Linux:
You don't need an extra package for CEGUILua and Tolua, it's already shipped with CEGUI.
However you do need to make sure that the OgreRenderer is installed too with CEGUI 0.7 (may be a separate package).
Also, Orxonox now recognises if you install the CgProgramManager (a separate package available on newer Ubuntu on Debian systems).

Windows:
Download the new dependency packages versioned 6.0 and use these. If you have problems with that or if you don't like the in game console problem mentioned below, you can download the new 4.3 version of the packages (only available for Visual Studio 2005/2008).

Key new features:

  • *Support for Mac OS X*
  • Visual Studio 2010 support
  • Bullet library update to 2.77
  • OIS library update to 1.3
  • Support for CEGUI 0.7 —> Support for Arch Linux and even SuSE
  • Improved install target
  • Compiles now with GCC 4.6
  • Ogre Cg Shader plugin activated for Linux if available
  • And of course lots of bug fixes

There are also some regressions:

  • No support for CEGUI 0.5, Ogre 1.4 and boost 1.35 - 1.39 any more
  • In game console is not working in main menu for CEGUI 0.7
  • Tolua (just the C lib, not the application) and CEGUILua libraries are no longer in our repository. —> You will need to get these as well when compiling Orxonox
  • And of course lots of new bugs we don't yet know about
File:
1 edited

Legend:

Unmodified
Added
Removed

  • code/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

    r5781 r8351  
    3535#include <string.h> //for memset
    3636
     37int             gNumSplitImpulseRecoveries = 0;
     38
    3739btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
    3840:m_btSeed2(0)
    … …  
    5658
    5759// Project Gauss Seidel or the equivalent Sequential Impulse
    58 void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
     60void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
    5961{
    6062#ifdef USE_SIMD
    … …  
    6365        __m128  upperLimit1 = _mm_set1_ps(c.m_upperLimit);
    6466        __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)));
    65         __m128 deltaVel1Dotn    =       _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128));
    66         __m128 deltaVel2Dotn    =       _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.m_deltaLinearVelocity.mVec128));
     67        __m128 deltaVel1Dotn    =       _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
     68        __m128 deltaVel2Dotn    =       _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128));
    6769        deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
    6870        deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
    … …  
    7779        deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) );
    7880        c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) );
    79         __m128  linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,_mm_set1_ps(body1.m_invMass));
    80         __m128  linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,_mm_set1_ps(body2.m_invMass));
     81        __m128  linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128);
     82        __m128  linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128);
    8183        __m128 impulseMagnitude = deltaImpulse;
    82         body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
    83         body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
    84         body2.m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
    85         body2.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body2.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
     84        body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
     85        body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
     86        body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
     87        body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
    8688#else
    8789        resolveSingleConstraintRowGeneric(body1,body2,c);
    … …  
    9092
    9193// Project Gauss Seidel or the equivalent Sequential Impulse
    92  void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
     94 void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
    9395{
    9496        btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
    95         const btScalar deltaVel1Dotn    =       c.m_contactNormal.dot(body1.m_deltaLinearVelocity)      + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity);
    96         const btScalar deltaVel2Dotn    =       -c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity);
    97 
    98         const btScalar delta_rel_vel    =       deltaVel1Dotn-deltaVel2Dotn;
     97        const btScalar deltaVel1Dotn    =       c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity())   + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
     98        const btScalar deltaVel2Dotn    =       -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity());
     99
     100//      const btScalar delta_rel_vel    =       deltaVel1Dotn-deltaVel2Dotn;
    99101        deltaImpulse    -=      deltaVel1Dotn*c.m_jacDiagABInv;
    100102        deltaImpulse    -=      deltaVel2Dotn*c.m_jacDiagABInv;
    … …  
    115117                c.m_appliedImpulse = sum;
    116118        }
    117         if (body1.m_invMass)
    118                 body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse);
    119         if (body2.m_invMass)
    120                 body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse);
    121 }
    122 
    123  void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
     119                body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
     120                body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
     121}
     122
     123 void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
    124124{
    125125#ifdef USE_SIMD
    … …  
    128128        __m128  upperLimit1 = _mm_set1_ps(c.m_upperLimit);
    129129        __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(_vmathVfDot3(c.m_contactNormal.mVec128,body1.m_deltaLinearVelocity.mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.m_deltaAngularVelocity.mVec128));
    131         __m128 deltaVel2Dotn    =       _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.m_deltaAngularVelocity.mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.m_deltaLinearVelocity.mVec128));
     130        __m128 deltaVel1Dotn    =       _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128));
     131        __m128 deltaVel2Dotn    =       _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),_vmathVfDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128));
    132132        deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
    133133        deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
    … …  
    139139        deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
    140140        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,_mm_set1_ps(body1.m_invMass));
    142         __m128  linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,_mm_set1_ps(body2.m_invMass));
     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);
    143143        __m128 impulseMagnitude = deltaImpulse;
    144         body1.m_deltaLinearVelocity.mVec128 = _mm_add_ps(body1.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude));
    145         body1.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body1.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
    146         body2.m_deltaLinearVelocity.mVec128 = _mm_sub_ps(body2.m_deltaLinearVelocity.mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
    147         body2.m_deltaAngularVelocity.mVec128 = _mm_add_ps(body2.m_deltaAngularVelocity.mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
     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));
    148148#else
    149149        resolveSingleConstraintRowLowerLimit(body1,body2,c);
    … …  
    152152
    153153// Project Gauss Seidel or the equivalent Sequential Impulse
    154  void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
     154 void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
    155155{
    156156        btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
    157         const btScalar deltaVel1Dotn    =       c.m_contactNormal.dot(body1.m_deltaLinearVelocity)      + c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity);
    158         const btScalar deltaVel2Dotn    =       -c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity);
     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());
    159159
    160160        deltaImpulse    -=      deltaVel1Dotn*c.m_jacDiagABInv;
    … …  
    170170                c.m_appliedImpulse = sum;
    171171        }
    172         if (body1.m_invMass)
    173                 body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse);
    174         if (body2.m_invMass)
    175                 body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse);
     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
     177void    btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly(
     178        btRigidBody& body1,
     179        btRigidBody& body2,
     180        const btSolverConstraint& c)
     181{
     182                if (c.m_rhsPenetration)
     183        {
     184                        gNumSplitImpulseRecoveries++;
     185                        btScalar deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm;
     186                        const btScalar deltaVel1Dotn    =       c.m_contactNormal.dot(body1.internalGetPushVelocity())  + c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity());
     187                        const btScalar deltaVel2Dotn    =       -c.m_contactNormal.dot(body2.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity());
     188
     189                        deltaImpulse    -=      deltaVel1Dotn*c.m_jacDiagABInv;
     190                        deltaImpulse    -=      deltaVel2Dotn*c.m_jacDiagABInv;
     191                        const btScalar sum = btScalar(c.m_appliedPushImpulse) + deltaImpulse;
     192                        if (sum < c.m_lowerLimit)
     193                        {
     194                                deltaImpulse = c.m_lowerLimit-c.m_appliedPushImpulse;
     195                                c.m_appliedPushImpulse = c.m_lowerLimit;
     196                        }
     197                        else
     198                        {
     199                                c.m_appliedPushImpulse = sum;
     200                        }
     201                        body1.internalApplyPushImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
     202                        body2.internalApplyPushImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
     203        }
     204}
     205
     206 void btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c)
     207{
     208#ifdef USE_SIMD
     209        if (!c.m_rhsPenetration)
     210                return;
     211
     212        gNumSplitImpulseRecoveries++;
     213
     214        __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedPushImpulse);
     215        __m128  lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
     216        __m128  upperLimit1 = _mm_set1_ps(c.m_upperLimit);
     217        __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhsPenetration), _mm_mul_ps(_mm_set1_ps(c.m_appliedPushImpulse),_mm_set1_ps(c.m_cfm)));
     218        __m128 deltaVel1Dotn    =       _mm_add_ps(_vmathVfDot3(c.m_contactNormal.mVec128,body1.internalGetPushVelocity().mVec128), _vmathVfDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128));
     219        __m128 deltaVel2Dotn    =       _mm_sub_ps(_vmathVfDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128),_vmathVfDot3((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_appliedImpulse = _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
    176239}
    177240
    … …  
    215278
    216279
    217 
     280#if 0
    218281void    btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
    219282{
    220283        btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0;
    221284
    222         solverBody->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
    223         solverBody->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
     285        solverBody->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
     286        solverBody->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
     287        solverBody->internalGetPushVelocity().setValue(0.f,0.f,0.f);
     288        solverBody->internalGetTurnVelocity().setValue(0.f,0.f,0.f);
    224289
    225290        if (rb)
    226291        {
    227                 solverBody->m_invMass = rb->getInvMass();
     292                solverBody->internalGetInvMass() = btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor();
    228293                solverBody->m_originalBody = rb;
    229294                solverBody->m_angularFactor = rb->getAngularFactor();
    230295        } else
    231296        {
    232                 solverBody->m_invMass = 0.f;
     297                solverBody->internalGetInvMass().setValue(0,0,0);
    233298                solverBody->m_originalBody = 0;
    234                 solverBody->m_angularFactor = 1.f;
    235         }
    236 }
    237 
    238 
    239 int             gNumSplitImpulseRecoveries = 0;
     299                solverBody->m_angularFactor.setValue(1,1,1);
     300        }
     301}
     302#endif
     303
     304
     305
     306
    240307
    241308btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution)
    … …  
    263330
    264331
    265 
    266 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)
     332void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
    267333{
    268334
    … …  
    271337        btRigidBody* body1=btRigidBody::upcast(colObj1);
    272338
    273         btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expand();
    274         memset(&solverConstraint,0xff,sizeof(btSolverConstraint));
    275339        solverConstraint.m_contactNormal = normalAxis;
    276340
    277         solverConstraint.m_solverBodyIdA = solverBodyIdA;
    278         solverConstraint.m_solverBodyIdB = solverBodyIdB;
    279         solverConstraint.m_frictionIndex = frictionIndex;
     341        solverConstraint.m_solverBodyA = body0 ? body0 : &getFixedBody();
     342        solverConstraint.m_solverBodyB = body1 ? body1 : &getFixedBody();
    280343
    281344        solverConstraint.m_friction = cp.m_combinedFriction;
    … …  
    283346
    284347        solverConstraint.m_appliedImpulse = 0.f;
    285         //      solverConstraint.m_appliedPushImpulse = 0.f;
     348        solverConstraint.m_appliedPushImpulse = 0.f;
    286349
    287350        {
    … …  
    338401                rel_vel = vel1Dotn+vel2Dotn;
    339402
    340                 btScalar positionalError = 0.f;
    341 
    342                 btSimdScalar velocityError =  - rel_vel;
     403//              btScalar positionalError = 0.f;
     404
     405                btSimdScalar velocityError =  desiredVelocity - rel_vel;
    343406                btSimdScalar    velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
    344407                solverConstraint.m_rhs = velocityImpulse;
    345                 solverConstraint.m_cfm = 0.f;
     408                solverConstraint.m_cfm = cfmSlip;
    346409                solverConstraint.m_lowerLimit = 0;
    347410                solverConstraint.m_upperLimit = 1e10f;
    348411        }
    349 
     412}
     413
     414
     415
     416btSolverConstraint&     btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
     417{
     418        btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
     419        solverConstraint.m_frictionIndex = frictionIndex;
     420        setupFrictionConstraint(solverConstraint, normalAxis, solverBodyA, solverBodyB, cp, rel_pos1, rel_pos2,
     421                                                        colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
    350422        return solverConstraint;
    351423}
    … …  
    353425int     btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body)
    354426{
     427#if 0
    355428        int solverBodyIdA = -1;
    356429
    … …  
    374447        }
    375448        return solverBodyIdA;
     449#endif
     450        return 0;
    376451}
    377452#include <stdio.h>
    378453
    379454
    380 
    381 void    btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
    382 {
    383         btCollisionObject* colObj0=0,*colObj1=0;
    384 
    385         colObj0 = (btCollisionObject*)manifold->getBody0();
    386         colObj1 = (btCollisionObject*)manifold->getBody1();
    387 
    388         int solverBodyIdA=-1;
    389         int solverBodyIdB=-1;
    390 
    391         if (manifold->getNumContacts())
    392         {
    393                 solverBodyIdA = getOrInitSolverBody(*colObj0);
    394                 solverBodyIdB = getOrInitSolverBody(*colObj1);
    395         }
    396 
    397         ///avoid collision response between two static objects
    398         if (!solverBodyIdA && !solverBodyIdB)
    399                 return;
    400 
    401         btVector3 rel_pos1;
    402         btVector3 rel_pos2;
    403         btScalar relaxation;
    404 
    405         for (int j=0;j<manifold->getNumContacts();j++)
    406         {
    407 
    408                 btManifoldPoint& cp = manifold->getContactPoint(j);
    409 
    410                 if (cp.getDistance() <= manifold->getContactProcessingThreshold())
    411                 {
     455void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint,
     456                                                                                                                                 btCollisionObject* colObj0, btCollisionObject* colObj1,
     457                                                                                                                                 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
     458                                                                                                                                 btVector3& vel, btScalar& rel_vel, btScalar& relaxation,
     459                                                                                                                                 btVector3& rel_pos1, btVector3& rel_pos2)
     460{
     461                        btRigidBody* rb0 = btRigidBody::upcast(colObj0);
     462                        btRigidBody* rb1 = btRigidBody::upcast(colObj1);
    412463
    413464                        const btVector3& pos1 = cp.getPositionWorldOnA();
    414465                        const btVector3& pos2 = cp.getPositionWorldOnB();
    415466
     467//                      btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
     468//                      btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
    416469                        rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
    417470                        rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
    418471
    419 
    420472                        relaxation = 1.f;
    421                         btScalar rel_vel;
    422                         btVector3 vel;
    423 
    424                         int frictionIndex = m_tmpSolverContactConstraintPool.size();
    425 
    426                         {
    427                                 btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expand();
    428                                 btRigidBody* rb0 = btRigidBody::upcast(colObj0);
    429                                 btRigidBody* rb1 = btRigidBody::upcast(colObj1);
    430 
    431                                 solverConstraint.m_solverBodyIdA = solverBodyIdA;
    432                                 solverConstraint.m_solverBodyIdB = solverBodyIdB;
    433 
    434                                 solverConstraint.m_originalContactPoint = &cp;
    435 
    436                                 btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
    437                                 solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
    438                                 btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);           
    439                                 solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
     473
     474                        btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
     475                        solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
     476                        btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);           
     477                        solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
     478
    440479                                {
    441480#ifdef COMPUTE_IMPULSE_DENOM
    … …  
    467506
    468507
    469                                 btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
    470                                 btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
    471 
    472                                 vel  = vel1 - vel2;
    473 
    474                                 rel_vel = cp.m_normalWorldOnB.dot(vel);
     508
     509
     510                        btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
     511                        btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
     512                        vel  = vel1 - vel2;
     513                        rel_vel = cp.m_normalWorldOnB.dot(vel);
    475514
    476515                                btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
    … …  
    499538                                        solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
    500539                                        if (rb0)
    501                                                 m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
     540                                                rb0->internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
    502541                                        if (rb1)
    503                                                 m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),-solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse);
     542                                                rb1->internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
    504543                                } else
    505544                                {
    … …  
    507546                                }
    508547
    509                                 //                                                      solverConstraint.m_appliedPushImpulse = 0.f;
     548                                solverConstraint.m_appliedPushImpulse = 0.f;
    510549
    511550                                {
    … …  
    523562                                        btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
    524563                                        btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
    525                                         solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
     564                                        if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
     565                                        {
     566                                                //combine position and velocity into rhs
     567                                                solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
     568                                                solverConstraint.m_rhsPenetration = 0.f;
     569                                        } else
     570                                        {
     571                                                //split position and velocity into rhs and m_rhsPenetration
     572                                                solverConstraint.m_rhs = velocityImpulse;
     573                                                solverConstraint.m_rhsPenetration = penetrationImpulse;
     574                                        }
    526575                                        solverConstraint.m_cfm = 0.f;
    527576                                        solverConstraint.m_lowerLimit = 0;
    … …  
    530579
    531580
    532                                 /////setup the friction constraints
    533 
    534 
    535 
    536                                 if (1)
    537                                 {
    538                                         solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
    539                                         if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
    540                                         {
    541                                                 cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
    542                                                 btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
    543                                                 if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
    544                                                 {
    545                                                         cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
    546                                                         applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
    547                                                         applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
    548                                                         addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
    549                                                         if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
    550                                                         {
    551                                                                 cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
    552                                                                 cp.m_lateralFrictionDir2.normalize();//??
    553                                                                 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
    554                                                                 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
    555                                                                 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
    556                                                         }
    557                                                         cp.m_lateralFrictionInitialized = true;
    558                                                 } else
    559                                                 {
    560                                                         //re-calculate friction direction every frame, todo: check if this is really needed
    561                                                         btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
    562                                                         applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
    563                                                         applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
    564 
    565                                                         addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
    566                                                         if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
    567                                                         {
    568                                                                 applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
    569                                                                 applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
    570                                                                 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
    571                                                         }
    572                                                         cp.m_lateralFrictionInitialized = true;
    573                                                 }
    574 
    575                                         } else
    576                                         {
    577                                                 addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
    578                                                 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
    579                                                         addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
    580                                         }
    581 
     581
     582
     583}
     584
     585
     586
     587void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint,
     588                                                                                                                                                btRigidBody* rb0, btRigidBody* rb1,
     589                                                                                                                                 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal)
     590{
    582591                                        if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
    583592                                        {
    … …  
    588597                                                                frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
    589598                                                                if (rb0)
    590                                                                         m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
     599                                                                        rb0->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
    591600                                                                if (rb1)
    592                                                                         m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),-frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
     601                                                                        rb1->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse);
    593602                                                        } else
    594603                                                        {
    … …  
    604613                                                                frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
    605614                                                                if (rb0)
    606                                                                         m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
     615                                                                        rb0->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
    607616                                                                if (rb1)
    608                                                                         m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);
     617                                                                        rb1->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse);
    609618                                                        } else
    610619                                                        {
    … …  
    622631                                                }
    623632                                        }
     633}
     634
     635
     636
     637
     638void    btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
     639{
     640        btCollisionObject* colObj0=0,*colObj1=0;
     641
     642        colObj0 = (btCollisionObject*)manifold->getBody0();
     643        colObj1 = (btCollisionObject*)manifold->getBody1();
     644
     645
     646        btRigidBody* solverBodyA = btRigidBody::upcast(colObj0);
     647        btRigidBody* solverBodyB = btRigidBody::upcast(colObj1);
     648
     649        ///avoid collision response between two static objects
     650        if ((!solverBodyA || !solverBodyA->getInvMass()) && (!solverBodyB || !solverBodyB->getInvMass()))
     651                return;
     652
     653        for (int j=0;j<manifold->getNumContacts();j++)
     654        {
     655
     656                btManifoldPoint& cp = manifold->getContactPoint(j);
     657
     658                if (cp.getDistance() <= manifold->getContactProcessingThreshold())
     659                {
     660                        btVector3 rel_pos1;
     661                        btVector3 rel_pos2;
     662                        btScalar relaxation;
     663                        btScalar rel_vel;
     664                        btVector3 vel;
     665
     666                        int frictionIndex = m_tmpSolverContactConstraintPool.size();
     667                        btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing();
     668                        btRigidBody* rb0 = btRigidBody::upcast(colObj0);
     669                        btRigidBody* rb1 = btRigidBody::upcast(colObj1);
     670                        solverConstraint.m_solverBodyA = rb0? rb0 : &getFixedBody();
     671                        solverConstraint.m_solverBodyB = rb1? rb1 : &getFixedBody();
     672                        solverConstraint.m_originalContactPoint = &cp;
     673
     674                        setupContactConstraint(solverConstraint, colObj0, colObj1, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2);
     675
     676//                      const btVector3& pos1 = cp.getPositionWorldOnA();
     677//                      const btVector3& pos2 = cp.getPositionWorldOnB();
     678
     679                        /////setup the friction constraints
     680
     681                        solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
     682
     683                        if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
     684                        {
     685                                cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
     686                                btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
     687                                if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
     688                                {
     689                                        cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
     690                                        if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
     691                                        {
     692                                                cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
     693                                                cp.m_lateralFrictionDir2.normalize();//??
     694                                                applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
     695                                                applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
     696                                                addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
     697                                        }
     698
     699                                        applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
     700                                        applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
     701                                        addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
     702                                        cp.m_lateralFrictionInitialized = true;
     703                                } else
     704                                {
     705                                        //re-calculate friction direction every frame, todo: check if this is really needed
     706                                        btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
     707                                        if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
     708                                        {
     709                                                applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
     710                                                applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
     711                                                addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
     712                                        }
     713
     714                                        applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
     715                                        applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
     716                                        addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
     717
     718                                        cp.m_lateralFrictionInitialized = true;
    624719                                }
    625                         }
    626 
    627 
    628                 }
    629         }
    630 }
    631 
    632 
    633 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /*bodies */,int /*numBodies */,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
     720
     721                        } else
     722                        {
     723                                addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1);
     724                                if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
     725                                        addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2);
     726                        }
     727                       
     728                        setFrictionConstraintImpulse( solverConstraint, rb0, rb1, cp, infoGlobal);
     729
     730                }
     731        }
     732}
     733
     734
     735btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
    634736{
    635737        BT_PROFILE("solveGroupCacheFriendlySetup");
    … …  
    644746        }
    645747
     748        if (infoGlobal.m_splitImpulse)
     749        {
     750                for (int i = 0; i < numBodies; i++)
     751                {
     752                        btRigidBody* body = btRigidBody::upcast(bodies[i]);
     753                        if (body)
     754                        {       
     755                                body->internalGetDeltaLinearVelocity().setZero();
     756                                body->internalGetDeltaAngularVelocity().setZero();
     757                                body->internalGetPushVelocity().setZero();
     758                                body->internalGetTurnVelocity().setZero();
     759                        }
     760                }
     761        }
     762        else
     763        {
     764                for (int i = 0; i < numBodies; i++)
     765                {
     766                        btRigidBody* body = btRigidBody::upcast(bodies[i]);
     767                        if (body)
     768                        {       
     769                                body->internalGetDeltaLinearVelocity().setZero();
     770                                body->internalGetDeltaAngularVelocity().setZero();
     771                        }
     772                }
     773        }
     774
    646775        if (1)
    647776        {
    … …  
    653782                }
    654783        }
    655 
    656         btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
    657         initSolverBody(&fixedBody,0);
    658 
    659784        //btRigidBody* rb0=0,*rb1=0;
    660785
    … …  
    665790                        int totalNumRows = 0;
    666791                        int i;
     792                       
     793                        m_tmpConstraintSizesPool.resize(numConstraints);
    667794                        //calculate the total number of contraint rows
    668795                        for (i=0;i<numConstraints;i++)
    669796                        {
    670 
    671                                 btTypedConstraint::btConstraintInfo1 info1;
     797                                btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
    672798                                constraints[i]->getInfo1(&info1);
    673799                                totalNumRows += info1.m_numConstraintRows;
    … …  
    675801                        m_tmpSolverNonContactConstraintPool.resize(totalNumRows);
    676802
    677                         btTypedConstraint::btConstraintInfo1 info1;
    678                         info1.m_numConstraintRows = 0;
    679 
    680 
     803                       
    681804                        ///setup the btSolverConstraints
    682805                        int currentRow = 0;
    683806
    684                         for (i=0;i<numConstraints;i++,currentRow+=info1.m_numConstraintRows)
     807                        for (i=0;i<numConstraints;i++)
    685808                        {
    686                                 constraints[i]->getInfo1(&info1);
     809                                const btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
     810                               
    687811                                if (info1.m_numConstraintRows)
    688812                                {
    … …  
    697821                                        btRigidBody& rbB = constraint->getRigidBodyB();
    698822
    699                                         int solverBodyIdA = getOrInitSolverBody(rbA);
    700                                         int solverBodyIdB = getOrInitSolverBody(rbB);
    701 
    702                                         btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
    703                                         btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
    704 
     823                                       
    705824                                        int j;
    706825                                        for ( j=0;j<info1.m_numConstraintRows;j++)
    … …  
    711830                                                currentConstraintRow[j].m_appliedImpulse = 0.f;
    712831                                                currentConstraintRow[j].m_appliedPushImpulse = 0.f;
    713                                                 currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
    714                                                 currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
     832                                                currentConstraintRow[j].m_solverBodyA = &rbA;
     833                                                currentConstraintRow[j].m_solverBodyB = &rbB;
    715834                                        }
    716835
    717                                         bodyAPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
    718                                         bodyAPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
    719                                         bodyBPtr->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
    720                                         bodyBPtr->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
     836                                        rbA.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
     837                                        rbA.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
     838                                        rbB.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f);
     839                                        rbB.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f);
    721840
    722841
    … …  
    733852                                        btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
    734853                                        info2.m_constraintError = &currentConstraintRow->m_rhs;
     854                                        currentConstraintRow->m_cfm = infoGlobal.m_globalCfm;
     855                                        info2.m_damping = infoGlobal.m_damping;
    735856                                        info2.cfm = &currentConstraintRow->m_cfm;
    736857                                        info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
    737858                                        info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
     859                                        info2.m_numIterations = infoGlobal.m_numIterations;
    738860                                        constraints[i]->getInfo2(&info2);
    739861
    … …  
    742864                                        {
    743865                                                btSolverConstraint& solverConstraint = currentConstraintRow[j];
     866                                                solverConstraint.m_originalContactPoint = constraint;
    744867
    745868                                                {
    … …  
    778901                                                        btScalar restitution = 0.f;
    779902                                                        btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
    780                                                         btScalar        velocityError = restitution - rel_vel;// * damping;
     903                                                        btScalar        velocityError = restitution - rel_vel * info2.m_damping;
    781904                                                        btScalar        penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
    782905                                                        btScalar        velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
    … …  
    787910                                        }
    788911                                }
     912                                currentRow+=m_tmpConstraintSizesPool[i].m_numConstraintRows;
    789913                        }
    790914                }
    … …  
    793917                        int i;
    794918                        btPersistentManifold* manifold = 0;
    795                         btCollisionObject* colObj0=0,*colObj1=0;
     919//                      btCollisionObject* colObj0=0,*colObj1=0;
    796920
    797921
    … …  
    830954}
    831955
    832 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
    833 {
    834         BT_PROFILE("solveGroupCacheFriendlyIterations");
     956btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
     957{
    835958
    836959        int numConstraintPool = m_tmpSolverContactConstraintPool.size();
    837960        int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
    838961
     962        int j;
     963
     964        if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
     965        {
     966                if ((iteration & 7) == 0) {
     967                        for (j=0; j<numConstraintPool; ++j) {
     968                                int tmp = m_orderTmpConstraintPool[j];
     969                                int swapi = btRandInt2(j+1);
     970                                m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
     971                                m_orderTmpConstraintPool[swapi] = tmp;
     972                        }
     973
     974                        for (j=0; j<numFrictionPool; ++j) {
     975                                int tmp = m_orderFrictionConstraintPool[j];
     976                                int swapi = btRandInt2(j+1);
     977                                m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
     978                                m_orderFrictionConstraintPool[swapi] = tmp;
     979                        }
     980                }
     981        }
     982
     983        if (infoGlobal.m_solverMode & SOLVER_SIMD)
     984        {
     985                ///solve all joint constraints, using SIMD, if available
     986                for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
     987                {
     988                        btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
     989                        resolveSingleConstraintRowGenericSIMD(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint);
     990                }
     991
     992                for (j=0;j<numConstraints;j++)
     993                {
     994                        constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
     995                }
     996
     997                ///solve all contact constraints using SIMD, if available
     998                int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
     999                for (j=0;j<numPoolConstraints;j++)
     1000                {
     1001                        const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
     1002                        resolveSingleConstraintRowLowerLimitSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
     1003
     1004                }
     1005                ///solve all friction constraints, using SIMD, if available
     1006                int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
     1007                for (j=0;j<numFrictionPoolConstraints;j++)
     1008                {
     1009                        btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
     1010                        btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
     1011
     1012                        if (totalImpulse>btScalar(0))
     1013                        {
     1014                                solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
     1015                                solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
     1016
     1017                                resolveSingleConstraintRowGenericSIMD(*solveManifold.m_solverBodyA,     *solveManifold.m_solverBodyB,solveManifold);
     1018                        }
     1019                }
     1020        } else
     1021        {
     1022
     1023                ///solve all joint constraints
     1024                for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
     1025                {
     1026                        btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
     1027                        resolveSingleConstraintRowGeneric(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint);
     1028                }
     1029
     1030                for (j=0;j<numConstraints;j++)
     1031                {
     1032                        constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
     1033                }
     1034                ///solve all contact constraints
     1035                int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
     1036                for (j=0;j<numPoolConstraints;j++)
     1037                {
     1038                        const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
     1039                        resolveSingleConstraintRowLowerLimit(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
     1040                }
     1041                ///solve all friction constraints
     1042                int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
     1043                for (j=0;j<numFrictionPoolConstraints;j++)
     1044                {
     1045                        btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
     1046                        btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
     1047
     1048                        if (totalImpulse>btScalar(0))
     1049                        {
     1050                                solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
     1051                                solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
     1052
     1053                                resolveSingleConstraintRowGeneric(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
     1054                        }
     1055                }
     1056        }
     1057        return 0.f;
     1058}
     1059
     1060
     1061void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
     1062{
     1063        int iteration;
     1064        if (infoGlobal.m_splitImpulse)
     1065        {
     1066                if (infoGlobal.m_solverMode & SOLVER_SIMD)
     1067                {
     1068                        for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
     1069                        {
     1070                                {
     1071                                        int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
     1072                                        int j;
     1073                                        for (j=0;j<numPoolConstraints;j++)
     1074                                        {
     1075                                                const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
     1076
     1077                                                resolveSplitPenetrationSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
     1078                                        }
     1079                                }
     1080                        }
     1081                }
     1082                else
     1083                {
     1084                        for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
     1085                        {
     1086                                {
     1087                                        int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
     1088                                        int j;
     1089                                        for (j=0;j<numPoolConstraints;j++)
     1090                                        {
     1091                                                const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
     1092
     1093                                                resolveSplitPenetrationImpulseCacheFriendly(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold);
     1094                                        }
     1095                                }
     1096                        }
     1097                }
     1098        }
     1099}
     1100
     1101btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
     1102{
     1103        BT_PROFILE("solveGroupCacheFriendlyIterations");
     1104
     1105       
    8391106        //should traverse the contacts random order...
    8401107        int iteration;
    … …  
    8421109                for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
    8431110                {                       
    844 
    845                         int j;
    846                         if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
    847                         {
    848                                 if ((iteration & 7) == 0) {
    849                                         for (j=0; j<numConstraintPool; ++j) {
    850                                                 int tmp = m_orderTmpConstraintPool[j];
    851                                                 int swapi = btRandInt2(j+1);
    852                                                 m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
    853                                                 m_orderTmpConstraintPool[swapi] = tmp;
    854                                         }
    855 
    856                                         for (j=0; j<numFrictionPool; ++j) {
    857                                                 int tmp = m_orderFrictionConstraintPool[j];
    858                                                 int swapi = btRandInt2(j+1);
    859                                                 m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
    860                                                 m_orderFrictionConstraintPool[swapi] = tmp;
    861                                         }
    862                                 }
    863                         }
    864 
    865                         if (infoGlobal.m_solverMode & SOLVER_SIMD)
    866                         {
    867                                 ///solve all joint constraints, using SIMD, if available
    868                                 for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
    869                                 {
    870                                         btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
    871                                         resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
    872                                 }
    873 
    874                                 for (j=0;j<numConstraints;j++)
    875                                 {
    876                                         int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
    877                                         int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
    878                                         btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
    879                                         btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
    880                                         constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
    881                                 }
    882 
    883                                 ///solve all contact constraints using SIMD, if available
    884                                 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
    885                                 for (j=0;j<numPoolConstraints;j++)
    886                                 {
    887                                         const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
    888                                         resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
    889 
    890                                 }
    891                                 ///solve all friction constraints, using SIMD, if available
    892                                 int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
    893                                 for (j=0;j<numFrictionPoolConstraints;j++)
    894                                 {
    895                                         btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
    896                                         btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
    897 
    898                                         if (totalImpulse>btScalar(0))
    899                                         {
    900                                                 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
    901                                                 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
    902 
    903                                                 resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],       m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
    904                                         }
    905                                 }
    906                         } else
    907                         {
    908 
    909                                 ///solve all joint constraints
    910                                 for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
    911                                 {
    912                                         btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
    913                                         resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
    914                                 }
    915 
    916                                 for (j=0;j<numConstraints;j++)
    917                                 {
    918                                         int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
    919                                         int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
    920                                         btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
    921                                         btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
    922 
    923                                         constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
    924                                 }
    925 
    926                                 ///solve all contact constraints
    927                                 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
    928                                 for (j=0;j<numPoolConstraints;j++)
    929                                 {
    930                                         const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
    931                                         resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
    932                                 }
    933                                 ///solve all friction constraints
    934                                 int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
    935                                 for (j=0;j<numFrictionPoolConstraints;j++)
    936                                 {
    937                                         btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
    938                                         btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
    939 
    940                                         if (totalImpulse>btScalar(0))
    941                                         {
    942                                                 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
    943                                                 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
    944 
    945                                                 resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],                                                   m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
    946                                         }
    947                                 }
    948                         }
    949 
    950 
    951 
    952                 }
     1111                        solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
     1112                }
     1113               
     1114                solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
    9531115        }
    9541116        return 0.f;
    9551117}
    9561118
    957 
    958 
    959 /// btSequentialImpulseConstraintSolver Sequentially applies impulses
    960 btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/)
    961 {
    962 
    963        
    964 
    965         BT_PROFILE("solveGroup");
    966         //we only implement SOLVER_CACHE_FRIENDLY now
    967         //you need to provide at least some bodies
    968         btAssert(bodies);
    969         btAssert(numBodies);
    970 
    971         int i;
    972 
    973         solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
    974         solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
    975 
     1119btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies ,int numBodies,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** /*constraints*/,int /* numConstraints*/,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
     1120{
    9761121        int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
    977         int j;
     1122        int i,j;
    9781123
    9791124        for (j=0;j<numPoolConstraints;j++)
    … …  
    9931138        }
    9941139
     1140        numPoolConstraints = m_tmpSolverNonContactConstraintPool.size();
     1141        for (j=0;j<numPoolConstraints;j++)
     1142        {
     1143                const btSolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j];
     1144                btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint;
     1145                btScalar sum = constr->internalGetAppliedImpulse();
     1146                sum += solverConstr.m_appliedImpulse;
     1147                constr->internalSetAppliedImpulse(sum);
     1148        }
     1149
     1150
    9951151        if (infoGlobal.m_splitImpulse)
    9961152        {               
    997                 for ( i=0;i<m_tmpSolverBodyPool.size();i++)
    998                 {
    999                         m_tmpSolverBodyPool[i].writebackVelocity(infoGlobal.m_timeStep);
     1153                for ( i=0;i<numBodies;i++)
     1154                {
     1155                        btRigidBody* body = btRigidBody::upcast(bodies[i]);
     1156                        if (body)
     1157                                body->internalWritebackVelocity(infoGlobal.m_timeStep);
    10001158                }
    10011159        } else
    10021160        {
    1003                 for ( i=0;i<m_tmpSolverBodyPool.size();i++)
    1004                 {
    1005                         m_tmpSolverBodyPool[i].writebackVelocity();
    1006                 }
    1007         }
    1008 
    1009 
    1010         m_tmpSolverBodyPool.resize(0);
     1161                for ( i=0;i<numBodies;i++)
     1162                {
     1163                        btRigidBody* body = btRigidBody::upcast(bodies[i]);
     1164                        if (body)
     1165                                body->internalWritebackVelocity();
     1166                }
     1167        }
     1168
     1169
    10111170        m_tmpSolverContactConstraintPool.resize(0);
    10121171        m_tmpSolverNonContactConstraintPool.resize(0);
    … …  
    10181177
    10191178
    1020 
    1021 
    1022 
    1023 
    1024 
     1179/// btSequentialImpulseConstraintSolver Sequentially applies impulses
     1180btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/)
     1181{
     1182
     1183        BT_PROFILE("solveGroup");
     1184        //you need to provide at least some bodies
     1185        btAssert(bodies);
     1186        btAssert(numBodies);
     1187
     1188        solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
     1189
     1190        solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
     1191
     1192        solveGroupCacheFriendlyFinish(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
     1193       
     1194        return 0.f;
     1195}
    10251196
    10261197void    btSequentialImpulseConstraintSolver::reset()
    … …  
    10291200}
    10301201
    1031 
     1202btRigidBody& btSequentialImpulseConstraintSolver::getFixedBody()
     1203{
     1204        static btRigidBody s_fixed(0, 0,0);
     1205        s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
     1206        return s_fixed;
     1207}
     1208
Note: See TracChangeset for help on using the changeset viewer.