Back to index

supertuxkart  0.5+dfsg1
btSequentialImpulseConstraintSolver.cpp
Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00015 
00016 //#define COMPUTE_IMPULSE_DENOM 1
00017 //It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
00018 //#define FORCE_REFESH_CONTACT_MANIFOLDS 1
00019 
00020 #include "btSequentialImpulseConstraintSolver.h"
00021 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
00022 #include "BulletDynamics/Dynamics/btRigidBody.h"
00023 #include "btContactConstraint.h"
00024 #include "btSolve2LinearConstraint.h"
00025 #include "btContactSolverInfo.h"
00026 #include "LinearMath/btIDebugDraw.h"
00027 #include "btJacobianEntry.h"
00028 #include "LinearMath/btMinMax.h"
00029 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
00030 #include <new>
00031 #include "LinearMath/btStackAlloc.h"
00032 #include "LinearMath/btQuickprof.h"
00033 #include "btSolverBody.h"
00034 #include "btSolverConstraint.h"
00035 
00036 
00037 #include "LinearMath/btAlignedObjectArray.h"
00038 
00039 
00040 int totalCpd = 0;
00041 
00042 int    gTotalContactPoints = 0;
00043 
00044 struct btOrderIndex
00045 {
00046        int    m_manifoldIndex;
00047        int    m_pointIndex;
00048 };
00049 
00050 
00051 
00052 #define SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS 16384
00053 static btOrderIndex  gOrder[SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS];
00054 
00055 
00056 unsigned long btSequentialImpulseConstraintSolver::btRand2()
00057 {
00058   m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
00059   return m_btSeed2;
00060 }
00061 
00062 
00063 
00064 //See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
00065 int btSequentialImpulseConstraintSolver::btRandInt2 (int n)
00066 {
00067   // seems good; xor-fold and modulus
00068   const unsigned long un = n;
00069   unsigned long r = btRand2();
00070 
00071   // note: probably more aggressive than it needs to be -- might be
00072   //       able to get away without one or two of the innermost branches.
00073   if (un <= 0x00010000UL) {
00074     r ^= (r >> 16);
00075     if (un <= 0x00000100UL) {
00076       r ^= (r >> 8);
00077       if (un <= 0x00000010UL) {
00078         r ^= (r >> 4);
00079         if (un <= 0x00000004UL) {
00080           r ^= (r >> 2);
00081           if (un <= 0x00000002UL) {
00082             r ^= (r >> 1);
00083           }
00084         }
00085      }
00086     }
00087    }
00088 
00089   return (int) (r % un);
00090 }
00091 
00092 
00093 
00094 
00095 
00096 bool  MyContactDestroyedCallback(void* userPersistentData)
00097 {
00098        assert (userPersistentData);
00099        btConstraintPersistentData* cpd = (btConstraintPersistentData*)userPersistentData;
00100        btAlignedFree(cpd);
00101        totalCpd--;
00102        //printf("totalCpd = %i. DELETED Ptr %x\n",totalCpd,userPersistentData);
00103        return true;
00104 }
00105 
00106 
00107 
00108 btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver()
00109 :m_solverMode(SOLVER_RANDMIZE_ORDER | SOLVER_CACHE_FRIENDLY | SOLVER_USE_WARMSTARTING ),
00110 m_btSeed2(0)
00111 {
00112        gContactDestroyedCallback = &MyContactDestroyedCallback;
00113 
00114        //initialize default friction/contact funcs
00115        int i,j;
00116        for (i=0;i<MAX_CONTACT_SOLVER_TYPES;i++)
00117               for (j=0;j<MAX_CONTACT_SOLVER_TYPES;j++)
00118               {
00119 
00120                      m_contactDispatch[i][j] = resolveSingleCollision;
00121                      m_frictionDispatch[i][j] = resolveSingleFriction;
00122               }
00123 }
00124 
00125 btSequentialImpulseConstraintSolver::~btSequentialImpulseConstraintSolver()
00126 {
00127 
00128 }
00129 
00130 void   initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject)
00131 {
00132        btRigidBody* rb = btRigidBody::upcast(collisionObject);
00133        if (rb)
00134        {
00135               solverBody->m_angularVelocity = rb->getAngularVelocity() ;
00136               solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin();
00137               solverBody->m_friction = collisionObject->getFriction();
00138               solverBody->m_invMass = rb->getInvMass();
00139               solverBody->m_linearVelocity = rb->getLinearVelocity();
00140               solverBody->m_originalBody = rb;
00141               solverBody->m_angularFactor = rb->getAngularFactor();
00142        } else
00143        {
00144               solverBody->m_angularVelocity.setValue(0,0,0);
00145               solverBody->m_centerOfMassPosition = collisionObject->getWorldTransform().getOrigin();
00146               solverBody->m_friction = collisionObject->getFriction();
00147               solverBody->m_invMass = 0.f;
00148               solverBody->m_linearVelocity.setValue(0,0,0);
00149               solverBody->m_originalBody = 0;
00150               solverBody->m_angularFactor = 1.f;
00151        }
00152 }
00153 
00154 btScalar penetrationResolveFactor = btScalar(0.9);
00155 btScalar restitutionCurve(btScalar rel_vel, btScalar restitution)
00156 {
00157        btScalar rest = restitution * -rel_vel;
00158        return rest;
00159 }
00160 
00161 
00162 
00163 
00164 
00165 
00166 //velocity + friction
00167 //response  between two dynamic objects with friction
00168 //SIMD_FORCE_INLINE 
00169 btScalar resolveSingleCollisionCombinedCacheFriendly(
00170        btSolverBody& body1,
00171        btSolverBody& body2,
00172        const btSolverConstraint& contactConstraint,
00173        const btContactSolverInfo& solverInfo)
00174 {
00175        (void)solverInfo;
00176 
00177        btScalar normalImpulse;
00178        
00179        //  Optimized version of projected relative velocity, use precomputed cross products with normal
00180        //     body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
00181        //     body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
00182        //     btVector3 vel = vel1 - vel2;
00183        //     btScalar  rel_vel = contactConstraint.m_contactNormal.dot(vel);
00184 
00185        btScalar rel_vel;
00186        btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity) 
00187                             + contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
00188        btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity) 
00189                             + contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);
00190 
00191        rel_vel = vel1Dotn-vel2Dotn;
00192 
00193 
00194        btScalar positionalError = contactConstraint.m_penetration;
00195        btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;
00196 
00197        btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
00198        btScalar      velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
00199        normalImpulse = penetrationImpulse+velocityImpulse;
00200        
00201        // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
00202        btScalar oldNormalImpulse = contactConstraint.m_appliedImpulse;
00203        btScalar sum = oldNormalImpulse + normalImpulse;
00204        contactConstraint.m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
00205 
00206        btScalar oldVelocityImpulse = contactConstraint.m_appliedVelocityImpulse;
00207        btScalar velocitySum = oldVelocityImpulse + velocityImpulse;
00208        contactConstraint.m_appliedVelocityImpulse = btScalar(0.) > velocitySum ? btScalar(0.): velocitySum;
00209 
00210        normalImpulse = contactConstraint.m_appliedImpulse - oldNormalImpulse;
00211 
00212        if (body1.m_invMass)
00213        {
00214               body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,
00215                      contactConstraint.m_angularComponentA,normalImpulse);
00216        }
00217        if (body2.m_invMass)
00218        {
00219               body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,
00220                      contactConstraint.m_angularComponentB,-normalImpulse);
00221        }
00222 
00223 
00224 
00225        
00226 
00227        return normalImpulse;
00228 }
00229 
00230 
00231 #ifndef NO_FRICTION_TANGENTIALS
00232 
00233 //SIMD_FORCE_INLINE 
00234 btScalar resolveSingleFrictionCacheFriendly(
00235        btSolverBody& body1,
00236        btSolverBody& body2,
00237        const btSolverConstraint& contactConstraint,
00238        const btContactSolverInfo& solverInfo,
00239        btScalar appliedNormalImpulse)
00240 {
00241        (void)solverInfo;
00242 
00243        
00244        const btScalar combinedFriction = contactConstraint.m_friction;
00245        
00246        const btScalar limit = appliedNormalImpulse * combinedFriction;
00247        
00248        if (appliedNormalImpulse>btScalar(0.))
00249        //friction
00250        {
00251               
00252               btScalar j1;
00253               {
00254 
00255                      btScalar rel_vel;
00256                      const btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity) 
00257                                           + contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
00258                      const btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity) 
00259                             + contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);
00260                      rel_vel = vel1Dotn-vel2Dotn;
00261 
00262                      // calculate j that moves us to zero relative velocity
00263                      j1 = -rel_vel * contactConstraint.m_jacDiagABInv;
00264 #define CLAMP_ACCUMULATED_FRICTION_IMPULSE 1
00265 #ifdef CLAMP_ACCUMULATED_FRICTION_IMPULSE
00266                      btScalar oldTangentImpulse = contactConstraint.m_appliedImpulse;
00267                      contactConstraint.m_appliedImpulse = oldTangentImpulse + j1;
00268                      
00269                      if (limit < contactConstraint.m_appliedImpulse)
00270                      {
00271                             contactConstraint.m_appliedImpulse = limit;
00272                      } else
00273                      {
00274                             if (contactConstraint.m_appliedImpulse < -limit)
00275                                    contactConstraint.m_appliedImpulse = -limit;
00276                      }
00277                      j1 = contactConstraint.m_appliedImpulse - oldTangentImpulse;
00278 #else
00279                      if (limit < j1)
00280                      {
00281                             j1 = limit;
00282                      } else
00283                      {
00284                             if (j1 < -limit)
00285                                    j1 = -limit;
00286                      }
00287 
00288 #endif //CLAMP_ACCUMULATED_FRICTION_IMPULSE
00289 
00290                      //GEN_set_min(contactConstraint.m_appliedImpulse, limit);
00291                      //GEN_set_max(contactConstraint.m_appliedImpulse, -limit);
00292 
00293                      
00294 
00295               }
00296        
00297               if (body1.m_invMass)
00298               {
00299                      body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,contactConstraint.m_angularComponentA,j1);
00300               }
00301               if (body2.m_invMass)
00302               {
00303                      body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,contactConstraint.m_angularComponentB,-j1);
00304               }
00305 
00306        } 
00307        return 0.f;
00308 }
00309 
00310 
00311 #else
00312 
00313 //velocity + friction
00314 //response  between two dynamic objects with friction
00315 btScalar resolveSingleFrictionCacheFriendly(
00316        btSolverBody& body1,
00317        btSolverBody& body2,
00318        btSolverConstraint& contactConstraint,
00319        const btContactSolverInfo& solverInfo)
00320 {
00321 
00322        btVector3 vel1;
00323        btVector3 vel2;
00324        btScalar normalImpulse(0.f);
00325 
00326        {
00327               const btVector3& normal = contactConstraint.m_contactNormal;
00328               if (contactConstraint.m_penetration < 0.f)
00329                      return 0.f;
00330 
00331 
00332               body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
00333               body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
00334               btVector3 vel = vel1 - vel2;
00335               btScalar rel_vel;
00336               rel_vel = normal.dot(vel);
00337 
00338               btVector3 lat_vel = vel - normal * rel_vel;
00339               btScalar lat_rel_vel = lat_vel.length2();
00340 
00341               btScalar combinedFriction = contactConstraint.m_friction;
00342               const btVector3& rel_pos1 = contactConstraint.m_rel_posA;
00343               const btVector3& rel_pos2 = contactConstraint.m_rel_posB;
00344 
00345 
00346               //if (contactConstraint.m_appliedVelocityImpulse > 0.f)
00347               if (lat_rel_vel > SIMD_EPSILON*SIMD_EPSILON)
00348               {
00349                      lat_rel_vel = btSqrt(lat_rel_vel);
00350 
00351                      lat_vel /= lat_rel_vel;
00352                      btVector3 temp1 = body1.m_invInertiaWorld * rel_pos1.cross(lat_vel);
00353                      btVector3 temp2 = body2.m_invInertiaWorld * rel_pos2.cross(lat_vel);
00354                      btScalar friction_impulse = lat_rel_vel /
00355                             (body1.m_invMass + body2.m_invMass + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
00356                      btScalar normal_impulse = contactConstraint.m_appliedVelocityImpulse * combinedFriction;
00357 
00358                      GEN_set_min(friction_impulse, normal_impulse);
00359                      GEN_set_max(friction_impulse, -normal_impulse);
00360                      body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1);
00361                      body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);
00362               }
00363        }
00364 
00365        return normalImpulse;
00366 }
00367 
00368 #endif //NO_FRICTION_TANGENTIALS
00369 
00370 
00371 
00372 
00373 
00374 void   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)
00375 {
00376 
00377        btRigidBody* body0=btRigidBody::upcast(colObj0);
00378        btRigidBody* body1=btRigidBody::upcast(colObj1);
00379 
00380        btSolverConstraint& solverConstraint = m_tmpSolverFrictionConstraintPool.expand();
00381        solverConstraint.m_contactNormal = normalAxis;
00382 
00383        solverConstraint.m_solverBodyIdA = solverBodyIdA;
00384        solverConstraint.m_solverBodyIdB = solverBodyIdB;
00385        solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D;
00386        solverConstraint.m_frictionIndex = frictionIndex;
00387 
00388        solverConstraint.m_friction = cp.m_combinedFriction;
00389        solverConstraint.m_originalContactPoint = 0;
00390 
00391        solverConstraint.m_appliedImpulse = btScalar(0.);
00392        solverConstraint.m_appliedVelocityImpulse = 0.f;
00393        solverConstraint.m_penetration = 0.f;
00394        {
00395               btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
00396               solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
00397               solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0);
00398        }
00399        {
00400               btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal);
00401               solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
00402               solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0);
00403        }
00404 
00405 #ifdef COMPUTE_IMPULSE_DENOM
00406        btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
00407        btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
00408 #else
00409        btVector3 vec;
00410        btScalar denom0 = 0.f;
00411        btScalar denom1 = 0.f;
00412        if (body0)
00413        {
00414               vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
00415               denom0 = body0->getInvMass() + normalAxis.dot(vec);
00416        }
00417        if (body1)
00418        {
00419               vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2);
00420               denom1 = body1->getInvMass() + normalAxis.dot(vec);
00421        }
00422 
00423 
00424 #endif //COMPUTE_IMPULSE_DENOM
00425        btScalar denom = relaxation/(denom0+denom1);
00426        solverConstraint.m_jacDiagABInv = denom;
00427 
00428 
00429 }
00430 
00431 
00432 
00433 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
00434 {
00435        BT_PROFILE("solveGroupCacheFriendlySetup");
00436        (void)stackAlloc;
00437        (void)debugDrawer;
00438 
00439 
00440        if (!(numConstraints + numManifolds))
00441        {
00442 //            printf("empty\n");
00443               return 0.f;
00444        }
00445        btPersistentManifold* manifold = 0;
00446        btCollisionObject* colObj0=0,*colObj1=0;
00447 
00448        //btRigidBody* rb0=0,*rb1=0;
00449 
00450 
00451 #ifdef FORCE_REFESH_CONTACT_MANIFOLDS
00452 
00453        BEGIN_PROFILE("refreshManifolds");
00454 
00455        int i;
00456        
00457        
00458 
00459        for (i=0;i<numManifolds;i++)
00460        {
00461               manifold = manifoldPtr[i];
00462               rb1 = (btRigidBody*)manifold->getBody1();
00463               rb0 = (btRigidBody*)manifold->getBody0();
00464               
00465               manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
00466 
00467        }
00468 
00469        END_PROFILE("refreshManifolds");
00470 #endif //FORCE_REFESH_CONTACT_MANIFOLDS
00471 
00472        btVector3 color(0,1,0);
00473 
00474 
00475 
00476        //int sizeofSB = sizeof(btSolverBody);
00477        //int sizeofSC = sizeof(btSolverConstraint);
00478 
00479 
00480        //if (1)
00481        {
00482               //if m_stackAlloc, try to pack bodies/constraints to speed up solving
00483 //            btBlock*                                  sablock;
00484 //            sablock = stackAlloc->beginBlock();
00485 
00486        //     int memsize = 16;
00487 //            unsigned char* stackMemory = stackAlloc->allocate(memsize);
00488 
00489               
00490               //todo: use stack allocator for this temp memory
00491               int minReservation = numManifolds*2;
00492 
00493               //m_tmpSolverBodyPool.reserve(minReservation);
00494 
00495               //don't convert all bodies, only the one we need so solver the constraints
00496 /*
00497               {
00498                      for (int i=0;i<numBodies;i++)
00499                      {
00500                             btRigidBody* rb = btRigidBody::upcast(bodies[i]);
00501                             if (rb &&     (rb->getIslandTag() >= 0))
00502                             {
00503                                    btAssert(rb->getCompanionId() < 0);
00504                                    int solverBodyId = m_tmpSolverBodyPool.size();
00505                                    btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
00506                                    initSolverBody(&solverBody,rb);
00507                                    rb->setCompanionId(solverBodyId);
00508                             } 
00509                      }
00510               }
00511 */
00512               
00513               //m_tmpSolverConstraintPool.reserve(minReservation);
00514               //m_tmpSolverFrictionConstraintPool.reserve(minReservation);
00515 
00516               {
00517                      int i;
00518 
00519                      for (i=0;i<numManifolds;i++)
00520                      {
00521                             manifold = manifoldPtr[i];
00522                             colObj0 = (btCollisionObject*)manifold->getBody0();
00523                             colObj1 = (btCollisionObject*)manifold->getBody1();
00524                      
00525                             int solverBodyIdA=-1;
00526                             int solverBodyIdB=-1;
00527 
00528                             if (manifold->getNumContacts())
00529                             {
00530 
00531                                    
00532 
00533                                    if (colObj0->getIslandTag() >= 0)
00534                                    {
00535                                           if (colObj0->getCompanionId() >= 0)
00536                                           {
00537                                                  //body has already been converted
00538                                                  solverBodyIdA = colObj0->getCompanionId();
00539                                           } else
00540                                           {
00541                                                  solverBodyIdA = m_tmpSolverBodyPool.size();
00542                                                  btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
00543                                                  initSolverBody(&solverBody,colObj0);
00544                                                  colObj0->setCompanionId(solverBodyIdA);
00545                                           }
00546                                    } else
00547                                    {
00548                                           //create a static body
00549                                           solverBodyIdA = m_tmpSolverBodyPool.size();
00550                                           btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
00551                                           initSolverBody(&solverBody,colObj0);
00552                                    }
00553 
00554                                    if (colObj1->getIslandTag() >= 0)
00555                                    {
00556                                           if (colObj1->getCompanionId() >= 0)
00557                                           {
00558                                                  solverBodyIdB = colObj1->getCompanionId();
00559                                           } else
00560                                           {
00561                                                  solverBodyIdB = m_tmpSolverBodyPool.size();
00562                                                  btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
00563                                                  initSolverBody(&solverBody,colObj1);
00564                                                  colObj1->setCompanionId(solverBodyIdB);
00565                                           }
00566                                    } else
00567                                    {
00568                                           //create a static body
00569                                           solverBodyIdB = m_tmpSolverBodyPool.size();
00570                                           btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
00571                                           initSolverBody(&solverBody,colObj1);
00572                                    }
00573                             }
00574 
00575                             btVector3 rel_pos1;
00576                             btVector3 rel_pos2;
00577                             btScalar relaxation;
00578 
00579                             for (int j=0;j<manifold->getNumContacts();j++)
00580                             {
00581                                    
00582                                    btManifoldPoint& cp = manifold->getContactPoint(j);
00583                                    
00584                                    if (debugDrawer)
00585                                           debugDrawer->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
00586 
00587                                    
00588                                    if (cp.getDistance() <= btScalar(0.))
00589                                    {
00590                                           
00591                                           const btVector3& pos1 = cp.getPositionWorldOnA();
00592                                           const btVector3& pos2 = cp.getPositionWorldOnB();
00593 
00594                                            rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); 
00595                                            rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();
00596 
00597                                           
00598                                           relaxation = 1.f;
00599                                           btScalar rel_vel;
00600                                           btVector3 vel;
00601 
00602                                           int frictionIndex = m_tmpSolverConstraintPool.size();
00603 
00604                                           {
00605                                                  btSolverConstraint& solverConstraint = m_tmpSolverConstraintPool.expand();
00606                                                  btRigidBody* rb0 = btRigidBody::upcast(colObj0);
00607                                                  btRigidBody* rb1 = btRigidBody::upcast(colObj1);
00608 
00609                                                  solverConstraint.m_solverBodyIdA = solverBodyIdA;
00610                                                  solverConstraint.m_solverBodyIdB = solverBodyIdB;
00611                                                  solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_CONTACT_1D;
00612 
00613                                                  solverConstraint.m_originalContactPoint = &cp;
00614 
00615                                                  btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
00616                                                  solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0 : btVector3(0,0,0);
00617                                                  btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);          
00618                                                  solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*torqueAxis1 : btVector3(0,0,0);
00619                                                  {
00620 #ifdef COMPUTE_IMPULSE_DENOM
00621                                                         btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
00622                                                         btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
00623 #else                                            
00624                                                         btVector3 vec;
00625                                                         btScalar denom0 = 0.f;
00626                                                         btScalar denom1 = 0.f;
00627                                                         if (rb0)
00628                                                         {
00629                                                                vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
00630                                                                denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
00631                                                         }
00632                                                         if (rb1)
00633                                                         {
00634                                                                vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2);
00635                                                                denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
00636                                                         }
00637 #endif //COMPUTE_IMPULSE_DENOM            
00638                                                         
00639                                                         btScalar denom = relaxation/(denom0+denom1);
00640                                                         solverConstraint.m_jacDiagABInv = denom;
00641                                                  }
00642 
00643                                                  solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
00644                                                  solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB);
00645                                                  solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(cp.m_normalWorldOnB);
00646 
00647 
00648                                                  btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
00649                                                  btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
00650                             
00651                                                  vel  = vel1 - vel2;
00652                                                  
00653                                                  rel_vel = cp.m_normalWorldOnB.dot(vel);
00654                                                  
00655 
00656                                                  solverConstraint.m_penetration = cp.getDistance();
00657                                                  solverConstraint.m_friction = cp.m_combinedFriction;
00658                                                  solverConstraint.m_restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
00659                                                  if (solverConstraint.m_restitution <= btScalar(0.))
00660                                                  {
00661                                                         solverConstraint.m_restitution = 0.f;
00662                                                  };
00663                                                  
00664                                                  btScalar penVel = -solverConstraint.m_penetration/infoGlobal.m_timeStep;
00665                                                  solverConstraint.m_penetration *= -(infoGlobal.m_erp/infoGlobal.m_timeStep);
00666 
00667                                                  if (solverConstraint.m_restitution > penVel)
00668                                                  {
00669                                                         solverConstraint.m_penetration = btScalar(0.);
00670                                                  }
00671                                                  
00672                                                  
00673 
00675                                                  if (m_solverMode & SOLVER_USE_WARMSTARTING)
00676                                                  {
00677                                                         solverConstraint.m_appliedImpulse = cp.m_appliedImpulse;
00678                                                  } else
00679                                                  {
00680                                                         solverConstraint.m_appliedImpulse = 0.f;
00681                                                  }
00682                                                  solverConstraint.m_appliedVelocityImpulse = 0.f;
00683                                                  
00684                                    
00685                                           
00686                                           }
00687 
00688                                    
00689                                           {
00690                                                  btVector3 frictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
00691                                                  btScalar lat_rel_vel = frictionDir1.length2();
00692                                                  if (lat_rel_vel > SIMD_EPSILON)//0.0f)
00693                                                  {
00694                                                         frictionDir1 /= btSqrt(lat_rel_vel);
00695                                                         addFrictionConstraint(frictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
00696                                                         btVector3 frictionDir2 = frictionDir1.cross(cp.m_normalWorldOnB);
00697                                                         frictionDir2.normalize();//??
00698                                                         addFrictionConstraint(frictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
00699                                                  } else
00700                                                  {
00701                                                         //re-calculate friction direction every frame, todo: check if this is really needed
00702                                                         btVector3     frictionDir1,frictionDir2;
00703                                                         btPlaneSpace1(cp.m_normalWorldOnB,frictionDir1,frictionDir2);
00704                                                         addFrictionConstraint(frictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
00705                                                         addFrictionConstraint(frictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
00706                                                  }
00707 
00708                                           }
00709                                    
00710 
00711                                    }
00712                             }
00713                      }
00714               }
00715        }
00716        
00717        btContactSolverInfo info = infoGlobal;
00718 
00719        {
00720               int j;
00721               for (j=0;j<numConstraints;j++)
00722               {
00723                      btTypedConstraint* constraint = constraints[j];
00724                      constraint->buildJacobian();
00725               }
00726        }
00727        
00728        
00729 
00730        int numConstraintPool = m_tmpSolverConstraintPool.size();
00731        int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
00732 
00734        m_orderTmpConstraintPool.resize(numConstraintPool);
00735        m_orderFrictionConstraintPool.resize(numFrictionPool);
00736        {
00737               int i;
00738               for (i=0;i<numConstraintPool;i++)
00739               {
00740                      m_orderTmpConstraintPool[i] = i;
00741               }
00742               for (i=0;i<numFrictionPool;i++)
00743               {
00744                      m_orderFrictionConstraintPool[i] = i;
00745               }
00746        }
00747 
00748 
00749 
00750        return 0.f;
00751 
00752 }
00753 
00754 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
00755 {
00756        BT_PROFILE("solveGroupCacheFriendlyIterations");
00757        int numConstraintPool = m_tmpSolverConstraintPool.size();
00758        int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();
00759 
00760        //should traverse the contacts random order...
00761        int iteration;
00762        {
00763               for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
00764               {                    
00765 
00766                      int j;
00767                      if (m_solverMode & SOLVER_RANDMIZE_ORDER)
00768                      {
00769                             if ((iteration & 7) == 0) {
00770                                    for (j=0; j<numConstraintPool; ++j) {
00771                                           int tmp = m_orderTmpConstraintPool[j];
00772                                           int swapi = btRandInt2(j+1);
00773                                           m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
00774                                           m_orderTmpConstraintPool[swapi] = tmp;
00775                                    }
00776 
00777                                    for (j=0; j<numFrictionPool; ++j) {
00778                                           int tmp = m_orderFrictionConstraintPool[j];
00779                                           int swapi = btRandInt2(j+1);
00780                                           m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
00781                                           m_orderFrictionConstraintPool[swapi] = tmp;
00782                                    }
00783                             }
00784                      }
00785 
00786                      for (j=0;j<numConstraints;j++)
00787                      {
00788                             btTypedConstraint* constraint = constraints[j];
00790 
00791                             if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0))
00792                             {
00793                                    m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].writebackVelocity();
00794                             }
00795                             if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0))
00796                             {
00797                                    m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].writebackVelocity();
00798                             }
00799 
00800                             constraint->solveConstraint(infoGlobal.m_timeStep);
00801 
00802                             if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0))
00803                             {
00804                                    m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].readVelocity();
00805                             }
00806                             if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0))
00807                             {
00808                                    m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].readVelocity();
00809                             }
00810 
00811                      }
00812 
00813                      {
00814                             int numPoolConstraints = m_tmpSolverConstraintPool.size();
00815                             for (j=0;j<numPoolConstraints;j++)
00816                             {
00817                                    
00818                                    const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]];
00819                                    resolveSingleCollisionCombinedCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
00820                                           m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal);
00821                             }
00822                      }
00823 
00824                      {
00825                              int numFrictionPoolConstraints = m_tmpSolverFrictionConstraintPool.size();
00826                             
00827                              for (j=0;j<numFrictionPoolConstraints;j++)
00828                             {
00829                                    const btSolverConstraint& solveManifold = m_tmpSolverFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
00830                                           
00831 
00832                                           resolveSingleFrictionCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
00833                                                  m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal,
00834                                                  m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse);
00835                             }
00836                      }
00837                      
00838 
00839 
00840               }
00841        }
00842 
00843        {
00844               int numPoolConstraints = m_tmpSolverConstraintPool.size();
00845               int j;
00846               for (j=0;j<numPoolConstraints;j++)
00847               {
00848                      
00849                      const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[j];
00850                      btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
00851                      btAssert(pt);
00852                      pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
00853                      //do a callback here?
00854 
00855               }
00856        }
00857        
00858               return 0.f;
00859 }
00860 
00861 
00862 btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc)
00863 {
00864        int i;
00865 
00866        solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
00867        solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
00868 
00869               
00870        for ( i=0;i<m_tmpSolverBodyPool.size();i++)
00871        {
00872               m_tmpSolverBodyPool[i].writebackVelocity();
00873        }
00874 
00875 
00876 //     printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size());
00877 
00878 /*
00879        printf("m_tmpSolverBodyPool.size() = %i\n",m_tmpSolverBodyPool.size());
00880        printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size());
00881        printf("m_tmpSolverFrictionConstraintPool.size() = %i\n",m_tmpSolverFrictionConstraintPool.size());
00882 
00883        
00884        printf("m_tmpSolverBodyPool.capacity() = %i\n",m_tmpSolverBodyPool.capacity());
00885        printf("m_tmpSolverConstraintPool.capacity() = %i\n",m_tmpSolverConstraintPool.capacity());
00886        printf("m_tmpSolverFrictionConstraintPool.capacity() = %i\n",m_tmpSolverFrictionConstraintPool.capacity());
00887 */
00888 
00889        m_tmpSolverBodyPool.resize(0);
00890        m_tmpSolverConstraintPool.resize(0);
00891        m_tmpSolverFrictionConstraintPool.resize(0);
00892 
00893 
00894        return 0.f;
00895 }
00896 
00898 btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* dispatcher)
00899 {
00900        BT_PROFILE("solveGroup");
00901        if (getSolverMode() & SOLVER_CACHE_FRIENDLY)
00902        {
00903               //you need to provide at least some bodies
00904               //btSimpleDynamicsWorld needs to switch off SOLVER_CACHE_FRIENDLY
00905               btAssert(bodies);
00906               btAssert(numBodies);
00907               return solveGroupCacheFriendly(bodies,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
00908        }
00909 
00910        
00911 
00912        btContactSolverInfo info = infoGlobal;
00913 
00914        int numiter = infoGlobal.m_numIterations;
00915 
00916        int totalPoints = 0;
00917 
00918 
00919        {
00920               short j;
00921               for (j=0;j<numManifolds;j++)
00922               {
00923                      btPersistentManifold* manifold = manifoldPtr[j];
00924                      prepareConstraints(manifold,info,debugDrawer);
00925 
00926                      for (short p=0;p<manifoldPtr[j]->getNumContacts();p++)
00927                      {
00928                             gOrder[totalPoints].m_manifoldIndex = j;
00929                             gOrder[totalPoints].m_pointIndex = p;
00930                             totalPoints++;
00931                      }
00932               }
00933        }
00934 
00935        {
00936               int j;
00937               for (j=0;j<numConstraints;j++)
00938               {
00939                      btTypedConstraint* constraint = constraints[j];
00940                      constraint->buildJacobian();
00941               }
00942        }
00943        
00944 
00945        //should traverse the contacts random order...
00946        int iteration;
00947 
00948        {
00949               for ( iteration = 0;iteration<numiter;iteration++)
00950               {
00951                      int j;
00952                      if (m_solverMode & SOLVER_RANDMIZE_ORDER)
00953                      {
00954                             if ((iteration & 7) == 0) {
00955                                    for (j=0; j<totalPoints; ++j) {
00956                                           btOrderIndex tmp = gOrder[j];
00957                                           int swapi = btRandInt2(j+1);
00958                                           gOrder[j] = gOrder[swapi];
00959                                           gOrder[swapi] = tmp;
00960                                    }
00961                             }
00962                      }
00963 
00964                      for (j=0;j<numConstraints;j++)
00965                      {
00966                             btTypedConstraint* constraint = constraints[j];
00967                             constraint->solveConstraint(info.m_timeStep);
00968                      }
00969 
00970                      for (j=0;j<totalPoints;j++)
00971                      {
00972                             btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
00973                             solve( (btRigidBody*)manifold->getBody0(),
00974                                                                (btRigidBody*)manifold->getBody1()
00975                             ,manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
00976                      }
00977               
00978                      for (j=0;j<totalPoints;j++)
00979                      {
00980                             btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
00981                             solveFriction((btRigidBody*)manifold->getBody0(),
00982                                    (btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
00983                      }
00984                      
00985               }
00986        }
00987               
00988 
00989 
00990 
00991        return btScalar(0.);
00992 }
00993 
00994 
00995 
00996 
00997 
00998 
00999 
01000 void   btSequentialImpulseConstraintSolver::prepareConstraints(btPersistentManifold* manifoldPtr, const btContactSolverInfo& info,btIDebugDraw* debugDrawer)
01001 {
01002 
01003        (void)debugDrawer;
01004 
01005        btRigidBody* body0 = (btRigidBody*)manifoldPtr->getBody0();
01006        btRigidBody* body1 = (btRigidBody*)manifoldPtr->getBody1();
01007 
01008 
01009        //only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
01010        {
01011 #ifdef FORCE_REFESH_CONTACT_MANIFOLDS
01012               manifoldPtr->refreshContactPoints(body0->getCenterOfMassTransform(),body1->getCenterOfMassTransform());
01013 #endif //FORCE_REFESH_CONTACT_MANIFOLDS          
01014               int numpoints = manifoldPtr->getNumContacts();
01015 
01016               gTotalContactPoints += numpoints;
01017 
01018               btVector3 color(0,1,0);
01019               for (int i=0;i<numpoints ;i++)
01020               {
01021                      btManifoldPoint& cp = manifoldPtr->getContactPoint(i);
01022                      if (cp.getDistance() <= btScalar(0.))
01023                      {
01024                             const btVector3& pos1 = cp.getPositionWorldOnA();
01025                             const btVector3& pos2 = cp.getPositionWorldOnB();
01026 
01027                             btVector3 rel_pos1 = pos1 - body0->getCenterOfMassPosition(); 
01028                             btVector3 rel_pos2 = pos2 - body1->getCenterOfMassPosition();
01029                             
01030 
01031                             //this jacobian entry is re-used for all iterations
01032                             btJacobianEntry jac(body0->getCenterOfMassTransform().getBasis().transpose(),
01033                                    body1->getCenterOfMassTransform().getBasis().transpose(),
01034                                    rel_pos1,rel_pos2,cp.m_normalWorldOnB,body0->getInvInertiaDiagLocal(),body0->getInvMass(),
01035                                    body1->getInvInertiaDiagLocal(),body1->getInvMass());
01036 
01037                             
01038                             btScalar jacDiagAB = jac.getDiagonal();
01039 
01040                             btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
01041                             if (cpd)
01042                             {
01043                                    //might be invalid
01044                                    cpd->m_persistentLifeTime++;
01045                                    if (cpd->m_persistentLifeTime != cp.getLifeTime())
01046                                    {
01047                                           //printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
01048                                           new (cpd) btConstraintPersistentData;
01049                                           cpd->m_persistentLifeTime = cp.getLifeTime();
01050 
01051                                    } else
01052                                    {
01053                                           //printf("Persistent: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
01054                                           
01055                                    }
01056                             } else
01057                             {
01058                                           
01059                                    //todo: should this be in a pool?
01060                                    void* mem = btAlignedAlloc(sizeof(btConstraintPersistentData),16);
01061                                    cpd = new (mem)btConstraintPersistentData;
01062                                    assert(cpd);
01063 
01064                                    totalCpd ++;
01065                                    //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd);
01066                                    cp.m_userPersistentData = cpd;
01067                                    cpd->m_persistentLifeTime = cp.getLifeTime();
01068                                    //printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime());
01069                                    
01070                             }
01071                             assert(cpd);
01072 
01073                             cpd->m_jacDiagABInv = btScalar(1.) / jacDiagAB;
01074 
01075                             //Dependent on Rigidbody A and B types, fetch the contact/friction response func
01076                             //perhaps do a similar thing for friction/restutution combiner funcs...
01077                             
01078                             cpd->m_frictionSolverFunc = m_frictionDispatch[body0->m_frictionSolverType][body1->m_frictionSolverType];
01079                             cpd->m_contactSolverFunc = m_contactDispatch[body0->m_contactSolverType][body1->m_contactSolverType];
01080                             
01081                             btVector3 vel1 = body0->getVelocityInLocalPoint(rel_pos1);
01082                             btVector3 vel2 = body1->getVelocityInLocalPoint(rel_pos2);
01083                             btVector3 vel = vel1 - vel2;
01084                             btScalar rel_vel;
01085                             rel_vel = cp.m_normalWorldOnB.dot(vel);
01086                             
01087                             btScalar combinedRestitution = cp.m_combinedRestitution;
01088                             
01089                             cpd->m_penetration = cp.getDistance();
01090                             cpd->m_friction = cp.m_combinedFriction;
01091                             cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution);
01092                             if (cpd->m_restitution <= btScalar(0.))
01093                             {
01094                                    cpd->m_restitution = btScalar(0.0);
01095 
01096                             };
01097                             
01098                             //restitution and penetration work in same direction so
01099                             //rel_vel 
01100                             
01101                             btScalar penVel = -cpd->m_penetration/info.m_timeStep;
01102 
01103                             if (cpd->m_restitution > penVel)
01104                             {
01105                                    cpd->m_penetration = btScalar(0.);
01106                             }                           
01107                             
01108 
01109                             btScalar relaxation = info.m_damping;
01110                             if (m_solverMode & SOLVER_USE_WARMSTARTING)
01111                             {
01112                                    cpd->m_appliedImpulse *= relaxation;
01113                             } else
01114                             {
01115                                    cpd->m_appliedImpulse =btScalar(0.);
01116                             }
01117        
01118                             //for friction
01119                             cpd->m_prevAppliedImpulse = cpd->m_appliedImpulse;
01120                             
01121                             //re-calculate friction direction every frame, todo: check if this is really needed
01122                             btPlaneSpace1(cp.m_normalWorldOnB,cpd->m_frictionWorldTangential0,cpd->m_frictionWorldTangential1);
01123 
01124 
01125 #define NO_FRICTION_WARMSTART 1
01126 
01127        #ifdef NO_FRICTION_WARMSTART
01128                             cpd->m_accumulatedTangentImpulse0 = btScalar(0.);
01129                             cpd->m_accumulatedTangentImpulse1 = btScalar(0.);
01130        #endif //NO_FRICTION_WARMSTART
01131                             btScalar denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential0);
01132                             btScalar denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential0);
01133                             btScalar denom = relaxation/(denom0+denom1);
01134                             cpd->m_jacDiagABInvTangent0 = denom;
01135 
01136 
01137                             denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential1);
01138                             denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential1);
01139                             denom = relaxation/(denom0+denom1);
01140                             cpd->m_jacDiagABInvTangent1 = denom;
01141 
01142                             btVector3 totalImpulse = 
01143        #ifndef NO_FRICTION_WARMSTART
01144                                    cpd->m_frictionWorldTangential0*cpd->m_accumulatedTangentImpulse0+
01145                                    cpd->m_frictionWorldTangential1*cpd->m_accumulatedTangentImpulse1+
01146        #endif //NO_FRICTION_WARMSTART
01147                                    cp.m_normalWorldOnB*cpd->m_appliedImpulse;
01148 
01149 
01150 
01152                             {
01153                             btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
01154                             cpd->m_angularComponentA = body0->getInvInertiaTensorWorld()*torqueAxis0;
01155                             btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);          
01156                             cpd->m_angularComponentB = body1->getInvInertiaTensorWorld()*torqueAxis1;
01157                             }
01158                             {
01159                                    btVector3 ftorqueAxis0 = rel_pos1.cross(cpd->m_frictionWorldTangential0);
01160                                    cpd->m_frictionAngularComponent0A = body0->getInvInertiaTensorWorld()*ftorqueAxis0;
01161                             }
01162                             {
01163                                    btVector3 ftorqueAxis1 = rel_pos1.cross(cpd->m_frictionWorldTangential1);
01164                                    cpd->m_frictionAngularComponent1A = body0->getInvInertiaTensorWorld()*ftorqueAxis1;
01165                             }
01166                             {
01167                                    btVector3 ftorqueAxis0 = rel_pos2.cross(cpd->m_frictionWorldTangential0);
01168                                    cpd->m_frictionAngularComponent0B = body1->getInvInertiaTensorWorld()*ftorqueAxis0;
01169                             }
01170                             {
01171                                    btVector3 ftorqueAxis1 = rel_pos2.cross(cpd->m_frictionWorldTangential1);
01172                                    cpd->m_frictionAngularComponent1B = body1->getInvInertiaTensorWorld()*ftorqueAxis1;
01173                             }
01174                             
01176 
01177 
01178 
01179                             //apply previous frames impulse on both bodies
01180                             body0->applyImpulse(totalImpulse, rel_pos1);
01181                             body1->applyImpulse(-totalImpulse, rel_pos2);
01182                      }
01183                      
01184               }
01185        }
01186 }
01187 
01188 
01189 btScalar btSequentialImpulseConstraintSolver::solveCombinedContactFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
01190 {
01191        btScalar maxImpulse = btScalar(0.);
01192 
01193        {
01194 
01195               btVector3 color(0,1,0);
01196               {
01197                      if (cp.getDistance() <= btScalar(0.))
01198                      {
01199 
01200                             if (iter == 0)
01201                             {
01202                                    if (debugDrawer)
01203                                           debugDrawer->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
01204                             }
01205 
01206                             {
01207 
01208                                    //btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
01209                                    btScalar impulse = resolveSingleCollisionCombined(
01210                                           *body0,*body1,
01211                                           cp,
01212                                           info);
01213 
01214                                    if (maxImpulse < impulse)
01215                                           maxImpulse  = impulse;
01216 
01217                             }
01218                      }
01219               }
01220        }
01221        return maxImpulse;
01222 }
01223 
01224 
01225 
01226 btScalar btSequentialImpulseConstraintSolver::solve(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
01227 {
01228 
01229        btScalar maxImpulse = btScalar(0.);
01230 
01231        {
01232 
01233               btVector3 color(0,1,0);
01234               {
01235                      if (cp.getDistance() <= btScalar(0.))
01236                      {
01237 
01238                             if (iter == 0)
01239                             {
01240                                    if (debugDrawer)
01241                                           debugDrawer->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
01242                             }
01243 
01244                             {
01245 
01246                                    btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
01247                                    btScalar impulse = cpd->m_contactSolverFunc(
01248                                           *body0,*body1,
01249                                           cp,
01250                                           info);
01251 
01252                                    if (maxImpulse < impulse)
01253                                           maxImpulse  = impulse;
01254 
01255                             }
01256                      }
01257               }
01258        }
01259        return maxImpulse;
01260 }
01261 
01262 btScalar btSequentialImpulseConstraintSolver::solveFriction(btRigidBody* body0,btRigidBody* body1, btManifoldPoint& cp, const btContactSolverInfo& info,int iter,btIDebugDraw* debugDrawer)
01263 {
01264 
01265        (void)debugDrawer;
01266        (void)iter;
01267 
01268 
01269        {
01270 
01271               btVector3 color(0,1,0);
01272               {
01273                      
01274                      if (cp.getDistance() <= btScalar(0.))
01275                      {
01276 
01277                             btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
01278                             cpd->m_frictionSolverFunc(
01279                                    *body0,*body1,
01280                                    cp,
01281                                    info);
01282 
01283                             
01284                      }
01285               }
01286 
01287        
01288        }
01289        return btScalar(0.);
01290 }
01291 
01292 
01293 void   btSequentialImpulseConstraintSolver::reset()
01294 {
01295        m_btSeed2 = 0;
01296 }
01297