Back to index

supertuxkart  0.5+dfsg1
Public Types | Public Member Functions | Protected Member Functions | Protected Attributes | Private Attributes
btSequentialImpulseConstraintSolver Class Reference

btSequentialImpulseConstraintSolver uses a Propagation Method and Sequentially applies impulses The approach is the 3D version of Erin Catto's GDC 2006 tutorial. See http://www.gphysics.com Although Sequential Impulse is more intuitive, it is mathematically equivalent to Projected Successive Overrelaxation (iterative LCP) Applies impulses for combined restitution and penetration recovery and to simulate friction More...

#include <btSequentialImpulseConstraintSolver.h>

Inheritance diagram for btSequentialImpulseConstraintSolver:
Inheritance graph
[legend]
Collaboration diagram for btSequentialImpulseConstraintSolver:
Collaboration graph
[legend]

List of all members.

Public Types

enum  eSolverMode { SOLVER_RANDMIZE_ORDER = 1, SOLVER_FRICTION_SEPARATE = 2, SOLVER_USE_WARMSTARTING = 4, SOLVER_CACHE_FRIENDLY = 8 }

Public Member Functions

 btSequentialImpulseConstraintSolver ()
void setContactSolverFunc (ContactSolverFunc func, int type0, int type1)
 Advanced: Override the default contact solving function for contacts, for certain types of rigidbody See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType.
void SetFrictionSolverFunc (ContactSolverFunc func, int type0, int type1)
 Advanced: Override the default friction solving function for contacts, for certain types of rigidbody See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType.
virtual ~btSequentialImpulseConstraintSolver ()
virtual btScalar solveGroup (btCollisionObject **bodies, int numBodies, btPersistentManifold **manifold, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &info, btIDebugDraw *debugDrawer, btStackAlloc *stackAlloc, btDispatcher *dispatcher)
 btSequentialImpulseConstraintSolver Sequentially applies impulses
virtual btScalar solveGroupCacheFriendly (btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer, btStackAlloc *stackAlloc)
btScalar solveGroupCacheFriendlyIterations (btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer, btStackAlloc *stackAlloc)
btScalar solveGroupCacheFriendlySetup (btCollisionObject **bodies, int numBodies, btPersistentManifold **manifoldPtr, int numManifolds, btTypedConstraint **constraints, int numConstraints, const btContactSolverInfo &infoGlobal, btIDebugDraw *debugDrawer, btStackAlloc *stackAlloc)
virtual void reset ()
 clear internal cached data and reset random seed
btScalar solveCombinedContactFriction (btRigidBody *body0, btRigidBody *body1, btManifoldPoint &cp, const btContactSolverInfo &info, int iter, btIDebugDraw *debugDrawer)
void setSolverMode (int mode)
int getSolverMode () const
unsigned long btRand2 ()
int btRandInt2 (int n)
void setRandSeed (unsigned long seed)
unsigned long getRandSeed () const
virtual void prepareSolve (int numBodies, int numManifolds)
virtual void allSolved (const btContactSolverInfo &info, class btIDebugDraw *debugDrawer, btStackAlloc *stackAlloc)

Protected Member Functions

btScalar solve (btRigidBody *body0, btRigidBody *body1, btManifoldPoint &cp, const btContactSolverInfo &info, int iter, btIDebugDraw *debugDrawer)
btScalar solveFriction (btRigidBody *body0, btRigidBody *body1, btManifoldPoint &cp, const btContactSolverInfo &info, int iter, btIDebugDraw *debugDrawer)
void prepareConstraints (btPersistentManifold *manifoldPtr, const btContactSolverInfo &info, btIDebugDraw *debugDrawer)
void 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)

Protected Attributes

ContactSolverFunc m_contactDispatch [MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES]
ContactSolverFunc m_frictionDispatch [MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES]
int m_solverMode
unsigned long m_btSeed2
 m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction

Private Attributes

btAlignedObjectArray
< btSolverBody > 
m_tmpSolverBodyPool
btAlignedObjectArray
< btSolverConstraint > 
m_tmpSolverConstraintPool
btAlignedObjectArray
< btSolverConstraint > 
m_tmpSolverFrictionConstraintPool
btAlignedObjectArray< int > m_orderTmpConstraintPool
btAlignedObjectArray< int > m_orderFrictionConstraintPool

Detailed Description

btSequentialImpulseConstraintSolver uses a Propagation Method and Sequentially applies impulses The approach is the 3D version of Erin Catto's GDC 2006 tutorial. See http://www.gphysics.com Although Sequential Impulse is more intuitive, it is mathematically equivalent to Projected Successive Overrelaxation (iterative LCP) Applies impulses for combined restitution and penetration recovery and to simulate friction

Definition at line 30 of file btSequentialImpulseConstraintSolver.h.


Member Enumeration Documentation

Enumerator:
SOLVER_RANDMIZE_ORDER 
SOLVER_FRICTION_SEPARATE 
SOLVER_USE_WARMSTARTING 
SOLVER_CACHE_FRIENDLY 

Definition at line 56 of file btSequentialImpulseConstraintSolver.h.


Constructor & Destructor Documentation

Definition at line 108 of file btSequentialImpulseConstraintSolver.cpp.

Here is the call graph for this function:

Definition at line 125 of file btSequentialImpulseConstraintSolver.cpp.

{

}

Member Function Documentation

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 
) [protected]

Definition at line 374 of file btSequentialImpulseConstraintSolver.cpp.

{

       btRigidBody* body0=btRigidBody::upcast(colObj0);
       btRigidBody* body1=btRigidBody::upcast(colObj1);

       btSolverConstraint& solverConstraint = m_tmpSolverFrictionConstraintPool.expand();
       solverConstraint.m_contactNormal = normalAxis;

       solverConstraint.m_solverBodyIdA = solverBodyIdA;
       solverConstraint.m_solverBodyIdB = solverBodyIdB;
       solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_FRICTION_1D;
       solverConstraint.m_frictionIndex = frictionIndex;

       solverConstraint.m_friction = cp.m_combinedFriction;
       solverConstraint.m_originalContactPoint = 0;

       solverConstraint.m_appliedImpulse = btScalar(0.);
       solverConstraint.m_appliedVelocityImpulse = 0.f;
       solverConstraint.m_penetration = 0.f;
       {
              btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
              solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
              solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0);
       }
       {
              btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal);
              solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
              solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1 : btVector3(0,0,0);
       }

#ifdef COMPUTE_IMPULSE_DENOM
       btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
       btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
#else
       btVector3 vec;
       btScalar denom0 = 0.f;
       btScalar denom1 = 0.f;
       if (body0)
       {
              vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
              denom0 = body0->getInvMass() + normalAxis.dot(vec);
       }
       if (body1)
       {
              vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2);
              denom1 = body1->getInvMass() + normalAxis.dot(vec);
       }


#endif //COMPUTE_IMPULSE_DENOM
       btScalar denom = relaxation/(denom0+denom1);
       solverConstraint.m_jacDiagABInv = denom;


}

Here is the call graph for this function:

Here is the caller graph for this function:

virtual void btConstraintSolver::allSolved ( const btContactSolverInfo info,
class btIDebugDraw debugDrawer,
btStackAlloc stackAlloc 
) [inline, virtual, inherited]

Definition at line 43 of file btConstraintSolver.h.

{;}

Here is the caller graph for this function:

Definition at line 56 of file btSequentialImpulseConstraintSolver.cpp.

{
  m_btSeed2 = (1664525L*m_btSeed2 + 1013904223L) & 0xffffffff;
  return m_btSeed2;
}

Here is the caller graph for this function:

Definition at line 65 of file btSequentialImpulseConstraintSolver.cpp.

{
  // seems good; xor-fold and modulus
  const unsigned long un = n;
  unsigned long r = btRand2();

  // note: probably more aggressive than it needs to be -- might be
  //       able to get away without one or two of the innermost branches.
  if (un <= 0x00010000UL) {
    r ^= (r >> 16);
    if (un <= 0x00000100UL) {
      r ^= (r >> 8);
      if (un <= 0x00000010UL) {
        r ^= (r >> 4);
        if (un <= 0x00000004UL) {
          r ^= (r >> 2);
          if (un <= 0x00000002UL) {
            r ^= (r >> 1);
          }
        }
     }
    }
   }

  return (int) (r % un);
}

Here is the call graph for this function:

Here is the caller graph for this function:

unsigned long btSequentialImpulseConstraintSolver::getRandSeed ( ) const [inline]

Definition at line 113 of file btSequentialImpulseConstraintSolver.h.

       {
              return m_btSeed2;
       }

Definition at line 100 of file btSequentialImpulseConstraintSolver.h.

       {
              return m_solverMode;
       }

Here is the caller graph for this function:

void btSequentialImpulseConstraintSolver::prepareConstraints ( btPersistentManifold *  manifoldPtr,
const btContactSolverInfo info,
btIDebugDraw debugDrawer 
) [protected]

btScalar(info.m_numIterations);

Definition at line 1000 of file btSequentialImpulseConstraintSolver.cpp.

{

       (void)debugDrawer;

       btRigidBody* body0 = (btRigidBody*)manifoldPtr->getBody0();
       btRigidBody* body1 = (btRigidBody*)manifoldPtr->getBody1();


       //only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
       {
#ifdef FORCE_REFESH_CONTACT_MANIFOLDS
              manifoldPtr->refreshContactPoints(body0->getCenterOfMassTransform(),body1->getCenterOfMassTransform());
#endif //FORCE_REFESH_CONTACT_MANIFOLDS          
              int numpoints = manifoldPtr->getNumContacts();

              gTotalContactPoints += numpoints;

              btVector3 color(0,1,0);
              for (int i=0;i<numpoints ;i++)
              {
                     btManifoldPoint& cp = manifoldPtr->getContactPoint(i);
                     if (cp.getDistance() <= btScalar(0.))
                     {
                            const btVector3& pos1 = cp.getPositionWorldOnA();
                            const btVector3& pos2 = cp.getPositionWorldOnB();

                            btVector3 rel_pos1 = pos1 - body0->getCenterOfMassPosition(); 
                            btVector3 rel_pos2 = pos2 - body1->getCenterOfMassPosition();
                            

                            //this jacobian entry is re-used for all iterations
                            btJacobianEntry jac(body0->getCenterOfMassTransform().getBasis().transpose(),
                                   body1->getCenterOfMassTransform().getBasis().transpose(),
                                   rel_pos1,rel_pos2,cp.m_normalWorldOnB,body0->getInvInertiaDiagLocal(),body0->getInvMass(),
                                   body1->getInvInertiaDiagLocal(),body1->getInvMass());

                            
                            btScalar jacDiagAB = jac.getDiagonal();

                            btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
                            if (cpd)
                            {
                                   //might be invalid
                                   cpd->m_persistentLifeTime++;
                                   if (cpd->m_persistentLifeTime != cp.getLifeTime())
                                   {
                                          //printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
                                          new (cpd) btConstraintPersistentData;
                                          cpd->m_persistentLifeTime = cp.getLifeTime();

                                   } else
                                   {
                                          //printf("Persistent: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
                                          
                                   }
                            } else
                            {
                                          
                                   //todo: should this be in a pool?
                                   void* mem = btAlignedAlloc(sizeof(btConstraintPersistentData),16);
                                   cpd = new (mem)btConstraintPersistentData;
                                   assert(cpd);

                                   totalCpd ++;
                                   //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd);
                                   cp.m_userPersistentData = cpd;
                                   cpd->m_persistentLifeTime = cp.getLifeTime();
                                   //printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime());
                                   
                            }
                            assert(cpd);

                            cpd->m_jacDiagABInv = btScalar(1.) / jacDiagAB;

                            //Dependent on Rigidbody A and B types, fetch the contact/friction response func
                            //perhaps do a similar thing for friction/restutution combiner funcs...
                            
                            cpd->m_frictionSolverFunc = m_frictionDispatch[body0->m_frictionSolverType][body1->m_frictionSolverType];
                            cpd->m_contactSolverFunc = m_contactDispatch[body0->m_contactSolverType][body1->m_contactSolverType];
                            
                            btVector3 vel1 = body0->getVelocityInLocalPoint(rel_pos1);
                            btVector3 vel2 = body1->getVelocityInLocalPoint(rel_pos2);
                            btVector3 vel = vel1 - vel2;
                            btScalar rel_vel;
                            rel_vel = cp.m_normalWorldOnB.dot(vel);
                            
                            btScalar combinedRestitution = cp.m_combinedRestitution;
                            
                            cpd->m_penetration = cp.getDistance();
                            cpd->m_friction = cp.m_combinedFriction;
                            cpd->m_restitution = restitutionCurve(rel_vel, combinedRestitution);
                            if (cpd->m_restitution <= btScalar(0.))
                            {
                                   cpd->m_restitution = btScalar(0.0);

                            };
                            
                            //restitution and penetration work in same direction so
                            //rel_vel 
                            
                            btScalar penVel = -cpd->m_penetration/info.m_timeStep;

                            if (cpd->m_restitution > penVel)
                            {
                                   cpd->m_penetration = btScalar(0.);
                            }                           
                            

                            btScalar relaxation = info.m_damping;
                            if (m_solverMode & SOLVER_USE_WARMSTARTING)
                            {
                                   cpd->m_appliedImpulse *= relaxation;
                            } else
                            {
                                   cpd->m_appliedImpulse =btScalar(0.);
                            }
       
                            //for friction
                            cpd->m_prevAppliedImpulse = cpd->m_appliedImpulse;
                            
                            //re-calculate friction direction every frame, todo: check if this is really needed
                            btPlaneSpace1(cp.m_normalWorldOnB,cpd->m_frictionWorldTangential0,cpd->m_frictionWorldTangential1);


#define NO_FRICTION_WARMSTART 1

       #ifdef NO_FRICTION_WARMSTART
                            cpd->m_accumulatedTangentImpulse0 = btScalar(0.);
                            cpd->m_accumulatedTangentImpulse1 = btScalar(0.);
       #endif //NO_FRICTION_WARMSTART
                            btScalar denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential0);
                            btScalar denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential0);
                            btScalar denom = relaxation/(denom0+denom1);
                            cpd->m_jacDiagABInvTangent0 = denom;


                            denom0 = body0->computeImpulseDenominator(pos1,cpd->m_frictionWorldTangential1);
                            denom1 = body1->computeImpulseDenominator(pos2,cpd->m_frictionWorldTangential1);
                            denom = relaxation/(denom0+denom1);
                            cpd->m_jacDiagABInvTangent1 = denom;

                            btVector3 totalImpulse = 
       #ifndef NO_FRICTION_WARMSTART
                                   cpd->m_frictionWorldTangential0*cpd->m_accumulatedTangentImpulse0+
                                   cpd->m_frictionWorldTangential1*cpd->m_accumulatedTangentImpulse1+
       #endif //NO_FRICTION_WARMSTART
                                   cp.m_normalWorldOnB*cpd->m_appliedImpulse;



                            {
                            btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
                            cpd->m_angularComponentA = body0->getInvInertiaTensorWorld()*torqueAxis0;
                            btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);          
                            cpd->m_angularComponentB = body1->getInvInertiaTensorWorld()*torqueAxis1;
                            }
                            {
                                   btVector3 ftorqueAxis0 = rel_pos1.cross(cpd->m_frictionWorldTangential0);
                                   cpd->m_frictionAngularComponent0A = body0->getInvInertiaTensorWorld()*ftorqueAxis0;
                            }
                            {
                                   btVector3 ftorqueAxis1 = rel_pos1.cross(cpd->m_frictionWorldTangential1);
                                   cpd->m_frictionAngularComponent1A = body0->getInvInertiaTensorWorld()*ftorqueAxis1;
                            }
                            {
                                   btVector3 ftorqueAxis0 = rel_pos2.cross(cpd->m_frictionWorldTangential0);
                                   cpd->m_frictionAngularComponent0B = body1->getInvInertiaTensorWorld()*ftorqueAxis0;
                            }
                            {
                                   btVector3 ftorqueAxis1 = rel_pos2.cross(cpd->m_frictionWorldTangential1);
                                   cpd->m_frictionAngularComponent1B = body1->getInvInertiaTensorWorld()*ftorqueAxis1;
                            }
                            



                            //apply previous frames impulse on both bodies
                            body0->applyImpulse(totalImpulse, rel_pos1);
                            body1->applyImpulse(-totalImpulse, rel_pos2);
                     }
                     
              }
       }
}

Here is the call graph for this function:

Here is the caller graph for this function:

virtual void btConstraintSolver::prepareSolve ( int  numBodies,
int  numManifolds 
) [inline, virtual, inherited]

Definition at line 38 of file btConstraintSolver.h.

{;}

Here is the caller graph for this function:

clear internal cached data and reset random seed

Implements btConstraintSolver.

Definition at line 1293 of file btSequentialImpulseConstraintSolver.cpp.

{
       m_btSeed2 = 0;
}
void btSequentialImpulseConstraintSolver::setContactSolverFunc ( ContactSolverFunc  func,
int  type0,
int  type1 
) [inline]

Advanced: Override the default contact solving function for contacts, for certain types of rigidbody See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType.

Definition at line 68 of file btSequentialImpulseConstraintSolver.h.

       {
              m_contactDispatch[type0][type1] = func;
       }
void btSequentialImpulseConstraintSolver::SetFrictionSolverFunc ( ContactSolverFunc  func,
int  type0,
int  type1 
) [inline]

Advanced: Override the default friction solving function for contacts, for certain types of rigidbody See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType.

Definition at line 75 of file btSequentialImpulseConstraintSolver.h.

       {
              m_frictionDispatch[type0][type1] = func;
       }
void btSequentialImpulseConstraintSolver::setRandSeed ( unsigned long  seed) [inline]

Definition at line 109 of file btSequentialImpulseConstraintSolver.h.

       {
              m_btSeed2 = seed;
       }

Definition at line 95 of file btSequentialImpulseConstraintSolver.h.

       {
              m_solverMode = mode;
       }
btScalar btSequentialImpulseConstraintSolver::solve ( btRigidBody body0,
btRigidBody body1,
btManifoldPoint cp,
const btContactSolverInfo info,
int  iter,
btIDebugDraw debugDrawer 
) [protected]

Definition at line 1226 of file btSequentialImpulseConstraintSolver.cpp.

{

       btScalar maxImpulse = btScalar(0.);

       {

              btVector3 color(0,1,0);
              {
                     if (cp.getDistance() <= btScalar(0.))
                     {

                            if (iter == 0)
                            {
                                   if (debugDrawer)
                                          debugDrawer->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
                            }

                            {

                                   btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
                                   btScalar impulse = cpd->m_contactSolverFunc(
                                          *body0,*body1,
                                          cp,
                                          info);

                                   if (maxImpulse < impulse)
                                          maxImpulse  = impulse;

                            }
                     }
              }
       }
       return maxImpulse;
}

Here is the call graph for this function:

Here is the caller graph for this function:

Definition at line 1189 of file btSequentialImpulseConstraintSolver.cpp.

{
       btScalar maxImpulse = btScalar(0.);

       {

              btVector3 color(0,1,0);
              {
                     if (cp.getDistance() <= btScalar(0.))
                     {

                            if (iter == 0)
                            {
                                   if (debugDrawer)
                                          debugDrawer->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
                            }

                            {

                                   //btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
                                   btScalar impulse = resolveSingleCollisionCombined(
                                          *body0,*body1,
                                          cp,
                                          info);

                                   if (maxImpulse < impulse)
                                          maxImpulse  = impulse;

                            }
                     }
              }
       }
       return maxImpulse;
}

Here is the call graph for this function:

btScalar btSequentialImpulseConstraintSolver::solveFriction ( btRigidBody body0,
btRigidBody body1,
btManifoldPoint cp,
const btContactSolverInfo info,
int  iter,
btIDebugDraw debugDrawer 
) [protected]

Definition at line 1262 of file btSequentialImpulseConstraintSolver.cpp.

{

       (void)debugDrawer;
       (void)iter;


       {

              btVector3 color(0,1,0);
              {
                     
                     if (cp.getDistance() <= btScalar(0.))
                     {

                            btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
                            cpd->m_frictionSolverFunc(
                                   *body0,*body1,
                                   cp,
                                   info);

                            
                     }
              }

       
       }
       return btScalar(0.);
}

Here is the call graph for this function:

Here is the caller graph for this function:

btScalar btSequentialImpulseConstraintSolver::solveGroup ( btCollisionObject **  bodies,
int  numBodies,
btPersistentManifold **  manifold,
int  numManifolds,
btTypedConstraint **  constraints,
int  numConstraints,
const btContactSolverInfo info,
btIDebugDraw debugDrawer,
btStackAlloc stackAlloc,
btDispatcher dispatcher 
) [virtual]

btSequentialImpulseConstraintSolver Sequentially applies impulses

Implements btConstraintSolver.

Reimplemented in Physics.

Definition at line 898 of file btSequentialImpulseConstraintSolver.cpp.

{
       BT_PROFILE("solveGroup");
       if (getSolverMode() & SOLVER_CACHE_FRIENDLY)
       {
              //you need to provide at least some bodies
              //btSimpleDynamicsWorld needs to switch off SOLVER_CACHE_FRIENDLY
              btAssert(bodies);
              btAssert(numBodies);
              return solveGroupCacheFriendly(bodies,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc);
       }

       

       btContactSolverInfo info = infoGlobal;

       int numiter = infoGlobal.m_numIterations;

       int totalPoints = 0;


       {
              short j;
              for (j=0;j<numManifolds;j++)
              {
                     btPersistentManifold* manifold = manifoldPtr[j];
                     prepareConstraints(manifold,info,debugDrawer);

                     for (short p=0;p<manifoldPtr[j]->getNumContacts();p++)
                     {
                            gOrder[totalPoints].m_manifoldIndex = j;
                            gOrder[totalPoints].m_pointIndex = p;
                            totalPoints++;
                     }
              }
       }

       {
              int j;
              for (j=0;j<numConstraints;j++)
              {
                     btTypedConstraint* constraint = constraints[j];
                     constraint->buildJacobian();
              }
       }
       

       //should traverse the contacts random order...
       int iteration;

       {
              for ( iteration = 0;iteration<numiter;iteration++)
              {
                     int j;
                     if (m_solverMode & SOLVER_RANDMIZE_ORDER)
                     {
                            if ((iteration & 7) == 0) {
                                   for (j=0; j<totalPoints; ++j) {
                                          btOrderIndex tmp = gOrder[j];
                                          int swapi = btRandInt2(j+1);
                                          gOrder[j] = gOrder[swapi];
                                          gOrder[swapi] = tmp;
                                   }
                            }
                     }

                     for (j=0;j<numConstraints;j++)
                     {
                            btTypedConstraint* constraint = constraints[j];
                            constraint->solveConstraint(info.m_timeStep);
                     }

                     for (j=0;j<totalPoints;j++)
                     {
                            btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
                            solve( (btRigidBody*)manifold->getBody0(),
                                                               (btRigidBody*)manifold->getBody1()
                            ,manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
                     }
              
                     for (j=0;j<totalPoints;j++)
                     {
                            btPersistentManifold* manifold = manifoldPtr[gOrder[j].m_manifoldIndex];
                            solveFriction((btRigidBody*)manifold->getBody0(),
                                   (btRigidBody*)manifold->getBody1(),manifold->getContactPoint(gOrder[j].m_pointIndex),info,iteration,debugDrawer);
                     }
                     
              }
       }
              



       return btScalar(0.);
}

Here is the call graph for this function:

btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly ( btCollisionObject **  bodies,
int  numBodies,
btPersistentManifold **  manifoldPtr,
int  numManifolds,
btTypedConstraint **  constraints,
int  numConstraints,
const btContactSolverInfo infoGlobal,
btIDebugDraw debugDrawer,
btStackAlloc stackAlloc 
) [virtual]

Definition at line 862 of file btSequentialImpulseConstraintSolver.cpp.

{
       int i;

       solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
       solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);

              
       for ( i=0;i<m_tmpSolverBodyPool.size();i++)
       {
              m_tmpSolverBodyPool[i].writebackVelocity();
       }


//     printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size());

/*
       printf("m_tmpSolverBodyPool.size() = %i\n",m_tmpSolverBodyPool.size());
       printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size());
       printf("m_tmpSolverFrictionConstraintPool.size() = %i\n",m_tmpSolverFrictionConstraintPool.size());

       
       printf("m_tmpSolverBodyPool.capacity() = %i\n",m_tmpSolverBodyPool.capacity());
       printf("m_tmpSolverConstraintPool.capacity() = %i\n",m_tmpSolverConstraintPool.capacity());
       printf("m_tmpSolverFrictionConstraintPool.capacity() = %i\n",m_tmpSolverFrictionConstraintPool.capacity());
*/

       m_tmpSolverBodyPool.resize(0);
       m_tmpSolverConstraintPool.resize(0);
       m_tmpSolverFrictionConstraintPool.resize(0);


       return 0.f;
}

Here is the call graph for this function:

Here is the caller graph for this function:

btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations ( btCollisionObject **  bodies,
int  numBodies,
btPersistentManifold **  manifoldPtr,
int  numManifolds,
btTypedConstraint **  constraints,
int  numConstraints,
const btContactSolverInfo infoGlobal,
btIDebugDraw debugDrawer,
btStackAlloc stackAlloc 
)

todo: use solver bodies, so we don't need to copy from/to btRigidBody

Definition at line 754 of file btSequentialImpulseConstraintSolver.cpp.

{
       BT_PROFILE("solveGroupCacheFriendlyIterations");
       int numConstraintPool = m_tmpSolverConstraintPool.size();
       int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();

       //should traverse the contacts random order...
       int iteration;
       {
              for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
              {                    

                     int j;
                     if (m_solverMode & SOLVER_RANDMIZE_ORDER)
                     {
                            if ((iteration & 7) == 0) {
                                   for (j=0; j<numConstraintPool; ++j) {
                                          int tmp = m_orderTmpConstraintPool[j];
                                          int swapi = btRandInt2(j+1);
                                          m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
                                          m_orderTmpConstraintPool[swapi] = tmp;
                                   }

                                   for (j=0; j<numFrictionPool; ++j) {
                                          int tmp = m_orderFrictionConstraintPool[j];
                                          int swapi = btRandInt2(j+1);
                                          m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
                                          m_orderFrictionConstraintPool[swapi] = tmp;
                                   }
                            }
                     }

                     for (j=0;j<numConstraints;j++)
                     {
                            btTypedConstraint* constraint = constraints[j];

                            if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0))
                            {
                                   m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].writebackVelocity();
                            }
                            if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0))
                            {
                                   m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].writebackVelocity();
                            }

                            constraint->solveConstraint(infoGlobal.m_timeStep);

                            if ((constraint->getRigidBodyA().getIslandTag() >= 0) && (constraint->getRigidBodyA().getCompanionId() >= 0))
                            {
                                   m_tmpSolverBodyPool[constraint->getRigidBodyA().getCompanionId()].readVelocity();
                            }
                            if ((constraint->getRigidBodyB().getIslandTag() >= 0) && (constraint->getRigidBodyB().getCompanionId() >= 0))
                            {
                                   m_tmpSolverBodyPool[constraint->getRigidBodyB().getCompanionId()].readVelocity();
                            }

                     }

                     {
                            int numPoolConstraints = m_tmpSolverConstraintPool.size();
                            for (j=0;j<numPoolConstraints;j++)
                            {
                                   
                                   const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]];
                                   resolveSingleCollisionCombinedCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
                                          m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal);
                            }
                     }

                     {
                             int numFrictionPoolConstraints = m_tmpSolverFrictionConstraintPool.size();
                            
                             for (j=0;j<numFrictionPoolConstraints;j++)
                            {
                                   const btSolverConstraint& solveManifold = m_tmpSolverFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
                                          

                                          resolveSingleFrictionCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
                                                 m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal,
                                                 m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse);
                            }
                     }
                     


              }
       }

       {
              int numPoolConstraints = m_tmpSolverConstraintPool.size();
              int j;
              for (j=0;j<numPoolConstraints;j++)
              {
                     
                     const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[j];
                     btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
                     btAssert(pt);
                     pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
                     //do a callback here?

              }
       }
       
              return 0.f;
}

Here is the call graph for this function:

Here is the caller graph for this function:

btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup ( btCollisionObject **  bodies,
int  numBodies,
btPersistentManifold **  manifoldPtr,
int  numManifolds,
btTypedConstraint **  constraints,
int  numConstraints,
const btContactSolverInfo infoGlobal,
btIDebugDraw debugDrawer,
btStackAlloc stackAlloc 
)

btScalar(infoGlobal.m_numIterations);

warm starting (or zero if disabled)

todo: use stack allocator for such temporarily memory, same for solver bodies/constraints

Definition at line 433 of file btSequentialImpulseConstraintSolver.cpp.

{
       BT_PROFILE("solveGroupCacheFriendlySetup");
       (void)stackAlloc;
       (void)debugDrawer;


       if (!(numConstraints + numManifolds))
       {
//            printf("empty\n");
              return 0.f;
       }
       btPersistentManifold* manifold = 0;
       btCollisionObject* colObj0=0,*colObj1=0;

       //btRigidBody* rb0=0,*rb1=0;


#ifdef FORCE_REFESH_CONTACT_MANIFOLDS

       BEGIN_PROFILE("refreshManifolds");

       int i;
       
       

       for (i=0;i<numManifolds;i++)
       {
              manifold = manifoldPtr[i];
              rb1 = (btRigidBody*)manifold->getBody1();
              rb0 = (btRigidBody*)manifold->getBody0();
              
              manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());

       }

       END_PROFILE("refreshManifolds");
#endif //FORCE_REFESH_CONTACT_MANIFOLDS

       btVector3 color(0,1,0);



       //int sizeofSB = sizeof(btSolverBody);
       //int sizeofSC = sizeof(btSolverConstraint);


       //if (1)
       {
              //if m_stackAlloc, try to pack bodies/constraints to speed up solving
//            btBlock*                                  sablock;
//            sablock = stackAlloc->beginBlock();

       //     int memsize = 16;
//            unsigned char* stackMemory = stackAlloc->allocate(memsize);

              
              //todo: use stack allocator for this temp memory
              int minReservation = numManifolds*2;

              //m_tmpSolverBodyPool.reserve(minReservation);

              //don't convert all bodies, only the one we need so solver the constraints
/*
              {
                     for (int i=0;i<numBodies;i++)
                     {
                            btRigidBody* rb = btRigidBody::upcast(bodies[i]);
                            if (rb &&     (rb->getIslandTag() >= 0))
                            {
                                   btAssert(rb->getCompanionId() < 0);
                                   int solverBodyId = m_tmpSolverBodyPool.size();
                                   btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
                                   initSolverBody(&solverBody,rb);
                                   rb->setCompanionId(solverBodyId);
                            } 
                     }
              }
*/
              
              //m_tmpSolverConstraintPool.reserve(minReservation);
              //m_tmpSolverFrictionConstraintPool.reserve(minReservation);

              {
                     int i;

                     for (i=0;i<numManifolds;i++)
                     {
                            manifold = manifoldPtr[i];
                            colObj0 = (btCollisionObject*)manifold->getBody0();
                            colObj1 = (btCollisionObject*)manifold->getBody1();
                     
                            int solverBodyIdA=-1;
                            int solverBodyIdB=-1;

                            if (manifold->getNumContacts())
                            {

                                   

                                   if (colObj0->getIslandTag() >= 0)
                                   {
                                          if (colObj0->getCompanionId() >= 0)
                                          {
                                                 //body has already been converted
                                                 solverBodyIdA = colObj0->getCompanionId();
                                          } else
                                          {
                                                 solverBodyIdA = m_tmpSolverBodyPool.size();
                                                 btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
                                                 initSolverBody(&solverBody,colObj0);
                                                 colObj0->setCompanionId(solverBodyIdA);
                                          }
                                   } else
                                   {
                                          //create a static body
                                          solverBodyIdA = m_tmpSolverBodyPool.size();
                                          btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
                                          initSolverBody(&solverBody,colObj0);
                                   }

                                   if (colObj1->getIslandTag() >= 0)
                                   {
                                          if (colObj1->getCompanionId() >= 0)
                                          {
                                                 solverBodyIdB = colObj1->getCompanionId();
                                          } else
                                          {
                                                 solverBodyIdB = m_tmpSolverBodyPool.size();
                                                 btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
                                                 initSolverBody(&solverBody,colObj1);
                                                 colObj1->setCompanionId(solverBodyIdB);
                                          }
                                   } else
                                   {
                                          //create a static body
                                          solverBodyIdB = m_tmpSolverBodyPool.size();
                                          btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
                                          initSolverBody(&solverBody,colObj1);
                                   }
                            }

                            btVector3 rel_pos1;
                            btVector3 rel_pos2;
                            btScalar relaxation;

                            for (int j=0;j<manifold->getNumContacts();j++)
                            {
                                   
                                   btManifoldPoint& cp = manifold->getContactPoint(j);
                                   
                                   if (debugDrawer)
                                          debugDrawer->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);

                                   
                                   if (cp.getDistance() <= btScalar(0.))
                                   {
                                          
                                          const btVector3& pos1 = cp.getPositionWorldOnA();
                                          const btVector3& pos2 = cp.getPositionWorldOnB();

                                           rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); 
                                           rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin();

                                          
                                          relaxation = 1.f;
                                          btScalar rel_vel;
                                          btVector3 vel;

                                          int frictionIndex = m_tmpSolverConstraintPool.size();

                                          {
                                                 btSolverConstraint& solverConstraint = m_tmpSolverConstraintPool.expand();
                                                 btRigidBody* rb0 = btRigidBody::upcast(colObj0);
                                                 btRigidBody* rb1 = btRigidBody::upcast(colObj1);

                                                 solverConstraint.m_solverBodyIdA = solverBodyIdA;
                                                 solverConstraint.m_solverBodyIdB = solverBodyIdB;
                                                 solverConstraint.m_constraintType = btSolverConstraint::BT_SOLVER_CONTACT_1D;

                                                 solverConstraint.m_originalContactPoint = &cp;

                                                 btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
                                                 solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0 : btVector3(0,0,0);
                                                 btVector3 torqueAxis1 = rel_pos2.cross(cp.m_normalWorldOnB);          
                                                 solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*torqueAxis1 : btVector3(0,0,0);
                                                 {
#ifdef COMPUTE_IMPULSE_DENOM
                                                        btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
                                                        btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
#else                                            
                                                        btVector3 vec;
                                                        btScalar denom0 = 0.f;
                                                        btScalar denom1 = 0.f;
                                                        if (rb0)
                                                        {
                                                               vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
                                                               denom0 = rb0->getInvMass() + cp.m_normalWorldOnB.dot(vec);
                                                        }
                                                        if (rb1)
                                                        {
                                                               vec = ( solverConstraint.m_angularComponentB).cross(rel_pos2);
                                                               denom1 = rb1->getInvMass() + cp.m_normalWorldOnB.dot(vec);
                                                        }
#endif //COMPUTE_IMPULSE_DENOM            
                                                        
                                                        btScalar denom = relaxation/(denom0+denom1);
                                                        solverConstraint.m_jacDiagABInv = denom;
                                                 }

                                                 solverConstraint.m_contactNormal = cp.m_normalWorldOnB;
                                                 solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB);
                                                 solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(cp.m_normalWorldOnB);


                                                 btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0);
                                                 btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
                            
                                                 vel  = vel1 - vel2;
                                                 
                                                 rel_vel = cp.m_normalWorldOnB.dot(vel);
                                                 

                                                 solverConstraint.m_penetration = cp.getDistance();
                                                 solverConstraint.m_friction = cp.m_combinedFriction;
                                                 solverConstraint.m_restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
                                                 if (solverConstraint.m_restitution <= btScalar(0.))
                                                 {
                                                        solverConstraint.m_restitution = 0.f;
                                                 };
                                                 
                                                 btScalar penVel = -solverConstraint.m_penetration/infoGlobal.m_timeStep;
                                                 solverConstraint.m_penetration *= -(infoGlobal.m_erp/infoGlobal.m_timeStep);

                                                 if (solverConstraint.m_restitution > penVel)
                                                 {
                                                        solverConstraint.m_penetration = btScalar(0.);
                                                 }
                                                 
                                                 

                                                 if (m_solverMode & SOLVER_USE_WARMSTARTING)
                                                 {
                                                        solverConstraint.m_appliedImpulse = cp.m_appliedImpulse;
                                                 } else
                                                 {
                                                        solverConstraint.m_appliedImpulse = 0.f;
                                                 }
                                                 solverConstraint.m_appliedVelocityImpulse = 0.f;
                                                 
                                   
                                          
                                          }

                                   
                                          {
                                                 btVector3 frictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
                                                 btScalar lat_rel_vel = frictionDir1.length2();
                                                 if (lat_rel_vel > SIMD_EPSILON)//0.0f)
                                                 {
                                                        frictionDir1 /= btSqrt(lat_rel_vel);
                                                        addFrictionConstraint(frictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
                                                        btVector3 frictionDir2 = frictionDir1.cross(cp.m_normalWorldOnB);
                                                        frictionDir2.normalize();//??
                                                        addFrictionConstraint(frictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
                                                 } else
                                                 {
                                                        //re-calculate friction direction every frame, todo: check if this is really needed
                                                        btVector3     frictionDir1,frictionDir2;
                                                        btPlaneSpace1(cp.m_normalWorldOnB,frictionDir1,frictionDir2);
                                                        addFrictionConstraint(frictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
                                                        addFrictionConstraint(frictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
                                                 }

                                          }
                                   

                                   }
                            }
                     }
              }
       }
       
       btContactSolverInfo info = infoGlobal;

       {
              int j;
              for (j=0;j<numConstraints;j++)
              {
                     btTypedConstraint* constraint = constraints[j];
                     constraint->buildJacobian();
              }
       }
       
       

       int numConstraintPool = m_tmpSolverConstraintPool.size();
       int numFrictionPool = m_tmpSolverFrictionConstraintPool.size();

       m_orderTmpConstraintPool.resize(numConstraintPool);
       m_orderFrictionConstraintPool.resize(numFrictionPool);
       {
              int i;
              for (i=0;i<numConstraintPool;i++)
              {
                     m_orderTmpConstraintPool[i] = i;
              }
              for (i=0;i<numFrictionPool;i++)
              {
                     m_orderFrictionConstraintPool[i] = i;
              }
       }



       return 0.f;

}

Here is the call graph for this function:

Here is the caller graph for this function:


Member Data Documentation

m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction

Definition at line 52 of file btSequentialImpulseConstraintSolver.h.

Definition at line 46 of file btSequentialImpulseConstraintSolver.h.

Definition at line 47 of file btSequentialImpulseConstraintSolver.h.

Definition at line 37 of file btSequentialImpulseConstraintSolver.h.

Definition at line 36 of file btSequentialImpulseConstraintSolver.h.

Definition at line 50 of file btSequentialImpulseConstraintSolver.h.

Definition at line 33 of file btSequentialImpulseConstraintSolver.h.

Definition at line 34 of file btSequentialImpulseConstraintSolver.h.

Definition at line 35 of file btSequentialImpulseConstraintSolver.h.


The documentation for this class was generated from the following files: