Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

Ignore:
Timestamp:
Apr 8, 2009, 12:58:47 AM (16 years ago)
Author:
dafrick
Message:

Reverted to revision 2906 (because I'm too stupid to merge correctly, 2nd try will follow shortly. ;))

Location:
code/branches/questsystem5
Files:
2 edited

Legend:

Unmodified
Added
Removed

  • code/branches/questsystem5

  • code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

    r2907 r2908  
    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
    1819
    1920#include "btSequentialImpulseConstraintSolver.h"
    … …  
    3233#include "btSolverBody.h"
    3334#include "btSolverConstraint.h"
     35
     36
    3437#include "LinearMath/btAlignedObjectArray.h"
    35 #include <string.h> //for memset
     38
     39
     40int totalCpd = 0;
     41
     42int     gTotalContactPoints = 0;
     43
     44struct  btOrderIndex
     45{
     46        int     m_manifoldIndex;
     47        int     m_pointIndex;
     48};
     49
     50
     51
     52#define SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS 16384
     53static btOrderIndex     gOrder[SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS];
     54
     55
     56unsigned long btSequentialImpulseConstraintSolver::btRand2()
     57{
     58  m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
     59  return m_btSeed2;
     60}
     61
     62
     63
     64//See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
     65int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
     66{
     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
     95bool  MyContactDestroyedCallback(void* userPersistentData);
     96bool  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
    36107
    37108btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
    38109:m_btSeed2(0)
    39110{
    40 
     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                }
    41122}
    42123
    43124btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
    44125{
    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))
    50 static 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
    58 void 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 
    178 
    179 
    180 unsigned long btSequentialImpulseConstraintSolver::btRand2()
    181 {
    182         m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
    183         return m_btSeed2;
    184 }
    185 
    186 
    187 
    188 //See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
    189 int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
    190 {
    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 
    218 void    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 
     126
     127}
     128
     129void    initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject);
     130void    initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
     131{
     132        btRigidBody* rb = btRigidBody::upcast(collisionObject);
    225133        if (rb)
    226134        {
     135                solverBody->m_angularVelocity = rb->getAngularVelocity() ;
     136                solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin();
     137                solverBody->m_friction = collisionObject->getFriction();
    227138                solverBody->m_invMass = rb->getInvMass();
     139                solverBody->m_linearVelocity = rb->getLinearVelocity();
    228140                solverBody->m_originalBody = rb;
    229141                solverBody->m_angularFactor = rb->getAngularFactor();
    230142        } else
    231143        {
     144                solverBody->m_angularVelocity.setValue(0,0,0);
     145                solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin();
     146                solverBody->m_friction = collisionObject->getFriction();
    232147                solverBody->m_invMass = 0.f;
     148                solverBody->m_linearVelocity.setValue(0,0,0);
    233149                solverBody->m_originalBody = 0;
    234150                solverBody->m_angularFactor = 1.f;
    235151        }
     152        solverBody->m_pushVelocity.setValue(0.f,0.f,0.f);
     153        solverBody->m_turnVelocity.setValue(0.f,0.f,0.f);
    236154}
    237155
    … …  
    239157int             gNumSplitImpulseRecoveries = 0;
    240158
    241 btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, btScalar restitution)
     159btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
     160btScalar restitutionCurve(btScalar rel_vel, btScalar restitution)
    242161{
    243162        btScalar rest = restitution * -rel_vel;
    … …  
    246165
    247166
    248 
    249 void    applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection);
    250 void    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 }
     167void    resolveSplitPenetrationImpulseCacheFriendly(
     168        btSolverBody& body1,
     169        btSolverBody& body2,
     170        const btSolverConstraint& contactConstraint,
     171        const btContactSolverInfo& solverInfo);
     172
     173//SIMD_FORCE_INLINE
     174void    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
     231btScalar resolveSingleCollisionCombinedCacheFriendly(
     232        btSolverBody& body1,
     233        btSolverBody& body2,
     234        const btSolverConstraint& contactConstraint,
     235        const btContactSolverInfo& solverInfo);
     236
     237//SIMD_FORCE_INLINE
     238btScalar 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
     298btScalar resolveSingleFrictionCacheFriendly(
     299        btSolverBody& body1,
     300        btSolverBody& body2,
     301        const btSolverConstraint& contactConstraint,
     302        const btContactSolverInfo& solverInfo,
     303        btScalar appliedNormalImpulse);
     304
     305//SIMD_FORCE_INLINE
     306btScalar 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
     382btScalar 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
    263437
    264438
    … …  
    266440btSolverConstraint&     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)
    267441{
    268 
    269442
    270443        btRigidBody* body0=btRigidBody::upcast(colObj0);
    271444        btRigidBody* body1=btRigidBody::upcast(colObj1);
    272445
    273         btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expand();
    274         memset(&solverConstraint,0xff,sizeof(btSolverConstraint));
     446        btSolverConstraint& solverConstraint = m_tmpSolverFrictionConstraintPool.expand();
    275447        solverConstraint.m_contactNormal = normalAxis;
    276448
    277449        solverConstraint.m_solverBodyIdA = solverBodyIdA;
    278450        solverConstraint.m_solverBodyIdB = solverBodyIdB;
     451        solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D;
    279452        solverConstraint.m_frictionIndex = frictionIndex;
    280453
    … …  
    282455        solverConstraint.m_originalContactPoint = 0;
    283456
    284         solverConstraint.m_appliedImpulse = 0.f;
    285         //      solverConstraint.m_appliedPushImpulse = 0.f;
    286 
     457        solverConstraint.m_appliedImpulse = btScalar(0.);
     458        solverConstraint.m_appliedPushImpulse = 0.f;
     459        solverConstraint.m_penetration = 0.f;
    287460        {
    288461                btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
    289462                solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
    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);
     463                solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0);
     464        }
     465        {
     466                btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal);
    294467                solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
    295                 solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0);
     468                solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0);
    296469        }
    297470
    … …  
    310483        if (body1)
    311484        {
    312                 vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
     485                vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2);
    313486                denom1 = body1->getInvMass() + normalAxis.dot(vec);
    314487        }
    … …  
    319492        solverConstraint.m_jacDiagABInv = denom;
    320493
    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 
    350494        return solverConstraint;
    351495}
    352496
    353 int     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 
    381 void    btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal)
    382 {
     497
     498
     499btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /*bodies */,int /*numBodies */,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
     500{
     501        BT_PROFILE("solveGroupCacheFriendlySetup");
     502        (void)stackAlloc;
     503        (void)debugDrawer;
     504
     505
     506        if (!(numConstraints + numManifolds))
     507        {
     508//              printf("empty\n");
     509                return 0.f;
     510        }
     511        btPersistentManifold* manifold = 0;
    383512        btCollisionObject* colObj0=0,*colObj1=0;
    384513
    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)
     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)
    449600                                        {
    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)
     601                                                if (colObj0->getCompanionId() >= 0)
    544602                                                {
    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;
     603                                                        //body has already been converted
     604                                                        solverBodyIdA = colObj0->getCompanionId();
    558605                                                } else
    559606                                                {
    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                                                         }
     607                                                        solverBodyIdA = m_tmpSolverBodyPool.size();
     608                                                        btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
     609                                                        initSolverBody(&solverBody,colObj0);
     610                                                        colObj0->setCompanionId(solverBodyIdA);
    613611                                                }
    614612                                        } else
    615613                                        {
    616                                                 btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex];
    617                                                 frictionConstraint1.m_appliedImpulse = 0.f;
    618                                                 if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
     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)
    619623                                                {
    620                                                         btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1];
    621                                                         frictionConstraint2.m_appliedImpulse = 0.f;
     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);
    622631                                                }
     632                                        } else
     633                                        {
     634                                                //create a static body
     635                                                solverBodyIdB = m_tmpSolverBodyPool.size();
     636                                                btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
     637                                                initSolverBody(&solverBody,colObj1);
    623638                                        }
    624639                                }
    625                         }
    626 
    627 
    628                 }
    629         }
    630 }
    631 
    632 
    633 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** /*bodies */,int /*numBodies */,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
    634 {
    635         BT_PROFILE("solveGroupCacheFriendlySetup");
    636         (void)stackAlloc;
    637         (void)debugDrawer;
    638 
    639 
    640         if (!(numConstraints + numManifolds))
    641         {
    642                 //              printf("empty\n");
    643                 return 0.f;
    644         }
    645 
    646         if (1)
     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
    647839        {
    648840                int j;
    … …  
    653845                }
    654846        }
    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();
     847       
     848       
     849
     850        int numConstraintPool = m_tmpSolverConstraintPool.size();
     851        int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
    812852
    813853        ///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
    … …  
    826866        }
    827867
     868
     869
    828870        return 0.f;
    829871
    … …  
    833875{
    834876        BT_PROFILE("solveGroupCacheFriendlyIterations");
    835 
    836         int numConstraintPool = m_tmpSolverContactConstraintPool.size();
    837         int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
     877        int numConstraintPool = m_tmpSolverConstraintPool.size();
     878        int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
    838879
    839880        //should traverse the contacts random order...
    … …  
    863904                        }
    864905
    865                         if (infoGlobal.m_solverMode & SOLVER_SIMD)
    866                         {
    867                                 ///solve all joint constraints, using SIMD, if available
    868                                 for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
    869                                 {
    870                                         btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
    871                                         resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
    872                                 }
    873 
    874                                 for (j=0;j<numConstraints;j++)
    875                                 {
    876                                         int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
    877                                         int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
    878                                         btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
    879                                         btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
    880                                         constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
    881                                 }
    882 
    883                                 ///solve all contact constraints using SIMD, if available
    884                                 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
     906                        for (j=0;j<numConstraints;j++)
     907                        {
     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
     931                        }
     932
     933                        {
     934                                int numPoolConstraints = m_tmpSolverConstraintPool.size();
    885935                                for (j=0;j<numPoolConstraints;j++)
    886936                                {
    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))
     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                }
     962       
     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++)
    899972                                        {
    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);
     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);
    904977                                        }
    905978                                }
    906                         } else
    907                         {
    908 
    909                                 ///solve all joint constraints
    910                                 for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
    911                                 {
    912                                         btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j];
    913                                         resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
    914                                 }
    915 
    916                                 for (j=0;j<numConstraints;j++)
    917                                 {
    918                                         int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA());
    919                                         int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB());
    920                                         btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
    921                                         btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
    922 
    923                                         constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
    924                                 }
    925 
    926                                 ///solve all contact constraints
    927                                 int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
    928                                 for (j=0;j<numPoolConstraints;j++)
    929                                 {
    930                                         const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
    931                                         resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
    932                                 }
    933                                 ///solve all friction constraints
    934                                 int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
    935                                 for (j=0;j<numFrictionPoolConstraints;j++)
    936                                 {
    937                                         btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
    938                                         btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
    939 
    940                                         if (totalImpulse>btScalar(0))
    941                                         {
    942                                                 solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
    943                                                 solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
    944 
    945                                                 resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],                                                   m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
    946                                         }
    947                                 }
    948                         }
    949 
    950 
    951 
    952                 }
    953         }
     979                        }
     980
     981                }
     982
     983        }
     984
    954985        return 0.f;
    955986}
    956987
    957988
    958 
    959 /// btSequentialImpulseConstraintSolver Sequentially applies impulses
    960 btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* /*dispatcher*/)
    961 {
    962 
    963        
    964 
    965         BT_PROFILE("solveGroup");
    966         //we only implement SOLVER_CACHE_FRIENDLY now
    967         //you need to provide at least some bodies
    968         btAssert(bodies);
    969         btAssert(numBodies);
    970 
     989btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
     990{
    971991        int i;
    972992
    … …  
    974994        solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
    975995
    976         int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
     996        int numPoolConstraints = m_tmpSolverConstraintPool.size();
    977997        int j;
    978 
    979998        for (j=0;j<numPoolConstraints;j++)
    980999        {
    981 
    982                 const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[j];
     1000               
     1001                const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[j];
    9831002                btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
    9841003                btAssert(pt);
    … …  
    9861005                if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING)
    9871006                {
    988                         pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
    989                         pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
     1007                        pt->m_appliedImpulseLateral1 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
     1008                        pt->m_appliedImpulseLateral2 = m_tmpSolverFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse;
    9901009                }
    9911010
    9921011                //do a callback here?
     1012
    9931013        }
    9941014
    … …  
    10021022        {
    10031023                for ( i=0;i<m_tmpSolverBodyPool.size();i++)
    1004                 {
    1005                         m_tmpSolverBodyPool[i].writebackVelocity();
    1006                 }
    1007         }
    1008 
     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*/
    10091041
    10101042        m_tmpSolverBodyPool.resize(0);
    1011         m_tmpSolverContactConstraintPool.resize(0);
    1012         m_tmpSolverNonContactConstraintPool.resize(0);
    1013         m_tmpSolverContactFrictionConstraintPool.resize(0);
     1043        m_tmpSolverConstraintPool.resize(0);
     1044        m_tmpSolverFrictionConstraintPool.resize(0);
     1045
    10141046
    10151047        return 0.f;
    10161048}
    10171049
    1018 
    1019 
    1020 
    1021 
    1022 
    1023 
     1050/// btSequentialImpulseConstraintSolver Sequentially applies impulses
     1051btScalar 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
     1153void    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
     1347btScalar 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
     1380btScalar 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
     1412btScalar 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}
    10241441
    10251442
Note: See TracChangeset for help on using the changeset viewer.