Back to index

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

btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc) More...

#include <btConeTwistConstraint.h>

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

List of all members.

Public Member Functions

 btConeTwistConstraint (btRigidBody &rbA, btRigidBody &rbB, const btTransform &rbAFrame, const btTransform &rbBFrame)
 btConeTwistConstraint (btRigidBody &rbA, const btTransform &rbAFrame)
 btConeTwistConstraint ()
virtual void buildJacobian ()
virtual void solveConstraint (btScalar timeStep)
void updateRHS (btScalar timeStep)
const btRigidBodygetRigidBodyA () const
const btRigidBodygetRigidBodyB () const
void setAngularOnly (bool angularOnly)
void setLimit (btScalar _swingSpan1, btScalar _swingSpan2, btScalar _twistSpan, btScalar _softness=0.8f, btScalar _biasFactor=0.3f, btScalar _relaxationFactor=1.0f)
const btTransformgetAFrame ()
const btTransformgetBFrame ()
int getSolveTwistLimit ()
int getSolveSwingLimit ()
btScalar getTwistLimitSign ()
btRigidBodygetRigidBodyA ()
btRigidBodygetRigidBodyB ()
int getUserConstraintType () const
void setUserConstraintType (int userConstraintType)
void setUserConstraintId (int uid)
int getUserConstraintId () const
int getUid () const
btScalar getAppliedImpulse () const
btTypedConstraintType getConstraintType () const

Protected Attributes

btRigidBodym_rbA
btRigidBodym_rbB
btScalar m_appliedImpulse

Private Attributes

btJacobianEntry m_jac [3]
btTransform m_rbAFrame
btTransform m_rbBFrame
btScalar m_limitSoftness
btScalar m_biasFactor
btScalar m_relaxationFactor
btScalar m_swingSpan1
btScalar m_swingSpan2
btScalar m_twistSpan
btVector3 m_swingAxis
btVector3 m_twistAxis
btScalar m_kSwing
btScalar m_kTwist
btScalar m_twistLimitSign
btScalar m_swingCorrection
btScalar m_twistCorrection
btScalar m_accSwingLimitImpulse
btScalar m_accTwistLimitImpulse
bool m_angularOnly
bool m_solveTwistLimit
bool m_solveSwingLimit

Detailed Description

btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc)

Definition at line 31 of file btConeTwistConstraint.h.


Constructor & Destructor Documentation

btConeTwistConstraint::btConeTwistConstraint ( btRigidBody rbA,
btRigidBody rbB,
const btTransform rbAFrame,
const btTransform rbBFrame 
)

Definition at line 31 of file btConeTwistConstraint.cpp.


Member Function Documentation

Implements btTypedConstraint.

Definition at line 64 of file btConeTwistConstraint.cpp.

{
       m_appliedImpulse = btScalar(0.);

       //set bias, sign, clear accumulator
       m_swingCorrection = btScalar(0.);
       m_twistLimitSign = btScalar(0.);
       m_solveTwistLimit = false;
       m_solveSwingLimit = false;
       m_accTwistLimitImpulse = btScalar(0.);
       m_accSwingLimitImpulse = btScalar(0.);

       if (!m_angularOnly)
       {
              btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
              btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
              btVector3 relPos = pivotBInW - pivotAInW;

              btVector3 normal[3];
              if (relPos.length2() > SIMD_EPSILON)
              {
                     normal[0] = relPos.normalized();
              }
              else
              {
                     normal[0].setValue(btScalar(1.0),0,0);
              }

              btPlaneSpace1(normal[0], normal[1], normal[2]);

              for (int i=0;i<3;i++)
              {
                     new (&m_jac[i]) btJacobianEntry(
                            m_rbA.getCenterOfMassTransform().getBasis().transpose(),
                            m_rbB.getCenterOfMassTransform().getBasis().transpose(),
                            pivotAInW - m_rbA.getCenterOfMassPosition(),
                            pivotBInW - m_rbB.getCenterOfMassPosition(),
                            normal[i],
                            m_rbA.getInvInertiaDiagLocal(),
                            m_rbA.getInvMass(),
                            m_rbB.getInvInertiaDiagLocal(),
                            m_rbB.getInvMass());
              }
       }

       btVector3 b1Axis1,b1Axis2,b1Axis3;
       btVector3 b2Axis1,b2Axis2;

       b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0);
       b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0);

       btScalar swing1=btScalar(0.),swing2 = btScalar(0.);

       // Get Frame into world space
       if (m_swingSpan1 >= btScalar(0.05f))
       {
              b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1);
              swing1  = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) );
       }

       if (m_swingSpan2 >= btScalar(0.05f))
       {
              b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);                   
              swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) );
       }

       btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1);           
       btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2);    
       btScalar EllipseAngle = btFabs(swing1*swing1)* RMaxAngle1Sq + btFabs(swing2*swing2) * RMaxAngle2Sq;

       if (EllipseAngle > 1.0f)
       {
              m_swingCorrection = EllipseAngle-1.0f;
              m_solveSwingLimit = true;
              
              // Calculate necessary axis & factors
              m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3));
              m_swingAxis.normalize();

              btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
              m_swingAxis *= swingAxisSign;

              m_kSwing =  btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) +
                     getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis));

       }

       // Twist limits
       if (m_twistSpan >= btScalar(0.))
       {
              btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1);
              btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1);
              btVector3 TwistRef = quatRotate(rotationArc,b2Axis2); 
              btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) );

              btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
              if (twist <= -m_twistSpan*lockedFreeFactor)
              {
                     m_twistCorrection = -(twist + m_twistSpan);
                     m_solveTwistLimit = true;

                     m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
                     m_twistAxis.normalize();
                     m_twistAxis *= -1.0f;

                     m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
                            getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));

              }      else
                     if (twist >  m_twistSpan*lockedFreeFactor)
                     {
                            m_twistCorrection = (twist - m_twistSpan);
                            m_solveTwistLimit = true;

                            m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
                            m_twistAxis.normalize();

                            m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
                                   getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));

                     }
       }
}

Here is the call graph for this function:

Definition at line 106 of file btConeTwistConstraint.h.

{ return m_rbAFrame; };     
btScalar btTypedConstraint::getAppliedImpulse ( ) const [inline, inherited]

Definition at line 108 of file btTypedConstraint.h.

       {
              return m_appliedImpulse;
       }

Definition at line 107 of file btConeTwistConstraint.h.

{ return m_rbBFrame; };

Definition at line 113 of file btTypedConstraint.h.

       {
              return m_constraintType;
       }

Here is the caller graph for this function:

Definition at line 74 of file btTypedConstraint.h.

       {
              return m_rbA;
       }

Reimplemented from btTypedConstraint.

Definition at line 81 of file btConeTwistConstraint.h.

       {
              return m_rbA;
       }

Here is the caller graph for this function:

Definition at line 78 of file btTypedConstraint.h.

       {
              return m_rbB;
       }

Reimplemented from btTypedConstraint.

Definition at line 85 of file btConeTwistConstraint.h.

       {
              return m_rbB;
       }

Here is the caller graph for this function:

Definition at line 114 of file btConeTwistConstraint.h.

       {
              return m_solveTwistLimit;
       }

Definition at line 109 of file btConeTwistConstraint.h.

       {
              return m_solveTwistLimit;
       }

Definition at line 119 of file btConeTwistConstraint.h.

       {
              return m_twistLimitSign;
       }
int btTypedConstraint::getUid ( ) const [inline, inherited]

Definition at line 103 of file btTypedConstraint.h.

       {
              return m_userConstraintId;   
       } 
int btTypedConstraint::getUserConstraintId ( ) const [inline, inherited]

Definition at line 98 of file btTypedConstraint.h.

       {
              return m_userConstraintId;
       }
int btTypedConstraint::getUserConstraintType ( ) const [inline, inherited]

Definition at line 83 of file btTypedConstraint.h.

       {
              return m_userConstraintType ;
       }
void btConeTwistConstraint::setAngularOnly ( bool  angularOnly) [inline]

Definition at line 90 of file btConeTwistConstraint.h.

       {
              m_angularOnly = angularOnly;
       }
void btConeTwistConstraint::setLimit ( btScalar  _swingSpan1,
btScalar  _swingSpan2,
btScalar  _twistSpan,
btScalar  _softness = 0.8f,
btScalar  _biasFactor = 0.3f,
btScalar  _relaxationFactor = 1.0f 
) [inline]

Definition at line 95 of file btConeTwistConstraint.h.

       {
              m_swingSpan1 = _swingSpan1;
              m_swingSpan2 = _swingSpan2;
              m_twistSpan  = _twistSpan;

              m_limitSoftness =  _softness;
              m_biasFactor = _biasFactor;
              m_relaxationFactor = _relaxationFactor;
       }
void btTypedConstraint::setUserConstraintId ( int  uid) [inline, inherited]

Definition at line 93 of file btTypedConstraint.h.

       {
              m_userConstraintId = uid;
       }
void btTypedConstraint::setUserConstraintType ( int  userConstraintType) [inline, inherited]

Definition at line 88 of file btTypedConstraint.h.

       {
              m_userConstraintType = userConstraintType;
       };
void btConeTwistConstraint::solveConstraint ( btScalar  timeStep) [virtual]

solve angular part

Implements btTypedConstraint.

Definition at line 188 of file btConeTwistConstraint.cpp.

{

       btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
       btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();

       btScalar tau = btScalar(0.3);

       //linear part
       if (!m_angularOnly)
       {
              btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
              btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();

              btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
              btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
              btVector3 vel = vel1 - vel2;

              for (int i=0;i<3;i++)
              {             
                     const btVector3& normal = m_jac[i].m_linearJointAxis;
                     btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();

                     btScalar rel_vel;
                     rel_vel = normal.dot(vel);
                     //positional error (zeroth order error)
                     btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
                     btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
                     m_appliedImpulse += impulse;
                     btVector3 impulse_vector = normal * impulse;
                     m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
                     m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
              }
       }
       
       {
              const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
              const btVector3& angVelB = getRigidBodyB().getAngularVelocity();

              // solve swing limit
              if (m_solveSwingLimit)
              {
                     btScalar amplitude = ((angVelB - angVelA).dot( m_swingAxis )*m_relaxationFactor*m_relaxationFactor + m_swingCorrection*(btScalar(1.)/timeStep)*m_biasFactor);
                     btScalar impulseMag = amplitude * m_kSwing;

                     // Clamp the accumulated impulse
                     btScalar temp = m_accSwingLimitImpulse;
                     m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) );
                     impulseMag = m_accSwingLimitImpulse - temp;

                     btVector3 impulse = m_swingAxis * impulseMag;

                     m_rbA.applyTorqueImpulse(impulse);
                     m_rbB.applyTorqueImpulse(-impulse);

              }

              // solve twist limit
              if (m_solveTwistLimit)
              {
                     btScalar amplitude = ((angVelB - angVelA).dot( m_twistAxis )*m_relaxationFactor*m_relaxationFactor + m_twistCorrection*(btScalar(1.)/timeStep)*m_biasFactor );
                     btScalar impulseMag = amplitude * m_kTwist;

                     // Clamp the accumulated impulse
                     btScalar temp = m_accTwistLimitImpulse;
                     m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) );
                     impulseMag = m_accTwistLimitImpulse - temp;

                     btVector3 impulse = m_twistAxis * impulseMag;

                     m_rbA.applyTorqueImpulse(impulse);
                     m_rbB.applyTorqueImpulse(-impulse);

              }
       
       }

}

Here is the call graph for this function:

Definition at line 268 of file btConeTwistConstraint.cpp.

{
       (void)timeStep;

}

Member Data Documentation

Definition at line 59 of file btConeTwistConstraint.h.

Definition at line 60 of file btConeTwistConstraint.h.

Definition at line 62 of file btConeTwistConstraint.h.

Definition at line 50 of file btTypedConstraint.h.

Definition at line 42 of file btConeTwistConstraint.h.

Definition at line 36 of file btConeTwistConstraint.h.

Definition at line 52 of file btConeTwistConstraint.h.

Definition at line 53 of file btConeTwistConstraint.h.

Definition at line 41 of file btConeTwistConstraint.h.

btRigidBody& btTypedConstraint::m_rbA [protected, inherited]

Definition at line 48 of file btTypedConstraint.h.

Definition at line 38 of file btConeTwistConstraint.h.

btRigidBody& btTypedConstraint::m_rbB [protected, inherited]

Definition at line 49 of file btTypedConstraint.h.

Definition at line 39 of file btConeTwistConstraint.h.

Definition at line 43 of file btConeTwistConstraint.h.

Definition at line 64 of file btConeTwistConstraint.h.

Definition at line 63 of file btConeTwistConstraint.h.

Definition at line 49 of file btConeTwistConstraint.h.

Definition at line 56 of file btConeTwistConstraint.h.

Definition at line 45 of file btConeTwistConstraint.h.

Definition at line 46 of file btConeTwistConstraint.h.

Definition at line 50 of file btConeTwistConstraint.h.

Definition at line 57 of file btConeTwistConstraint.h.

Definition at line 55 of file btConeTwistConstraint.h.

Definition at line 47 of file btConeTwistConstraint.h.


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