Back to index

supertuxkart  0.5+dfsg1
Defines | Functions
btContactConstraint.cpp File Reference
#include "btContactConstraint.h"
#include "BulletDynamics/Dynamics/btRigidBody.h"
#include "LinearMath/btVector3.h"
#include "btJacobianEntry.h"
#include "btContactSolverInfo.h"
#include "LinearMath/btMinMax.h"
#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"

Go to the source code of this file.

Defines

#define ASSERT2   assert
#define USE_INTERNAL_APPLY_IMPULSE   1

Functions

void resolveSingleBilateral (btRigidBody &body1, const btVector3 &pos1, btRigidBody &body2, const btVector3 &pos2, btScalar distance, const btVector3 &normal, btScalar &impulse, btScalar timeStep)
 bilateral constraint between two dynamic objects positive distance = separation, negative distance = penetration
btScalar resolveSingleCollision (btRigidBody &body1, btRigidBody &body2, btManifoldPoint &contactPoint, const btContactSolverInfo &solverInfo)
 contact constraint resolution: calculate and apply impulse to satisfy non-penetration and non-negative relative velocity constraint positive distance = separation, negative distance = penetration
btScalar resolveSingleFriction (btRigidBody &body1, btRigidBody &body2, btManifoldPoint &contactPoint, const btContactSolverInfo &solverInfo)
btScalar resolveSingleFrictionOriginal (btRigidBody &body1, btRigidBody &body2, btManifoldPoint &contactPoint, const btContactSolverInfo &solverInfo)
btScalar resolveSingleCollisionCombined (btRigidBody &body1, btRigidBody &body2, btManifoldPoint &contactPoint, const btContactSolverInfo &solverInfo)
btScalar resolveSingleFrictionEmpty (btRigidBody &body1, btRigidBody &body2, btManifoldPoint &contactPoint, const btContactSolverInfo &solverInfo)

Define Documentation

#define ASSERT2   assert

Definition at line 25 of file btContactConstraint.cpp.

Definition at line 27 of file btContactConstraint.cpp.


Function Documentation

void resolveSingleBilateral ( btRigidBody body1,
const btVector3 pos1,
btRigidBody body2,
const btVector3 pos2,
btScalar  distance,
const btVector3 normal,
btScalar impulse,
btScalar  timeStep 
)

bilateral constraint between two dynamic objects positive distance = separation, negative distance = penetration

Definition at line 31 of file btContactConstraint.cpp.

{
       (void)timeStep;
       (void)distance;


       btScalar normalLenSqr = normal.length2();
       ASSERT2(btFabs(normalLenSqr) < btScalar(1.1));
       if (normalLenSqr > btScalar(1.1))
       {
              impulse = btScalar(0.);
              return;
       }
       btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
       btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
       //this jacobian entry could be re-used for all iterations
       
       btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
       btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
       btVector3 vel = vel1 - vel2;
       

         btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
              body2.getCenterOfMassTransform().getBasis().transpose(),
              rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
              body2.getInvInertiaDiagLocal(),body2.getInvMass());

       btScalar jacDiagAB = jac.getDiagonal();
       btScalar jacDiagABInv = btScalar(1.) / jacDiagAB;
       
         btScalar rel_vel = jac.getRelativeVelocity(
              body1.getLinearVelocity(),
              body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(),
              body2.getLinearVelocity(),
              body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity()); 
       btScalar a;
       a=jacDiagABInv;


       rel_vel = normal.dot(vel);
       
       //todo: move this into proper structure
       btScalar contactDamping = btScalar(0.2);

#ifdef ONLY_USE_LINEAR_MASS
       btScalar massTerm = btScalar(1.) / (body1.getInvMass() + body2.getInvMass());
       impulse = - contactDamping * rel_vel * massTerm;
#else  
       btScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
       impulse = velocityImpulse;
#endif
}

Here is the call graph for this function:

Here is the caller graph for this function:

btScalar resolveSingleCollision ( btRigidBody body1,
btRigidBody body2,
btManifoldPoint contactPoint,
const btContactSolverInfo solverInfo 
)

contact constraint resolution: calculate and apply impulse to satisfy non-penetration and non-negative relative velocity constraint positive distance = separation, negative distance = penetration

Definition at line 89 of file btContactConstraint.cpp.

{

       const btVector3& pos1_ = contactPoint.getPositionWorldOnA();
       const btVector3& pos2_ = contactPoint.getPositionWorldOnB();
       const btVector3& normal = contactPoint.m_normalWorldOnB;

       //constant over all iterations
       btVector3 rel_pos1 = pos1_ - body1.getCenterOfMassPosition(); 
       btVector3 rel_pos2 = pos2_ - body2.getCenterOfMassPosition();
       
       btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
       btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
       btVector3 vel = vel1 - vel2;
       btScalar rel_vel;
       rel_vel = normal.dot(vel);
       
       btScalar Kfps = btScalar(1.) / solverInfo.m_timeStep ;

       // btScalar damping = solverInfo.m_damping ;
       btScalar Kerp = solverInfo.m_erp;
       btScalar Kcor = Kerp *Kfps;

       btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
       assert(cpd);
       btScalar distance = cpd->m_penetration;
       btScalar positionalError = Kcor *-distance;
       btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;

       btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;

       btScalar      velocityImpulse = velocityError * cpd->m_jacDiagABInv;

       btScalar normalImpulse = penetrationImpulse+velocityImpulse;
       
       // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
       btScalar oldNormalImpulse = cpd->m_appliedImpulse;
       btScalar sum = oldNormalImpulse + normalImpulse;
       cpd->m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;

       normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;

#ifdef USE_INTERNAL_APPLY_IMPULSE
       if (body1.getInvMass())
       {
              body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse);
       }
       if (body2.getInvMass())
       {
              body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
       }
#else //USE_INTERNAL_APPLY_IMPULSE
       body1.applyImpulse(normal*(normalImpulse), rel_pos1);
       body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
#endif //USE_INTERNAL_APPLY_IMPULSE

       return normalImpulse;
}

Here is the call graph for this function:

Here is the caller graph for this function:

btScalar resolveSingleCollisionCombined ( btRigidBody body1,
btRigidBody body2,
btManifoldPoint contactPoint,
const btContactSolverInfo solverInfo 
)

Definition at line 307 of file btContactConstraint.cpp.

{

       const btVector3& pos1 = contactPoint.getPositionWorldOnA();
       const btVector3& pos2 = contactPoint.getPositionWorldOnB();
       const btVector3& normal = contactPoint.m_normalWorldOnB;

       btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
       btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
       
       btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
       btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
       btVector3 vel = vel1 - vel2;
       btScalar rel_vel;
       rel_vel = normal.dot(vel);
       
       btScalar Kfps = btScalar(1.) / solverInfo.m_timeStep ;

       //btScalar damping = solverInfo.m_damping ;
       btScalar Kerp = solverInfo.m_erp;
       btScalar Kcor = Kerp *Kfps;

       btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
       assert(cpd);
       btScalar distance = cpd->m_penetration;
       btScalar positionalError = Kcor *-distance;
       btScalar velocityError = cpd->m_restitution - rel_vel;// * damping;

       btScalar penetrationImpulse = positionalError * cpd->m_jacDiagABInv;

       btScalar      velocityImpulse = velocityError * cpd->m_jacDiagABInv;

       btScalar normalImpulse = penetrationImpulse+velocityImpulse;
       
       // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
       btScalar oldNormalImpulse = cpd->m_appliedImpulse;
       btScalar sum = oldNormalImpulse + normalImpulse;
       cpd->m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;

       normalImpulse = cpd->m_appliedImpulse - oldNormalImpulse;


#ifdef USE_INTERNAL_APPLY_IMPULSE
       if (body1.getInvMass())
       {
              body1.internalApplyImpulse(contactPoint.m_normalWorldOnB*body1.getInvMass(),cpd->m_angularComponentA,normalImpulse);
       }
       if (body2.getInvMass())
       {
              body2.internalApplyImpulse(contactPoint.m_normalWorldOnB*body2.getInvMass(),cpd->m_angularComponentB,-normalImpulse);
       }
#else //USE_INTERNAL_APPLY_IMPULSE
       body1.applyImpulse(normal*(normalImpulse), rel_pos1);
       body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
#endif //USE_INTERNAL_APPLY_IMPULSE

       {
              //friction
              btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
              btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
              btVector3 vel = vel1 - vel2;
         
              rel_vel = normal.dot(vel);


              btVector3 lat_vel = vel - normal * rel_vel;
              btScalar lat_rel_vel = lat_vel.length();

              btScalar combinedFriction = cpd->m_friction;

              if (cpd->m_appliedImpulse > 0)
              if (lat_rel_vel > SIMD_EPSILON)
              {
                     lat_vel /= lat_rel_vel;
                     btVector3 temp1 = body1.getInvInertiaTensorWorld() * rel_pos1.cross(lat_vel);
                     btVector3 temp2 = body2.getInvInertiaTensorWorld() * rel_pos2.cross(lat_vel);
                     btScalar friction_impulse = lat_rel_vel /
                            (body1.getInvMass() + body2.getInvMass() + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
                     btScalar normal_impulse = cpd->m_appliedImpulse * combinedFriction;

                     btSetMin(friction_impulse, normal_impulse);
                     btSetMax(friction_impulse, -normal_impulse);
                     body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1);
                     body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);
              }
       }



       return normalImpulse;
}

Here is the call graph for this function:

Here is the caller graph for this function:

btScalar resolveSingleFriction ( btRigidBody body1,
btRigidBody body2,
btManifoldPoint contactPoint,
const btContactSolverInfo solverInfo 
)

Definition at line 153 of file btContactConstraint.cpp.

{

       (void)solverInfo;

       const btVector3& pos1 = contactPoint.getPositionWorldOnA();
       const btVector3& pos2 = contactPoint.getPositionWorldOnB();

       btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
       btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
       
       btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
       assert(cpd);

       btScalar combinedFriction = cpd->m_friction;
       
       btScalar limit = cpd->m_appliedImpulse * combinedFriction;
       
       if (cpd->m_appliedImpulse>btScalar(0.))
       //friction
       {
              //apply friction in the 2 tangential directions
              
              // 1st tangent
              btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
              btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
              btVector3 vel = vel1 - vel2;
       
              btScalar j1,j2;

              {
                            
                     btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);

                     // calculate j that moves us to zero relative velocity
                     j1 = -vrel * cpd->m_jacDiagABInvTangent0;
                     btScalar oldTangentImpulse = cpd->m_accumulatedTangentImpulse0;
                     cpd->m_accumulatedTangentImpulse0 = oldTangentImpulse + j1;
                     btSetMin(cpd->m_accumulatedTangentImpulse0, limit);
                     btSetMax(cpd->m_accumulatedTangentImpulse0, -limit);
                     j1 = cpd->m_accumulatedTangentImpulse0 - oldTangentImpulse;

              }
              {
                     // 2nd tangent

                     btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);
                     
                     // calculate j that moves us to zero relative velocity
                     j2 = -vrel * cpd->m_jacDiagABInvTangent1;
                     btScalar oldTangentImpulse = cpd->m_accumulatedTangentImpulse1;
                     cpd->m_accumulatedTangentImpulse1 = oldTangentImpulse + j2;
                     btSetMin(cpd->m_accumulatedTangentImpulse1, limit);
                     btSetMax(cpd->m_accumulatedTangentImpulse1, -limit);
                     j2 = cpd->m_accumulatedTangentImpulse1 - oldTangentImpulse;
              }

#ifdef USE_INTERNAL_APPLY_IMPULSE
       if (body1.getInvMass())
       {
              body1.internalApplyImpulse(cpd->m_frictionWorldTangential0*body1.getInvMass(),cpd->m_frictionAngularComponent0A,j1);
              body1.internalApplyImpulse(cpd->m_frictionWorldTangential1*body1.getInvMass(),cpd->m_frictionAngularComponent1A,j2);
       }
       if (body2.getInvMass())
       {
              body2.internalApplyImpulse(cpd->m_frictionWorldTangential0*body2.getInvMass(),cpd->m_frictionAngularComponent0B,-j1);
              body2.internalApplyImpulse(cpd->m_frictionWorldTangential1*body2.getInvMass(),cpd->m_frictionAngularComponent1B,-j2);  
       }
#else //USE_INTERNAL_APPLY_IMPULSE
       body1.applyImpulse((j1 * cpd->m_frictionWorldTangential0)+(j2 * cpd->m_frictionWorldTangential1), rel_pos1);
       body2.applyImpulse((j1 * -cpd->m_frictionWorldTangential0)+(j2 * -cpd->m_frictionWorldTangential1), rel_pos2);
#endif //USE_INTERNAL_APPLY_IMPULSE


       } 
       return cpd->m_appliedImpulse;
}

Here is the call graph for this function:

Here is the caller graph for this function:

btScalar resolveSingleFrictionEmpty ( btRigidBody body1,
btRigidBody body2,
btManifoldPoint contactPoint,
const btContactSolverInfo solverInfo 
)

Definition at line 403 of file btContactConstraint.cpp.

{
       (void)contactPoint;
       (void)body1;
       (void)body2;
       (void)solverInfo;


       return btScalar(0.);
};
btScalar resolveSingleFrictionOriginal ( btRigidBody body1,
btRigidBody body2,
btManifoldPoint contactPoint,
const btContactSolverInfo solverInfo 
)

Definition at line 236 of file btContactConstraint.cpp.

{

       (void)solverInfo;

       const btVector3& pos1 = contactPoint.getPositionWorldOnA();
       const btVector3& pos2 = contactPoint.getPositionWorldOnB();

       btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
       btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
       
       btConstraintPersistentData* cpd = (btConstraintPersistentData*) contactPoint.m_userPersistentData;
       assert(cpd);

       btScalar combinedFriction = cpd->m_friction;
       
       btScalar limit = cpd->m_appliedImpulse * combinedFriction;
       //if (contactPoint.m_appliedImpulse>btScalar(0.))
       //friction
       {
              //apply friction in the 2 tangential directions
              
              {
                     // 1st tangent
                     btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
                     btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
                     btVector3 vel = vel1 - vel2;
                     
                     btScalar vrel = cpd->m_frictionWorldTangential0.dot(vel);

                     // calculate j that moves us to zero relative velocity
                     btScalar j = -vrel * cpd->m_jacDiagABInvTangent0;
                     btScalar total = cpd->m_accumulatedTangentImpulse0 + j;
                     btSetMin(total, limit);
                     btSetMax(total, -limit);
                     j = total - cpd->m_accumulatedTangentImpulse0;
                     cpd->m_accumulatedTangentImpulse0 = total;
                     body1.applyImpulse(j * cpd->m_frictionWorldTangential0, rel_pos1);
                     body2.applyImpulse(j * -cpd->m_frictionWorldTangential0, rel_pos2);
              }

                            
              {
                     // 2nd tangent
                     btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
                     btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
                     btVector3 vel = vel1 - vel2;

                     btScalar vrel = cpd->m_frictionWorldTangential1.dot(vel);
                     
                     // calculate j that moves us to zero relative velocity
                     btScalar j = -vrel * cpd->m_jacDiagABInvTangent1;
                     btScalar total = cpd->m_accumulatedTangentImpulse1 + j;
                     btSetMin(total, limit);
                     btSetMax(total, -limit);
                     j = total - cpd->m_accumulatedTangentImpulse1;
                     cpd->m_accumulatedTangentImpulse1 = total;
                     body1.applyImpulse(j * cpd->m_frictionWorldTangential1, rel_pos1);
                     body2.applyImpulse(j * -cpd->m_frictionWorldTangential1, rel_pos2);
              }
       } 
       return cpd->m_appliedImpulse;
}

Here is the call graph for this function: