Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

Ignore:
Timestamp:
Mar 31, 2009, 8:05:51 PM (15 years ago)
Author:
rgrieder
Message:

Update from Bullet 2.73 to 2.74.

File:
1 edited

Legend:

Unmodified
Added
Removed

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

    r2662 r2882  
    1616//#define COMPUTE_IMPULSE_DENOM 1
    1717//It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
    18 //#define FORCE_REFESH_CONTACT_MANIFOLDS 1
    1918
    2019#include "btSequentialImpulseConstraintSolver.h"
    … …  
    3332#include "btSolverBody.h"
    3433#include "btSolverConstraint.h"
    35 
    36 
    3734#include "LinearMath/btAlignedObjectArray.h"
    38 
    39 
    40 int totalCpd = 0;
    41 
    42 int     gTotalContactPoints = 0;
    43 
    44 struct  btOrderIndex
    45 {
    46         int     m_manifoldIndex;
    47         int     m_pointIndex;
    48 };
    49 
    50 
    51 
    52 #define SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS 16384
    53 static btOrderIndex     gOrder[SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS];
     35#include <string.h> //for memset
     36
     37btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
     38:m_btSeed2(0)
     39{
     40
     41}
     42
     43btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
     44{
     45}
     46
     47#ifdef USE_SIMD
     48#include <emmintrin.h>
     49#define vec_splat(x, e) _mm_shuffle_ps(x, x, _MM_SHUFFLE(e,e,e,e))
     50static inline __m128 _vmathVfDot3( __m128 vec0, __m128 vec1 )
     51{
     52        __m128 result = _mm_mul_ps( vec0, vec1);
     53        return _mm_add_ps( vec_splat( result, 0 ), _mm_add_ps( vec_splat( result, 1 ), vec_splat( result, 2 ) ) );
     54}
     55#endif//USE_SIMD
     56
     57// Project Gauss Seidel or the equivalent Sequential Impulse
     58void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
     59{
     60#ifdef USE_SIMD
     61        __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
     62        __m128  lowerLimit1 = _mm_set1_ps(c.m_lowerLimit);
     63        __m128  upperLimit1 = _mm_set1_ps(c.m_upperLimit);
     64        __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        deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
     68        deltaImpulse    =       _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv)));
     69        btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse);
     70        btSimdScalar resultLowerLess,resultUpperLess;
     71        resultLowerLess = _mm_cmplt_ps(sum,lowerLimit1);
     72        resultUpperLess = _mm_cmplt_ps(sum,upperLimit1);
     73        __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp);
     74        deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) );
     75        c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) );
     76        __m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp);
     77        deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) );
     78        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 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));
     86#else
     87        resolveSingleConstraintRowGeneric(body1,body2,c);
     88#endif
     89}
     90
     91// Project Gauss Seidel or the equivalent Sequential Impulse
     92 void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
     93{
     94        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;
     99        deltaImpulse    -=      deltaVel1Dotn*c.m_jacDiagABInv;
     100        deltaImpulse    -=      deltaVel2Dotn*c.m_jacDiagABInv;
     101
     102        const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
     103        if (sum < c.m_lowerLimit)
     104        {
     105                deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
     106                c.m_appliedImpulse = c.m_lowerLimit;
     107        }
     108        else if (sum > c.m_upperLimit)
     109        {
     110                deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
     111                c.m_appliedImpulse = c.m_upperLimit;
     112        }
     113        else
     114        {
     115                c.m_appliedImpulse = sum;
     116        }
     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)
     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(_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));
     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,_mm_set1_ps(body1.m_invMass));
     142        __m128  linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,_mm_set1_ps(body2.m_invMass));
     143        __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));
     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.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);
     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        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);
     176}
     177
    54178
    55179
    56180unsigned long btSequentialImpulseConstraintSolver::btRand2()
    57181{
    58   m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
    59   return m_btSeed2;
     182        m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
     183        return m_btSeed2;
    60184}
    61185
    … …  
    65189int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
    66190{
    67   // seems good; xor-fold and modulus
    68   const unsigned long un = static_cast<unsigned long>(n);
    69   unsigned long r = btRand2();
    70 
    71   // note: probably more aggressive than it needs to be -- might be
    72   //       able to get away without one or two of the innermost branches.
    73   if (un <= 0x00010000UL) {
    74     r ^= (r >> 16);
    75     if (un <= 0x00000100UL) {
    76       r ^= (r >> 8);
    77       if (un <= 0x00000010UL) {
    78         r ^= (r >> 4);
    79         if (un <= 0x00000004UL) {
    80           r ^= (r >> 2);
    81           if (un <= 0x00000002UL) {
    82             r ^= (r >> 1);
    83           }
    84         }
    85      }
    86     }
    87    }
    88 
    89   return (int) (r % un);
    90 }
    91 
    92 
    93 
    94 
    95 bool  MyContactDestroyedCallback(void* userPersistentData);
    96 bool  MyContactDestroyedCallback(void* userPersistentData)
    97 {
    98         assert (userPersistentData);
    99         btConstraintPersistentData* cpd = (btConstraintPersistentData*)userPersistentData;
    100         btAlignedFree(cpd);
    101         totalCpd--;
    102         //printf("totalCpd = %i. DELETED Ptr %x\n",totalCpd,userPersistentData);
    103         return true;
    104 }
    105 
    106 
    107 
    108 btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
    109 :m_btSeed2(0)
    110 {
    111         gContactDestroyedCallback = &MyContactDestroyedCallback;
    112 
    113         //initialize default friction/contact funcs
    114         int i,j;
    115         for (i=0;i<MAX_CONTACT_SOLVER_TYPES;i++)
    116                 for (j=0;j<MAX_CONTACT_SOLVER_TYPES;j++)
    117                 {
    118 
    119                         m_contactDispatch[i][j] = resolveSingleCollision;
    120                         m_frictionDispatch[i][j] = resolveSingleFriction;
    121                 }
    122 }
    123 
    124 btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
    125 {
    126 
    127 }
    128 
    129 void    initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
    130 void    initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
    131 {
    132         btRigidBody* rb = btRigidBody::upcast(collisionObject);
     191        // seems good; xor-fold and modulus
     192        const unsigned long un = static_cast<unsigned long>(n);
     193        unsigned long r = btRand2();
     194
     195        // note: probably more aggressive than it needs to be -- might be
     196        //       able to get away without one or two of the innermost branches.
     197        if (un <= 0x00010000UL) {
     198                r ^= (r >> 16);
     199                if (un <= 0x00000100UL) {
     200                        r ^= (r >> 8);
     201                        if (un <= 0x00000010UL) {
     202                                r ^= (r >> 4);
     203                                if (un <= 0x00000004UL) {
     204                                        r ^= (r >> 2);
     205                                        if (un <= 0x00000002UL) {
     206                                                r ^= (r >> 1);
     207                                        }
     208                                }
     209                        }
     210                }
     211        }
     212
     213        return (int) (r % un);
     214}
     215
     216
     217
     218void    btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
     219{
     220        btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0;
     221
     222        solverBody->m_deltaLinearVelocity.setValue(0.f,0.f,0.f);
     223        solverBody->m_deltaAngularVelocity.setValue(0.f,0.f,0.f);
     224
    133225        if (rb)
    134226        {
    135                 solverBody->m_angularVelocity = rb->getAngularVelocity() ;
    136                 solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin();
    137                 solverBody->m_friction = collisionObject->getFriction();
    138227                solverBody->m_invMass = rb->getInvMass();
    139                 solverBody->m_linearVelocity = rb->getLinearVelocity();
    140228                solverBody->m_originalBody = rb;
    141229                solverBody->m_angularFactor = rb->getAngularFactor();
    142230        } else
    143231        {
    144                 solverBody->m_angularVelocity.setValue(0,0,0);
    145                 solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin();
    146                 solverBody->m_friction = collisionObject->getFriction();
    147232                solverBody->m_invMass = 0.f;
    148                 solverBody->m_linearVelocity.setValue(0,0,0);
    149233                solverBody->m_originalBody = 0;
    150234                solverBody->m_angularFactor = 1.f;
    151235        }
    152         solverBody->m_pushVelocity.setValue(0.f,0.f,0.f);
    153         solverBody->m_turnVelocity.setValue(0.f,0.f,0.f);
    154236}
    155237
    … …  
    157239int             gNumSplitImpulseRecoveries = 0;
    158240
    159 btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
    160 btScalar restitutionCurve(btScalar rel_vel, btScalar restitution)
     241btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution)
    161242{
    162243        btScalar rest = restitution * -rel_vel;
    … …  
    165246
    166247
    167 void    resolveSplitPenetrationImpulseCacheFriendly(
    168         btSolverBody& body1,
    169         btSolverBody& body2,
    170         const btSolverConstraint& contactConstraint,
    171         const btContactSolverInfo& solverInfo);
    172 
    173 //SIMD_FORCE_INLINE
    174 void    resolveSplitPenetrationImpulseCacheFriendly(
    175         btSolverBody& body1,
    176         btSolverBody& body2,
    177         const btSolverConstraint& contactConstraint,
    178         const btContactSolverInfo& solverInfo)
    179 {
    180         (void)solverInfo;
    181 
    182                 if (contactConstraint.m_penetration < solverInfo.m_splitImpulsePenetrationThreshold)
    183         {
    184 
    185                                 gNumSplitImpulseRecoveries++;
    186                 btScalar normalImpulse;
    187 
    188                 //  Optimized version of projected relative velocity, use precomputed cross products with normal
    189                 //      body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
    190                 //      body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
    191                 //      btVector3 vel = vel1 - vel2;
    192                 //      btScalar  rel_vel = contactConstraint.m_contactNormal.dot(vel);
    193 
    194                 btScalar rel_vel;
    195                 btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_pushVelocity)
    196                 + contactConstraint.m_relpos1CrossNormal.dot(body1.m_turnVelocity);
    197                 btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_pushVelocity)
    198                 + contactConstraint.m_relpos2CrossNormal.dot(body2.m_turnVelocity);
    199 
    200                 rel_vel = vel1Dotn-vel2Dotn;
    201 
    202 
    203                                 btScalar positionalError = -contactConstraint.m_penetration * solverInfo.m_erp2/solverInfo.m_timeStep;
    204                 //      btScalar positionalError = contactConstraint.m_penetration;
    205 
    206                 btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;
    207 
    208                 btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
    209                 btScalar        velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
    210                 normalImpulse = penetrationImpulse+velocityImpulse;
    211 
    212                 // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
    213                 btScalar oldNormalImpulse = contactConstraint.m_appliedPushImpulse;
    214                 btScalar sum = oldNormalImpulse + normalImpulse;
    215                 contactConstraint.m_appliedPushImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
    216 
    217                 normalImpulse = contactConstraint.m_appliedPushImpulse - oldNormalImpulse;
    218 
    219                                 body1.internalApplyPushImpulse(contactConstraint.m_contactNormal*body1.m_invMass, contactConstraint.m_angularComponentA,normalImpulse);
    220                
    221                                 body2.internalApplyPushImpulse(contactConstraint.m_contactNormal*body2.m_invMass, contactConstraint.m_angularComponentB,-normalImpulse);
    222                
    223         }
    224 
    225 }
    226 
    227 
    228 //velocity + friction
    229 //response  between two dynamic objects with friction
    230 
    231 btScalar resolveSingleCollisionCombinedCacheFriendly(
    232         btSolverBody& body1,
    233         btSolverBody& body2,
    234         const btSolverConstraint& contactConstraint,
    235         const btContactSolverInfo& solverInfo);
    236 
    237 //SIMD_FORCE_INLINE
    238 btScalar resolveSingleCollisionCombinedCacheFriendly(
    239         btSolverBody& body1,
    240         btSolverBody& body2,
    241         const btSolverConstraint& contactConstraint,
    242         const btContactSolverInfo& solverInfo)
    243 {
    244         (void)solverInfo;
    245 
    246         btScalar normalImpulse;
    247 
    248         {
    249 
    250                
    251                 //  Optimized version of projected relative velocity, use precomputed cross products with normal
    252                 //      body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
    253                 //      body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
    254                 //      btVector3 vel = vel1 - vel2;
    255                 //      btScalar  rel_vel = contactConstraint.m_contactNormal.dot(vel);
    256 
    257                 btScalar rel_vel;
    258                 btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity)
    259                                         + contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
    260                 btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity)
    261                                         + contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);
    262 
    263                 rel_vel = vel1Dotn-vel2Dotn;
    264 
    265                 btScalar positionalError = 0.f;
    266                 if (!solverInfo.m_splitImpulse || (contactConstraint.m_penetration > solverInfo.m_splitImpulsePenetrationThreshold))
    267                 {
    268                         positionalError = -contactConstraint.m_penetration * solverInfo.m_erp/solverInfo.m_timeStep;
    269                 }
    270 
    271                 btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;
    272 
    273                 btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
    274                 btScalar        velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
    275                 normalImpulse = penetrationImpulse+velocityImpulse;
    276                
    277                
    278                 // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
    279                 btScalar oldNormalImpulse = contactConstraint.m_appliedImpulse;
    280                 btScalar sum = oldNormalImpulse + normalImpulse;
    281                 contactConstraint.m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
    282 
    283                 normalImpulse = contactConstraint.m_appliedImpulse - oldNormalImpulse;
    284 
    285                 body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,
    286                                 contactConstraint.m_angularComponentA,normalImpulse);
    287                
    288                 body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,
    289                                 contactConstraint.m_angularComponentB,-normalImpulse);
    290         }
    291 
    292         return normalImpulse;
    293 }
    294 
    295 
    296 #ifndef NO_FRICTION_TANGENTIALS
    297 
    298 btScalar resolveSingleFrictionCacheFriendly(
    299         btSolverBody& body1,
    300         btSolverBody& body2,
    301         const btSolverConstraint& contactConstraint,
    302         const btContactSolverInfo& solverInfo,
    303         btScalar appliedNormalImpulse);
    304 
    305 //SIMD_FORCE_INLINE
    306 btScalar resolveSingleFrictionCacheFriendly(
    307         btSolverBody& body1,
    308         btSolverBody& body2,
    309         const btSolverConstraint& contactConstraint,
    310         const btContactSolverInfo& solverInfo,
    311         btScalar appliedNormalImpulse)
    312 {
    313         (void)solverInfo;
    314 
    315        
    316         const btScalar combinedFriction = contactConstraint.m_friction;
    317        
    318         const btScalar limit = appliedNormalImpulse * combinedFriction;
    319        
    320         if (appliedNormalImpulse>btScalar(0.))
    321         //friction
    322         {
    323                
    324                 btScalar j1;
    325                 {
    326 
    327                         btScalar rel_vel;
    328                         const btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity)
    329                                                 + contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
    330                         const btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity)
    331                                 + contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);
    332                         rel_vel = vel1Dotn-vel2Dotn;
    333 
    334                         // calculate j that moves us to zero relative velocity
    335                         j1 = -rel_vel * contactConstraint.m_jacDiagABInv;
    336 #define CLAMP_ACCUMULATED_FRICTION_IMPULSE 1
    337 #ifdef CLAMP_ACCUMULATED_FRICTION_IMPULSE
    338                         btScalar oldTangentImpulse = contactConstraint.m_appliedImpulse;
    339                         contactConstraint.m_appliedImpulse = oldTangentImpulse + j1;
    340                        
    341                         if (limit < contactConstraint.m_appliedImpulse)
    342                         {
    343                                 contactConstraint.m_appliedImpulse = limit;
    344                         } else
    345                         {
    346                                 if (contactConstraint.m_appliedImpulse < -limit)
    347                                         contactConstraint.m_appliedImpulse = -limit;
    348                         }
    349                         j1 = contactConstraint.m_appliedImpulse - oldTangentImpulse;
    350 #else
    351                         if (limit < j1)
    352                         {
    353                                 j1 = limit;
    354                         } else
    355                         {
    356                                 if (j1 < -limit)
    357                                         j1 = -limit;
    358                         }
    359 
    360 #endif //CLAMP_ACCUMULATED_FRICTION_IMPULSE
    361 
    362                         //GEN_set_min(contactConstraint.m_appliedImpulse, limit);
    363                         //GEN_set_max(contactConstraint.m_appliedImpulse, -limit);
    364 
    365                        
    366 
    367                 }
    368        
    369                 body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,contactConstraint.m_angularComponentA,j1);
    370                
    371                 body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,contactConstraint.m_angularComponentB,-j1);
    372 
    373         }
    374         return 0.f;
    375 }
    376 
    377 
    378 #else
    379 
    380 //velocity + friction
    381 //response  between two dynamic objects with friction
    382 btScalar resolveSingleFrictionCacheFriendly(
    383         btSolverBody& body1,
    384         btSolverBody& body2,
    385         btSolverConstraint& contactConstraint,
    386         const btContactSolverInfo& solverInfo)
    387 {
    388 
    389         btVector3 vel1;
    390         btVector3 vel2;
    391         btScalar normalImpulse(0.f);
    392 
    393         {
    394                 const btVector3& normal = contactConstraint.m_contactNormal;
    395                 if (contactConstraint.m_penetration < 0.f)
    396                         return 0.f;
    397 
    398 
    399                 body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
    400                 body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
    401                 btVector3 vel = vel1 - vel2;
    402                 btScalar rel_vel;
    403                 rel_vel = normal.dot(vel);
    404 
    405                 btVector3 lat_vel = vel - normal * rel_vel;
    406                 btScalar lat_rel_vel = lat_vel.length2();
    407 
    408                 btScalar combinedFriction = contactConstraint.m_friction;
    409                 const btVector3& rel_pos1 = contactConstraint.m_rel_posA;
    410                 const btVector3& rel_pos2 = contactConstraint.m_rel_posB;
    411 
    412 
    413                 if (lat_rel_vel > SIMD_EPSILON*SIMD_EPSILON)
    414                 {
    415                         lat_rel_vel = btSqrt(lat_rel_vel);
    416 
    417                         lat_vel /= lat_rel_vel;
    418                         btVector3 temp1 = body1.m_invInertiaWorld * rel_pos1.cross(lat_vel);
    419                         btVector3 temp2 = body2.m_invInertiaWorld * rel_pos2.cross(lat_vel);
    420                         btScalar friction_impulse = lat_rel_vel /
    421                                 (body1.m_invMass + body2.m_invMass + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
    422                         btScalar normal_impulse = contactConstraint.m_appliedImpulse * combinedFriction;
    423 
    424                         GEN_set_min(friction_impulse, normal_impulse);
    425                         GEN_set_max(friction_impulse, -normal_impulse);
    426                         body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1);
    427                         body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);
    428                 }
    429         }
    430 
    431         return normalImpulse;
    432 }
    433 
    434 #endif //NO_FRICTION_TANGENTIALS
    435 
    436 
     248
     249void    applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection);
     250void    applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection)
     251{
     252        if (colObj && colObj->hasAnisotropicFriction())
     253        {
     254                // transform to local coordinates
     255                btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis();
     256                const btVector3& friction_scaling = colObj->getAnisotropicFriction();
     257                //apply anisotropic friction
     258                loc_lateral *= friction_scaling;
     259                // ... and transform it back to global coordinates
     260                frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral;
     261        }
     262}
    437263
    438264
    … …  
    440266btSolverConstraint&     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)
    441267{
     268
    442269
    443270        btRigidBody* body0=btRigidBody::upcast(colObj0);
    444271        btRigidBody* body1=btRigidBody::upcast(colObj1);
    445272
    446         btSolverConstraint& solverConstraint = m_tmpSolverFrictionConstraintPool.expand();
     273        btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expand();
     274        memset(&solverConstraint,0xff,sizeof(btSolverConstraint));
    447275        solverConstraint.m_contactNormal = normalAxis;
    448276
    449277        solverConstraint.m_solverBodyIdA = solverBodyIdA;
    450278        solverConstraint.m_solverBodyIdB = solverBodyIdB;
    451         solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D;
    452279        solverConstraint.m_frictionIndex = frictionIndex;
    453280
    … …  
    455282        solverConstraint.m_originalContactPoint = 0;
    456283
    457         solverConstraint.m_appliedImpulse = btScalar(0.);
    458         solverConstraint.m_appliedPushImpulse = 0.f;
    459         solverConstraint.m_penetration = 0.f;
     284        solverConstraint.m_appliedImpulse = 0.f;
     285        //      solverConstraint.m_appliedPushImpulse = 0.f;
     286
    460287        {
    461288                btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
    462289                solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
    463                 solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0);
    464         }
    465         {
    466                 btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal);
     290                solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0);
     291        }
     292        {
     293                btVector3 ftorqueAxis1 = rel_pos2.cross(-solverConstraint.m_contactNormal);
    467294                solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
    468                 solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0);
     295                solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0);
    469296        }
    470297
    … …  
    483310        if (body1)
    484311        {
    485                 vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2);
     312                vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
    486313                denom1 = body1->getInvMass() + normalAxis.dot(vec);
    487314        }
    … …  
    492319        solverConstraint.m_jacDiagABInv = denom;
    493320
     321#ifdef _USE_JACOBIAN
     322        solverConstraint.m_jac =  btJacobianEntry (
     323                rel_pos1,rel_pos2,solverConstraint.m_contactNormal,
     324                body0->getInvInertiaDiagLocal(),
     325                body0->getInvMass(),
     326                body1->getInvInertiaDiagLocal(),
     327                body1->getInvMass());
     328#endif //_USE_JACOBIAN
     329
     330
     331        {
     332                btScalar rel_vel;
     333                btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?body0->getLinearVelocity():btVector3(0,0,0))
     334                        + solverConstraint.m_relpos1CrossNormal.dot(body0?body0->getAngularVelocity():btVector3(0,0,0));
     335                btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?body1->getLinearVelocity():btVector3(0,0,0))
     336                        + solverConstraint.m_relpos2CrossNormal.dot(body1?body1->getAngularVelocity():btVector3(0,0,0));
     337
     338                rel_vel = vel1Dotn+vel2Dotn;
     339
     340                btScalar positionalError = 0.f;
     341
     342                btSimdScalar velocityError =  - rel_vel;
     343                btSimdScalar    velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
     344                solverConstraint.m_rhs = velocityImpulse;
     345                solverConstraint.m_cfm = 0.f;
     346                solverConstraint.m_lowerLimit = 0;
     347                solverConstraint.m_upperLimit = 1e10f;
     348        }
     349
    494350        return solverConstraint;
    495351}
    496352
     353int     btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body)
     354{
     355        int solverBodyIdA = -1;
     356
     357        if (body.getCompanionId() >= 0)
     358        {
     359                //body has already been converted
     360                solverBodyIdA = body.getCompanionId();
     361        } else
     362        {
     363                btRigidBody* rb = btRigidBody::upcast(&body);
     364                if (rb && rb->getInvMass())
     365                {
     366                        solverBodyIdA = m_tmpSolverBodyPool.size();
     367                        btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
     368                        initSolverBody(&solverBody,&body);
     369                        body.setCompanionId(solverBodyIdA);
     370                } else
     371                {
     372                        return 0;//assume first one is a fixed solver body
     373                }
     374        }
     375        return solverBodyIdA;
     376}
     377#include <stdio.h>
     378
     379
     380
     381void    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                {
     412
     413                        const btVector3& pos1 = cp.getPositionWorldOnA();
     414                        const btVector3& pos2 = cp.getPositionWorldOnB();
     415
     416                        rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
     417                        rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
     418
     419
     420                        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);
     440                                {
     441#ifdef COMPUTE_IMPULSE_DENOM
     442                                        btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
     443                                        btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
     444#else                                                   
     445                                        btVector3 vec;
     446                                        btScalar denom0 = 0.f;
     447                                        btScalar denom1 = 0.f;
     448                                        if (rb0)
     449                                        {
     450                                                vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
     451                                                denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
     452                                        }
     453                                        if (rb1)
     454                                        {
     455                                                vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
     456                                                denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
     457                                        }
     458#endif //COMPUTE_IMPULSE_DENOM         
     459
     460                                        btScalar denom = relaxation/(denom0+denom1);
     461                                        solverConstraint.m_jacDiagABInv = denom;
     462                                }
     463
     464                                solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
     465                                solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB);
     466                                solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(-cp.m_normalWorldOnB);
     467
     468
     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);
     475
     476                                btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop;
     477
     478
     479                                solverConstraint.m_friction = cp.m_combinedFriction;
     480
     481                                btScalar restitution = 0.f;
     482                               
     483                                if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold)
     484                                {
     485                                        restitution = 0.f;
     486                                } else
     487                                {
     488                                        restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
     489                                        if (restitution <= btScalar(0.))
     490                                        {
     491                                                restitution = 0.f;
     492                                        };
     493                                }
     494
     495
     496                                ///warm starting (or zero if disabled)
     497                                if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
     498                                {
     499                                        solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
     500                                        if (rb0)
     501                                                m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
     502                                        if (rb1)
     503                                                m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),-solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse);
     504                                } else
     505                                {
     506                                        solverConstraint.m_appliedImpulse = 0.f;
     507                                }
     508
     509                                //                                                      solverConstraint.m_appliedPushImpulse = 0.f;
     510
     511                                {
     512                                        btScalar rel_vel;
     513                                        btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?rb0->getLinearVelocity():btVector3(0,0,0))
     514                                                + solverConstraint.m_relpos1CrossNormal.dot(rb0?rb0->getAngularVelocity():btVector3(0,0,0));
     515                                        btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1?rb1->getLinearVelocity():btVector3(0,0,0))
     516                                                + solverConstraint.m_relpos2CrossNormal.dot(rb1?rb1->getAngularVelocity():btVector3(0,0,0));
     517
     518                                        rel_vel = vel1Dotn+vel2Dotn;
     519
     520                                        btScalar positionalError = 0.f;
     521                                        positionalError = -penetration * infoGlobal.m_erp/infoGlobal.m_timeStep;
     522                                        btScalar        velocityError = restitution - rel_vel;// * damping;
     523                                        btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
     524                                        btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
     525                                        solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
     526                                        solverConstraint.m_cfm = 0.f;
     527                                        solverConstraint.m_lowerLimit = 0;
     528                                        solverConstraint.m_upperLimit = 1e10f;
     529                                }
     530
     531
     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
     582                                        if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
     583                                        {
     584                                                {
     585                                                        btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
     586                                                        if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
     587                                                        {
     588                                                                frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
     589                                                                if (rb0)
     590                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
     591                                                                if (rb1)
     592                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),-frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
     593                                                        } else
     594                                                        {
     595                                                                frictionConstraint1.m_appliedImpulse = 0.f;
     596                                                        }
     597                                                }
     598
     599                                                if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
     600                                                {
     601                                                        btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
     602                                                        if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
     603                                                        {
     604                                                                frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
     605                                                                if (rb0)
     606                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].applyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
     607                                                                if (rb1)
     608                                                                        m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].applyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);
     609                                                        } else
     610                                                        {
     611                                                                frictionConstraint2.m_appliedImpulse = 0.f;
     612                                                        }
     613                                                }
     614                                        } else
     615                                        {
     616                                                btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
     617                                                frictionConstraint1.m_appliedImpulse = 0.f;
     618                                                if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
     619                                                {
     620                                                        btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
     621                                                        frictionConstraint2.m_appliedImpulse = 0.f;
     622                                                }
     623                                        }
     624                                }
     625                        }
     626
     627
     628                }
     629        }
     630}
    497631
    498632
    … …  
    506640        if (!(numConstraints + numManifolds))
    507641        {
    508 //              printf("empty\n");
     642                //              printf("empty\n");
    509643                return 0.f;
    510644        }
    511         btPersistentManifold* manifold = 0;
    512         btCollisionObject* colObj0=0,*colObj1=0;
    513 
    514         //btRigidBody* rb0=0,*rb1=0;
    515 
    516 
    517 #ifdef FORCE_REFESH_CONTACT_MANIFOLDS
    518 
    519         BEGIN_PROFILE("refreshManifolds");
    520 
    521         int i;
    522        
    523        
    524 
    525         for (i=0;i<numManifolds;i++)
    526         {
    527                 manifold = manifoldPtr[i];
    528                 rb1 = (btRigidBody*)manifold->getBody1();
    529                 rb0 = (btRigidBody*)manifold->getBody0();
    530                
    531                 manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
    532 
    533         }
    534 
    535         END_PROFILE("refreshManifolds");
    536 #endif //FORCE_REFESH_CONTACT_MANIFOLDS
    537 
    538        
    539 
    540 
    541 
    542         //int sizeofSB = sizeof(btSolverBody);
    543         //int sizeofSC = sizeof(btSolverConstraint);
    544 
    545 
    546         //if (1)
    547         {
    548                 //if m_stackAlloc, try to pack bodies/constraints to speed up solving
    549 //              btBlock*                                        sablock;
    550 //              sablock = stackAlloc->beginBlock();
    551 
    552         //      int memsize = 16;
    553 //              unsigned char* stackMemory = stackAlloc->allocate(memsize);
    554 
    555                
    556                 //todo: use stack allocator for this temp memory
    557 //              int minReservation = numManifolds*2;
    558 
    559                 //m_tmpSolverBodyPool.reserve(minReservation);
    560 
    561                 //don't convert all bodies, only the one we need so solver the constraints
    562 /*
    563                 {
    564                         for (int i=0;i<numBodies;i++)
    565                         {
    566                                 btRigidBody* rb = btRigidBody::upcast(bodies[i]);
    567                                 if (rb &&       (rb->getIslandTag() >= 0))
    568                                 {
    569                                         btAssert(rb->getCompanionId() < 0);
    570                                         int solverBodyId = m_tmpSolverBodyPool.size();
    571                                         btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
    572                                         initSolverBody(&solverBody,rb);
    573                                         rb->setCompanionId(solverBodyId);
    574                                 }
    575                         }
    576                 }
    577 */
    578                
    579                 //m_tmpSolverConstraintPool.reserve(minReservation);
    580                 //m_tmpSolverFrictionConstraintPool.reserve(minReservation);
    581 
    582                 {
    583                         int i;
    584 
    585                         for (i=0;i<numManifolds;i++)
    586                         {
    587                                 manifold = manifoldPtr[i];
    588                                 colObj0 = (btCollisionObject*)manifold->getBody0();
    589                                 colObj1 = (btCollisionObject*)manifold->getBody1();
    590                        
    591                                 int solverBodyIdA=-1;
    592                                 int solverBodyIdB=-1;
    593 
    594                                 if (manifold->getNumContacts())
    595                                 {
    596 
    597                                        
    598 
    599                                         if (colObj0->getIslandTag() >= 0)
    600                                         {
    601                                                 if (colObj0->getCompanionId() >= 0)
    602                                                 {
    603                                                         //body has already been converted
    604                                                         solverBodyIdA = colObj0->getCompanionId();
    605                                                 } else
    606                                                 {
    607                                                         solverBodyIdA = m_tmpSolverBodyPool.size();
    608                                                         btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
    609                                                         initSolverBody(&solverBody,colObj0);
    610                                                         colObj0->setCompanionId(solverBodyIdA);
    611                                                 }
    612                                         } else
    613                                         {
    614                                                 //create a static body
    615                                                 solverBodyIdA = m_tmpSolverBodyPool.size();
    616                                                 btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
    617                                                 initSolverBody(&solverBody,colObj0);
    618                                         }
    619 
    620                                         if (colObj1->getIslandTag() >= 0)
    621                                         {
    622                                                 if (colObj1->getCompanionId() >= 0)
    623                                                 {
    624                                                         solverBodyIdB = colObj1->getCompanionId();
    625                                                 } else
    626                                                 {
    627                                                         solverBodyIdB = m_tmpSolverBodyPool.size();
    628                                                         btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
    629                                                         initSolverBody(&solverBody,colObj1);
    630                                                         colObj1->setCompanionId(solverBodyIdB);
    631                                                 }
    632                                         } else
    633                                         {
    634                                                 //create a static body
    635                                                 solverBodyIdB = m_tmpSolverBodyPool.size();
    636                                                 btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
    637                                                 initSolverBody(&solverBody,colObj1);
    638                                         }
    639                                 }
    640 
    641                                 btVector3 rel_pos1;
    642                                 btVector3 rel_pos2;
    643                                 btScalar relaxation;
    644 
    645                                 for (int j=0;j<manifold->getNumContacts();j++)
    646                                 {
    647                                        
    648                                         btManifoldPoint& cp = manifold->getContactPoint(j);
    649                                        
    650                                         if (cp.getDistance() <= btScalar(0.))
    651                                         {
    652                                                
    653                                                 const btVector3& pos1 = cp.getPositionWorldOnA();
    654                                                 const btVector3& pos2 = cp.getPositionWorldOnB();
    655 
    656                                                  rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin();
    657                                                  rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
    658 
    659                                                
    660                                                 relaxation = 1.f;
    661                                                 btScalar rel_vel;
    662                                                 btVector3 vel;
    663 
    664                                                 int frictionIndex = m_tmpSolverConstraintPool.size();
    665 
    666                                                 {
    667                                                         btSolverConstraint& solverConstraint = m_tmpSolverConstraintPool.expand();
    668                                                         btRigidBody* rb0 = btRigidBody::upcast(colObj0);
    669                                                         btRigidBody* rb1 = btRigidBody::upcast(colObj1);
    670 
    671                                                         solverConstraint.m_solverBodyIdA = solverBodyIdA;
    672                                                         solverConstraint.m_solverBodyIdB = solverBodyIdB;
    673                                                         solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_CONTACT_1D;
    674 
    675                                                         solverConstraint.m_originalContactPoint = &cp;
    676 
    677                                                         btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
    678                                                         solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0 : btVector3(0,0,0);
    679                                                         btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);           
    680                                                         solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*torqueAxis1 : btVector3(0,0,0);
    681                                                         {
    682 #ifdef COMPUTE_IMPULSE_DENOM
    683                                                                 btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
    684                                                                 btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
    685 #else                                                   
    686                                                                 btVector3 vec;
    687                                                                 btScalar denom0 = 0.f;
    688                                                                 btScalar denom1 = 0.f;
    689                                                                 if (rb0)
    690                                                                 {
    691                                                                         vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
    692                                                                         denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
    693                                                                 }
    694                                                                 if (rb1)
    695                                                                 {
    696                                                                         vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2);
    697                                                                         denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
    698                                                                 }
    699 #endif //COMPUTE_IMPULSE_DENOM         
    700                                                                
    701                                                                 btScalar denom = relaxation/(denom0+denom1);
    702                                                                 solverConstraint.m_jacDiagABInv = denom;
    703                                                         }
    704 
    705                                                         solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
    706                                                         solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB);
    707                                                         solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(cp.m_normalWorldOnB);
    708 
    709 
    710                                                         btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
    711                                                         btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
    712                                
    713                                                         vel  = vel1 - vel2;
    714                                                        
    715                                                         rel_vel = cp.m_normalWorldOnB.dot(vel);
    716                                                        
    717                                                         solverConstraint.m_penetration = btMin(cp.getDistance()+infoGlobal.m_linearSlop,btScalar(0.));
    718                                                         //solverConstraint.m_penetration = cp.getDistance();
    719 
    720                                                         solverConstraint.m_friction = cp.m_combinedFriction;
    721 
    722                                                        
    723                                                         if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold)
    724                                                         {
    725                                                                 solverConstraint.m_restitution = 0.f;
    726                                                         } else
    727                                                         {
    728                                                                 solverConstraint.m_restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
    729                                                                 if (solverConstraint.m_restitution <= btScalar(0.))
    730                                                                 {
    731                                                                         solverConstraint.m_restitution = 0.f;
    732                                                                 };
    733                                                         }
    734 
    735                                                        
    736                                                         btScalar penVel = -solverConstraint.m_penetration/infoGlobal.m_timeStep;
    737 
    738                                                        
    739 
    740                                                         if (solverConstraint.m_restitution > penVel)
    741                                                         {
    742                                                                 solverConstraint.m_penetration = btScalar(0.);
    743                                                         }
    744                                                          
    745                                                        
    746                                                        
    747                                                         ///warm starting (or zero if disabled)
    748                                                         if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
    749                                                         {
    750                                                                 solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
    751                                                                 if (rb0)
    752                                                                         m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
    753                                                                 if (rb1)
    754                                                                         m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),solverConstraint.m_angularComponentB,-solverConstraint.m_appliedImpulse);
    755                                                         } else
    756                                                         {
    757                                                                 solverConstraint.m_appliedImpulse = 0.f;
    758                                                         }
    759 
    760                                                         solverConstraint.m_appliedPushImpulse = 0.f;
    761                                                        
    762                                                         solverConstraint.m_frictionIndex = m_tmpSolverFrictionConstraintPool.size();
    763                                                         if (!cp.m_lateralFrictionInitialized)
    764                                                         {
    765                                                                 cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
    766                                                                 btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
    767                                                                 if (lat_rel_vel > SIMD_EPSILON)//0.0f)
    768                                                                 {
    769                                                                         cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
    770                                                                         addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
    771                                                                         if(infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
    772                                                                         {
    773                                                                                 cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
    774                                                                                 cp.m_lateralFrictionDir2.normalize();//??
    775                                                                                 addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
    776                                                                                 cp.m_lateralFrictionInitialized = true;
    777                                                                         }
    778                                                                 } else
    779                                                                 {
    780                                                                         //re-calculate friction direction every frame, todo: check if this is really needed
    781                                                                         btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
    782                                                                         addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
    783                                                                         addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
    784                                                                         if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
    785                                                                         {
    786                                                                                 cp.m_lateralFrictionInitialized = true;
    787                                                                         }
    788                                                                 }
    789                                                                
    790                                                         } else
    791                                                         {
    792                                                                 addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
    793                                                                 if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
    794                                                                         addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
    795                                                         }
    796 
    797                                                         if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
    798                                                         {
    799                                                                 {
    800                                                                         btSolverConstraint& frictionConstraint1 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex];
    801                                                                         if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
    802                                                                         {
    803                                                                                 frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor;
    804                                                                                 if (rb0)
    805                                                                                         m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse);
    806                                                                                 if (rb1)
    807                                                                                         m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass(),frictionConstraint1.m_angularComponentB,-frictionConstraint1.m_appliedImpulse);
    808                                                                         } else
    809                                                                         {
    810                                                                                 frictionConstraint1.m_appliedImpulse = 0.f;
    811                                                                         }
    812                                                                 }
    813                                                                 {
    814                                                                         btSolverConstraint& frictionConstraint2 = m_tmpSolverFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
    815                                                                         if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
    816                                                                         {
    817                                                                                 frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor;
    818                                                                                 if (rb0)
    819                                                                                         m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse);
    820                                                                                 if (rb1)
    821                                                                                         m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),frictionConstraint2.m_angularComponentB,-frictionConstraint2.m_appliedImpulse);
    822                                                                         } else
    823                                                                         {
    824                                                                                 frictionConstraint2.m_appliedImpulse = 0.f;
    825                                                                         }
    826                                                                 }
    827                                                         }
    828                                                 }
    829 
    830 
    831                                         }
    832                                 }
    833                         }
    834                 }
    835         }
    836        
    837         btContactSolverInfo info = infoGlobal;
    838 
     645
     646        if (1)
    839647        {
    840648                int j;
    … …  
    845653                }
    846654        }
    847        
    848        
    849 
    850         int numConstraintPool = m_tmpSolverConstraintPool.size();
    851         int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
     655
     656        btSolverBody& fixedBody = m_tmpSolverBodyPool.expand();
     657        initSolverBody(&fixedBody,0);
     658
     659        //btRigidBody* rb0=0,*rb1=0;
     660
     661        //if (1)
     662        {
     663                {
     664
     665                        int totalNumRows = 0;
     666                        int i;
     667                        //calculate the total number of contraint rows
     668                        for (i=0;i<numConstraints;i++)
     669                        {
     670
     671                                btTypedConstraint::btConstraintInfo1 info1;
     672                                constraints[i]->getInfo1(&info1);
     673                                totalNumRows += info1.m_numConstraintRows;
     674                        }
     675                        m_tmpSolverNonContactConstraintPool.resize(totalNumRows);
     676
     677                        btTypedConstraint::btConstraintInfo1 info1;
     678                        info1.m_numConstraintRows = 0;
     679
     680
     681                        ///setup the btSolverConstraints
     682                        int currentRow = 0;
     683
     684                        for (i=0;i<numConstraints;i++,currentRow+=info1.m_numConstraintRows)
     685                        {
     686                                constraints[i]->getInfo1(&info1);
     687                                if (info1.m_numConstraintRows)
     688                                {
     689                                        btAssert(currentRow<totalNumRows);
     690
     691                                        btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow];
     692                                        btTypedConstraint* constraint = constraints[i];
     693
     694
     695
     696                                        btRigidBody& rbA = constraint->getRigidBodyA();
     697                                        btRigidBody& rbB = constraint->getRigidBodyB();
     698
     699                                        int solverBodyIdA = getOrInitSolverBody(rbA);
     700                                        int solverBodyIdB = getOrInitSolverBody(rbB);
     701
     702                                        btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA];
     703                                        btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB];
     704
     705                                        int j;
     706                                        for ( j=0;j<info1.m_numConstraintRows;j++)
     707                                        {
     708                                                memset(&currentConstraintRow[j],0,sizeof(btSolverConstraint));
     709                                                currentConstraintRow[j].m_lowerLimit = -FLT_MAX;
     710                                                currentConstraintRow[j].m_upperLimit = FLT_MAX;
     711                                                currentConstraintRow[j].m_appliedImpulse = 0.f;
     712                                                currentConstraintRow[j].m_appliedPushImpulse = 0.f;
     713                                                currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA;
     714                                                currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB;
     715                                        }
     716
     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);
     721
     722
     723
     724                                        btTypedConstraint::btConstraintInfo2 info2;
     725                                        info2.fps = 1.f/infoGlobal.m_timeStep;
     726                                        info2.erp = infoGlobal.m_erp;
     727                                        info2.m_J1linearAxis = currentConstraintRow->m_contactNormal;
     728                                        info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
     729                                        info2.m_J2linearAxis = 0;
     730                                        info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
     731                                        info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this
     732                                        ///the size of btSolverConstraint needs be a multiple of btScalar
     733                                        btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint));
     734                                        info2.m_constraintError = &currentConstraintRow->m_rhs;
     735                                        info2.cfm = &currentConstraintRow->m_cfm;
     736                                        info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
     737                                        info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
     738                                        constraints[i]->getInfo2(&info2);
     739
     740                                        ///finalize the constraint setup
     741                                        for ( j=0;j<info1.m_numConstraintRows;j++)
     742                                        {
     743                                                btSolverConstraint& solverConstraint = currentConstraintRow[j];
     744
     745                                                {
     746                                                        const btVector3& ftorqueAxis1 = solverConstraint.m_relpos1CrossNormal;
     747                                                        solverConstraint.m_angularComponentA = constraint->getRigidBodyA().getInvInertiaTensorWorld()*ftorqueAxis1*constraint->getRigidBodyA().getAngularFactor();
     748                                                }
     749                                                {
     750                                                        const btVector3& ftorqueAxis2 = solverConstraint.m_relpos2CrossNormal;
     751                                                        solverConstraint.m_angularComponentB = constraint->getRigidBodyB().getInvInertiaTensorWorld()*ftorqueAxis2*constraint->getRigidBodyB().getAngularFactor();
     752                                                }
     753
     754                                                {
     755                                                        btVector3 iMJlA = solverConstraint.m_contactNormal*rbA.getInvMass();
     756                                                        btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal;
     757                                                        btVector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal?
     758                                                        btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal;
     759
     760                                                        btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal);
     761                                                        sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
     762                                                        sum += iMJlB.dot(solverConstraint.m_contactNormal);
     763                                                        sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
     764
     765                                                        solverConstraint.m_jacDiagABInv = btScalar(1.)/sum;
     766                                                }
     767
     768
     769                                                ///fix rhs
     770                                                ///todo: add force/torque accelerators
     771                                                {
     772                                                        btScalar rel_vel;
     773                                                        btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity());
     774                                                        btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity());
     775
     776                                                        rel_vel = vel1Dotn+vel2Dotn;
     777
     778                                                        btScalar restitution = 0.f;
     779                                                        btScalar positionalError = solverConstraint.m_rhs;//already filled in by getConstraintInfo2
     780                                                        btScalar        velocityError = restitution - rel_vel;// * damping;
     781                                                        btScalar        penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
     782                                                        btScalar        velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
     783                                                        solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
     784                                                        solverConstraint.m_appliedImpulse = 0.f;
     785
     786                                                }
     787                                        }
     788                                }
     789                        }
     790                }
     791
     792                {
     793                        int i;
     794                        btPersistentManifold* manifold = 0;
     795                        btCollisionObject* colObj0=0,*colObj1=0;
     796
     797
     798                        for (i=0;i<numManifolds;i++)
     799                        {
     800                                manifold = manifoldPtr[i];
     801                                convertContact(manifold,infoGlobal);
     802                        }
     803                }
     804        }
     805
     806        btContactSolverInfo info = infoGlobal;
     807
     808
     809
     810        int numConstraintPool = m_tmpSolverContactConstraintPool.size();
     811        int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
    852812
    853813        ///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
    … …  
    866826        }
    867827
    868 
    869 
    870828        return 0.f;
    871829
    … …  
    875833{
    876834        BT_PROFILE("solveGroupCacheFriendlyIterations");
    877         int numConstraintPool = m_tmpSolverConstraintPool.size();
    878         int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
     835
     836        int numConstraintPool = m_tmpSolverContactConstraintPool.size();
     837        int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
    879838
    880839        //should traverse the contacts random order...
    … …  
    904863                        }
    905864
    906                         for (j=0;j<numConstraints;j++)
     865                        if (infoGlobal.m_solverMode & SOLVER_SIMD)
    907866                        {
    908                                 btTypedConstraint* constraint = constraints[j];
    909                                 ///todo: use solver bodies, so we don't need to copy from/to btRigidBody
    910 
    911                                 if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0))
    912                                 {
    913                                         m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].writebackVelocity();
    914                                 }
    915                                 if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0))
    916                                 {
    917                                         m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].writebackVelocity();
    918                                 }
    919 
    920                                 constraint->solveConstraint(infoGlobal.m_timeStep);
    921 
    922                                 if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0))
    923                                 {
    924                                         m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].readVelocity();
    925                                 }
    926                                 if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0))
    927                                 {
    928                                         m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].readVelocity();
    929                                 }
    930 
     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                                }
    931948                        }
    932949
    933                         {
    934                                 int numPoolConstraints = m_tmpSolverConstraintPool.size();
    935                                 for (j=0;j<numPoolConstraints;j++)
    936                                 {
    937                                        
    938                                         const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]];
    939                                         resolveSingleCollisionCombinedCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
    940                                                 m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal);
    941                                 }
    942                         }
    943 
    944                         {
    945                                  int numFrictionPoolConstraints = m_tmpSolverFrictionConstraintPool.size();
    946                                
    947                                  for (j=0;j<numFrictionPoolConstraints;j++)
    948                                 {
    949                                         const btSolverConstraint& solveManifold = m_tmpSolverFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
    950                                         btScalar totalImpulse = m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse+
    951                                                                 m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedPushImpulse;                 
    952 
    953                                                 resolveSingleFrictionCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
    954                                                         m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal,
    955                                                         totalImpulse);
    956                                 }
    957                         }
    958                        
    959 
    960 
    961                 }
     950
     951
     952                }
     953        }
     954        return 0.f;
     955}
     956
     957
     958
     959/// btSequentialImpulseConstraintSolver Sequentially applies impulses
     960btScalar 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
    962963       
    963                 if (infoGlobal.m_splitImpulse)
    964                 {
    965                        
    966                         for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
    967                         {
    968                                 {
    969                                         int numPoolConstraints = m_tmpSolverConstraintPool.size();
    970                                         int j;
    971                                         for (j=0;j<numPoolConstraints;j++)
    972                                         {
    973                                                 const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]];
    974 
    975                                                 resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
    976                                                         m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal);
    977                                         }
    978                                 }
    979                         }
    980 
    981                 }
    982 
    983         }
    984 
    985         return 0.f;
    986 }
    987 
    988 
    989 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
    990 {
     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
    991971        int i;
    992972
    … …  
    994974        solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
    995975
    996         int numPoolConstraints = m_tmpSolverConstraintPool.size();
     976        int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
    997977        int j;
     978
    998979        for (j=0;j<numPoolConstraints;j++)
    999980        {
    1000                
    1001                 const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[j];
     981
     982                const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
    1002983                btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
    1003984                btAssert(pt);
    … …  
    1005986                if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
    1006987                {
    1007                         pt->m_appliedImpulseLateral1 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
    1008                         pt->m_appliedImpulseLateral2 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
     988                        pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
     989                        pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
    1009990                }
    1010991
    1011992                //do a callback here?
    1012 
    1013993        }
    1014994
    … …  
    10221002        {
    10231003                for ( i=0;i<m_tmpSolverBodyPool.size();i++)
    1024         {
    1025                 m_tmpSolverBodyPool[i].writebackVelocity();
    1026         }
    1027         }
    1028 
    1029 //      printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size());
    1030 
    1031 /*
    1032         printf("m_tmpSolverBodyPool.size() = %i\n",m_tmpSolverBodyPool.size());
    1033         printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size());
    1034         printf("m_tmpSolverFrictionConstraintPool.size() = %i\n",m_tmpSolverFrictionConstraintPool.size());
    1035 
    1036        
    1037         printf("m_tmpSolverBodyPool.capacity() = %i\n",m_tmpSolverBodyPool.capacity());
    1038         printf("m_tmpSolverConstraintPool.capacity() = %i\n",m_tmpSolverConstraintPool.capacity());
    1039         printf("m_tmpSolverFrictionConstraintPool.capacity() = %i\n",m_tmpSolverFrictionConstraintPool.capacity());
    1040 */
     1004                {
     1005                        m_tmpSolverBodyPool[i].writebackVelocity();
     1006                }
     1007        }
     1008
    10411009
    10421010        m_tmpSolverBodyPool.resize(0);
    1043         m_tmpSolverConstraintPool.resize(0);
    1044         m_tmpSolverFrictionConstraintPool.resize(0);
    1045 
     1011        m_tmpSolverContactConstraintPool.resize(0);
     1012        m_tmpSolverNonContactConstraintPool.resize(0);
     1013        m_tmpSolverContactFrictionConstraintPool.resize(0);
    10461014
    10471015        return 0.f;
    10481016}
    10491017
    1050 /// btSequentialImpulseConstraintSolver Sequentially applies impulses
    1051 btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/)
    1052 {
    1053         BT_PROFILE("solveGroup");
    1054         if (infoGlobal.m_solverMode & SOLVER_CACHE_FRIENDLY)
    1055         {
    1056                 //you need to provide at least some bodies
    1057                 //btSimpleDynamicsWorld needs to switch off SOLVER_CACHE_FRIENDLY
    1058                 btAssert(bodies);
    1059                 btAssert(numBodies);
    1060                 return solveGroupCacheFriendly(bodies,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
    1061         }
    1062 
    1063        
    1064 
    1065         btContactSolverInfo info = infoGlobal;
    1066 
    1067         int numiter = infoGlobal.m_numIterations;
    1068 
    1069         int totalPoints = 0;
    1070 
    1071 
    1072         {
    1073                 short j;
    1074                 for (j=0;j<numManifolds;j++)
    1075                 {
    1076                         btPersistentManifold* manifold = manifoldPtr[j];
    1077                         prepareConstraints(manifold,info,debugDrawer);
    1078 
    1079                         for (short p=0;p<manifoldPtr[j]->getNumContacts();p++)
    1080                         {
    1081                                 gOrder[totalPoints].m_manifoldIndex = j;
    1082                                 gOrder[totalPoints].m_pointIndex = p;
    1083                                 totalPoints++;
    1084                         }
    1085                 }
    1086         }
    1087 
    1088         {
    1089                 int j;
    1090                 for (j=0;j<numConstraints;j++)
    1091                 {
    1092                         btTypedConstraint* constraint = constraints[j];
    1093                         constraint->buildJacobian();
    1094                 }
    1095         }
    1096        
    1097 
    1098         //should traverse the contacts random order...
    1099         int iteration;
    1100 
    1101         {
    1102                 for ( iteration = 0;iteration<numiter;iteration++)
    1103                 {
    1104                         int j;
    1105                         if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
    1106                         {
    1107                                 if ((iteration & 7) == 0) {
    1108                                         for (j=0; j<totalPoints; ++j) {
    1109                                                 btOrderIndex tmp = gOrder[j];
    1110                                                 int swapi = btRandInt2(j+1);
    1111                                                 gOrder[j] = gOrder[swapi];
    1112                                                 gOrder[swapi] = tmp;
    1113                                         }
    1114                                 }
    1115                         }
    1116 
    1117                         for (j=0;j<numConstraints;j++)
    1118                         {
    1119                                 btTypedConstraint* constraint = constraints[j];
    1120                                 constraint->solveConstraint(info.m_timeStep);
    1121                         }
    1122 
    1123                         for (j=0;j<totalPoints;j++)
    1124                         {
    1125                                 btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
    1126                                 solve( (btRigidBody*)manifold->getBody0(),
    1127                                                                         (btRigidBody*)manifold->getBody1()
    1128                                 ,manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
    1129                         }
    1130                
    1131                         for (j=0;j<totalPoints;j++)
    1132                         {
    1133                                 btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
    1134                                 solveFriction((btRigidBody*)manifold->getBody0(),
    1135                                         (btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
    1136                         }
    1137                        
    1138                 }
    1139         }
    1140                
    1141 
    1142 
    1143 
    1144         return btScalar(0.);
    1145 }
    1146 
    1147 
    1148 
    1149 
    1150 
    1151 
    1152 
    1153 void    btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer)
    1154 {
    1155 
    1156         (void)debugDrawer;
    1157 
    1158         btRigidBody* body0 = (btRigidBody*)manifoldPtr->getBody0();
    1159         btRigidBody* body1 = (btRigidBody*)manifoldPtr->getBody1();
    1160 
    1161 
    1162         //only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
    1163         {
    1164 #ifdef FORCE_REFESH_CONTACT_MANIFOLDS
    1165                 manifoldPtr->refreshContactPoints(body0->getCenterOfMassTransform(),body1->getCenterOfMassTransform());
    1166 #endif //FORCE_REFESH_CONTACT_MANIFOLDS         
    1167                 int numpoints = manifoldPtr->getNumContacts();
    1168 
    1169                 gTotalContactPoints += numpoints;
    1170 
    1171                
    1172                 for (int i=0;i<numpoints ;i++)
    1173                 {
    1174                         btManifoldPoint& cp = manifoldPtr->getContactPoint(i);
    1175                         if (cp.getDistance() <= btScalar(0.))
    1176                         {
    1177                                 const btVector3& pos1 = cp.getPositionWorldOnA();
    1178                                 const btVector3& pos2 = cp.getPositionWorldOnB();
    1179 
    1180                                 btVector3 rel_pos1 = pos1 - body0->getCenterOfMassPosition();
    1181                                 btVector3 rel_pos2 = pos2 - body1->getCenterOfMassPosition();
    1182                                
    1183 
    1184                                 //this jacobian entry is re-used for all iterations
    1185                                 btJacobianEntry jac(body0->getCenterOfMassTransform().getBasis().transpose(),
    1186                                         body1->getCenterOfMassTransform().getBasis().transpose(),
    1187                                         rel_pos1,rel_pos2,cp.m_normalWorldOnB,body0->getInvInertiaDiagLocal(),body0->getInvMass(),
    1188                                         body1->getInvInertiaDiagLocal(),body1->getInvMass());
    1189 
    1190                                
    1191                                 btScalar jacDiagAB = jac.getDiagonal();
    1192 
    1193                                 btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
    1194                                 if (cpd)
    1195                                 {
    1196                                         //might be invalid
    1197                                         cpd->m_persistentLifeTime++;
    1198                                         if (cpd->m_persistentLifeTime != cp.getLifeTime())
    1199                                         {
    1200                                                 //printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
    1201                                                 new (cpd) btConstraintPersistentData;
    1202                                                 cpd->m_persistentLifeTime = cp.getLifeTime();
    1203 
    1204                                         } else
    1205                                         {
    1206                                                 //printf("Persistent: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
    1207                                                
    1208                                         }
    1209                                 } else
    1210                                 {
    1211                                                
    1212                                         //todo: should this be in a pool?
    1213                                         void* mem = btAlignedAlloc(sizeof(btConstraintPersistentData),16);
    1214                                         cpd = new (mem)btConstraintPersistentData;
    1215                                         assert(cpd);
    1216 
    1217                                         totalCpd ++;
    1218                                         //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd);
    1219                                         cp.m_userPersistentData = cpd;
    1220                                         cpd->m_persistentLifeTime = cp.getLifeTime();
    1221                                         //printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime());
    1222                                        
    1223                                 }
    1224                                 assert(cpd);
    1225 
    1226                                 cpd->m_jacDiagABInv = btScalar(1.) / jacDiagAB;
    1227 
    1228                                 //Dependent on Rigidbody A and B types, fetch the contact/friction response func
    1229                                 //perhaps do a similar thing for friction/restutution combiner funcs...
    1230                                
    1231                                 cpd->m_frictionSolverFunc = m_frictionDispatch[body0->m_frictionSolverType][body1->m_frictionSolverType];
    1232                                 cpd->m_contactSolverFunc = m_contactDispatch[body0->m_contactSolverType][body1->m_contactSolverType];
    1233                                
    1234                                 btVector3 vel1 = body0->getVelocityInLocalPoint(rel_pos1);
    1235                                 btVector3 vel2 = body1->getVelocityInLocalPoint(rel_pos2);
    1236                                 btVector3 vel = vel1 - vel2;
    1237                                 btScalar rel_vel;
    1238                                 rel_vel = cp.m_normalWorldOnB.dot(vel);
    1239                                
    1240                                 btScalar combinedRestitution = cp.m_combinedRestitution;
    1241                                
    1242                                 cpd->m_penetration = cp.getDistance();///btScalar(info.m_numIterations);
    1243                                 cpd->m_friction = cp.m_combinedFriction;
    1244                                 if (cp.m_lifeTime>info.m_restingContactRestitutionThreshold)
    1245                                 {
    1246                                         cpd->m_restitution = 0.f;
    1247                                 } else
    1248                                 {
    1249                                         cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution);
    1250                                         if (cpd->m_restitution <= btScalar(0.))
    1251                                         {
    1252                                                 cpd->m_restitution = btScalar(0.0);
    1253                                         };
    1254                                 }
    1255                                
    1256                                 //restitution and penetration work in same direction so
    1257                                 //rel_vel
    1258                                
    1259                                 btScalar penVel = -cpd->m_penetration/info.m_timeStep;
    1260 
    1261                                 if (cpd->m_restitution > penVel)
    1262                                 {
    1263                                         cpd->m_penetration = btScalar(0.);
    1264                                 }                               
    1265                                
    1266 
    1267                                 btScalar relaxation = info.m_damping;
    1268                                 if (info.m_solverMode & SOLVER_USE_WARMSTARTING)
    1269                                 {
    1270                                         cpd->m_appliedImpulse *= relaxation;
    1271                                 } else
    1272                                 {
    1273                                         cpd->m_appliedImpulse =btScalar(0.);
    1274                                 }
    1275        
    1276                                 //for friction
    1277                                 cpd->m_prevAppliedImpulse = cpd->m_appliedImpulse;
    1278                                
    1279                                 //re-calculate friction direction every frame, todo: check if this is really needed
    1280                                 btPlaneSpace1(cp.m_normalWorldOnB,cpd->m_frictionWorldTangential0,cpd->m_frictionWorldTangential1);
    1281 
    1282 
    1283 #define NO_FRICTION_WARMSTART 1
    1284 
    1285         #ifdef NO_FRICTION_WARMSTART
    1286                                 cpd->m_accumulatedTangentImpulse0 = btScalar(0.);
    1287                                 cpd->m_accumulatedTangentImpulse1 = btScalar(0.);
    1288         #endif //NO_FRICTION_WARMSTART
    1289                                 btScalar denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential0);
    1290                                 btScalar denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential0);
    1291                                 btScalar denom = relaxation/(denom0+denom1);
    1292                                 cpd->m_jacDiagABInvTangent0 = denom;
    1293 
    1294 
    1295                                 denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential1);
    1296                                 denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential1);
    1297                                 denom = relaxation/(denom0+denom1);
    1298                                 cpd->m_jacDiagABInvTangent1 = denom;
    1299 
    1300                                 btVector3 totalImpulse =
    1301         #ifndef NO_FRICTION_WARMSTART
    1302                                         cpd->m_frictionWorldTangential0*cpd->m_accumulatedTangentImpulse0+
    1303                                         cpd->m_frictionWorldTangential1*cpd->m_accumulatedTangentImpulse1+
    1304         #endif //NO_FRICTION_WARMSTART
    1305                                         cp.m_normalWorldOnB*cpd->m_appliedImpulse;
    1306 
    1307 
    1308 
    1309                                 ///
    1310                                 {
    1311                                 btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
    1312                                 cpd->m_angularComponentA = body0->getInvInertiaTensorWorld()*torqueAxis0;
    1313                                 btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);           
    1314                                 cpd->m_angularComponentB = body1->getInvInertiaTensorWorld()*torqueAxis1;
    1315                                 }
    1316                                 {
    1317                                         btVector3 ftorqueAxis0 = rel_pos1.cross(cpd->m_frictionWorldTangential0);
    1318                                         cpd->m_frictionAngularComponent0A = body0->getInvInertiaTensorWorld()*ftorqueAxis0;
    1319                                 }
    1320                                 {
    1321                                         btVector3 ftorqueAxis1 = rel_pos1.cross(cpd->m_frictionWorldTangential1);
    1322                                         cpd->m_frictionAngularComponent1A = body0->getInvInertiaTensorWorld()*ftorqueAxis1;
    1323                                 }
    1324                                 {
    1325                                         btVector3 ftorqueAxis0 = rel_pos2.cross(cpd->m_frictionWorldTangential0);
    1326                                         cpd->m_frictionAngularComponent0B = body1->getInvInertiaTensorWorld()*ftorqueAxis0;
    1327                                 }
    1328                                 {
    1329                                         btVector3 ftorqueAxis1 = rel_pos2.cross(cpd->m_frictionWorldTangential1);
    1330                                         cpd->m_frictionAngularComponent1B = body1->getInvInertiaTensorWorld()*ftorqueAxis1;
    1331                                 }
    1332                                
    1333                                 ///
    1334 
    1335 
    1336 
    1337                                 //apply previous frames impulse on both bodies
    1338                                 body0->applyImpulse(totalImpulse, rel_pos1);
    1339                                 body1->applyImpulse(-totalImpulse, rel_pos2);
    1340                         }
    1341                        
    1342                 }
    1343         }
    1344 }
    1345 
    1346 
    1347 btScalar btSequentialImpulseConstraintSolver::solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
    1348 {
    1349         btScalar maxImpulse = btScalar(0.);
    1350 
    1351         {
    1352 
    1353                
    1354                 {
    1355                         if (cp.getDistance() <= btScalar(0.))
    1356                         {
    1357 
    1358                                
    1359 
    1360                                 {
    1361 
    1362                                         //btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
    1363                                         btScalar impulse = resolveSingleCollisionCombined(
    1364                                                 *body0,*body1,
    1365                                                 cp,
    1366                                                 info);
    1367 
    1368                                         if (maxImpulse < impulse)
    1369                                                 maxImpulse  = impulse;
    1370 
    1371                                 }
    1372                         }
    1373                 }
    1374         }
    1375         return maxImpulse;
    1376 }
    1377 
    1378 
    1379 
    1380 btScalar btSequentialImpulseConstraintSolver::solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
    1381 {
    1382 
    1383         btScalar maxImpulse = btScalar(0.);
    1384 
    1385         {
    1386 
    1387                
    1388                 {
    1389                         if (cp.getDistance() <= btScalar(0.))
    1390                         {
    1391 
    1392                                
    1393 
    1394                                 {
    1395 
    1396                                         btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
    1397                                         btScalar impulse = cpd->m_contactSolverFunc(
    1398                                                 *body0,*body1,
    1399                                                 cp,
    1400                                                 info);
    1401 
    1402                                         if (maxImpulse < impulse)
    1403                                                 maxImpulse  = impulse;
    1404 
    1405                                 }
    1406                         }
    1407                 }
    1408         }
    1409         return maxImpulse;
    1410 }
    1411 
    1412 btScalar btSequentialImpulseConstraintSolver::solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
    1413 {
    1414 
    1415         (void)debugDrawer;
    1416         (void)iter;
    1417 
    1418 
    1419         {
    1420 
    1421                
    1422                 {
    1423                        
    1424                         if (cp.getDistance() <= btScalar(0.))
    1425                         {
    1426 
    1427                                 btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
    1428                                 cpd->m_frictionSolverFunc(
    1429                                         *body0,*body1,
    1430                                         cp,
    1431                                         info);
    1432 
    1433                                
    1434                         }
    1435                 }
    1436 
    1437        
    1438         }
    1439         return btScalar(0.);
    1440 }
     1018
     1019
     1020
     1021
     1022
     1023
    14411024
    14421025
Note: See TracChangeset for help on using the changeset viewer.