Back to index

supertuxkart  0.5+dfsg1
btHingeConstraint.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 
00017 #include "btHingeConstraint.h"
00018 #include "BulletDynamics/Dynamics/btRigidBody.h"
00019 #include "LinearMath/btTransformUtil.h"
00020 #include "LinearMath/btMinMax.h"
00021 #include <new>
00022 
00023 
00024 btHingeConstraint::btHingeConstraint()
00025 : btTypedConstraint (HINGE_CONSTRAINT_TYPE),
00026 m_enableAngularMotor(false)
00027 {
00028 }
00029 
00030 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
00031                                                                 btVector3& axisInA,btVector3& axisInB)
00032                                                                 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
00033                                                                 m_angularOnly(false),
00034                                                                 m_enableAngularMotor(false)
00035 {
00036        m_rbAFrame.getOrigin() = pivotInA;
00037        
00038        // since no frame is given, assume this to be zero angle and just pick rb transform axis
00039        btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);
00040 
00041        btVector3 rbAxisA2;
00042        btScalar projection = axisInA.dot(rbAxisA1);
00043        if (projection >= 1.0f - SIMD_EPSILON) {
00044               rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2);
00045               rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
00046        } else if (projection <= -1.0f + SIMD_EPSILON) {
00047               rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2);
00048               rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);      
00049        } else {
00050               rbAxisA2 = axisInA.cross(rbAxisA1);
00051               rbAxisA1 = rbAxisA2.cross(axisInA);
00052        }
00053 
00054        m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
00055                                                                rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
00056                                                                rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
00057 
00058        btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
00059        btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
00060        btVector3 rbAxisB2 =  axisInB.cross(rbAxisB1);   
00061        
00062        m_rbBFrame.getOrigin() = pivotInB;
00063        m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),-axisInB.getX(),
00064                                                                rbAxisB1.getY(),rbAxisB2.getY(),-axisInB.getY(),
00065                                                                rbAxisB1.getZ(),rbAxisB2.getZ(),-axisInB.getZ() );
00066        
00067        //start with free
00068        m_lowerLimit = btScalar(1e30);
00069        m_upperLimit = btScalar(-1e30);
00070        m_biasFactor = 0.3f;
00071        m_relaxationFactor = 1.0f;
00072        m_limitSoftness = 0.9f;
00073        m_solveLimit = false;
00074 
00075 }
00076 
00077 
00078 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA)
00079 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false)
00080 {
00081 
00082        // since no frame is given, assume this to be zero angle and just pick rb transform axis
00083        // fixed axis in worldspace
00084        btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);
00085        btScalar projection = rbAxisA1.dot(axisInA);
00086        if (projection > SIMD_EPSILON)
00087               rbAxisA1 = rbAxisA1*projection - axisInA;
00088        else
00089               rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
00090 
00091        btVector3 rbAxisA2 = axisInA.cross(rbAxisA1);
00092 
00093        m_rbAFrame.getOrigin() = pivotInA;
00094        m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
00095                                                                rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
00096                                                                rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
00097 
00098 
00099        btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * -axisInA;
00100 
00101        btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
00102        btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
00103        btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
00104 
00105 
00106        m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA);
00107        m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
00108                                                                rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
00109                                                                rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
00110        
00111        //start with free
00112        m_lowerLimit = btScalar(1e30);
00113        m_upperLimit = btScalar(-1e30);
00114        m_biasFactor = 0.3f;
00115        m_relaxationFactor = 1.0f;
00116        m_limitSoftness = 0.9f;
00117        m_solveLimit = false;
00118 }
00119 
00120 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, 
00121                                                              const btTransform& rbAFrame, const btTransform& rbBFrame)
00122 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
00123 m_angularOnly(false),
00124 m_enableAngularMotor(false)
00125 {
00126        // flip axis
00127        m_rbBFrame.getBasis()[0][2] *= btScalar(-1.);
00128        m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);
00129        m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
00130 
00131        //start with free
00132        m_lowerLimit = btScalar(1e30);
00133        m_upperLimit = btScalar(-1e30);
00134        m_biasFactor = 0.3f;
00135        m_relaxationFactor = 1.0f;
00136        m_limitSoftness = 0.9f;
00137        m_solveLimit = false;
00138 }                    
00139 
00140 
00141 
00142 btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame)
00143 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame),
00144 m_angularOnly(false),
00145 m_enableAngularMotor(false)
00146 {
00148 
00149        // flip axis
00150        m_rbBFrame.getBasis()[0][2] *= btScalar(-1.);
00151        m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);
00152        m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
00153 
00154        m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin());
00155 
00156        //start with free
00157        m_lowerLimit = btScalar(1e30);
00158        m_upperLimit = btScalar(-1e30);    
00159        m_biasFactor = 0.3f;
00160        m_relaxationFactor = 1.0f;
00161        m_limitSoftness = 0.9f;
00162        m_solveLimit = false;
00163 }
00164 
00165 void   btHingeConstraint::buildJacobian()
00166 {
00167        m_appliedImpulse = btScalar(0.);
00168 
00169        if (!m_angularOnly)
00170        {
00171               btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
00172               btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
00173               btVector3 relPos = pivotBInW - pivotAInW;
00174 
00175               btVector3 normal[3];
00176               if (relPos.length2() > SIMD_EPSILON)
00177               {
00178                      normal[0] = relPos.normalized();
00179               }
00180               else
00181               {
00182                      normal[0].setValue(btScalar(1.0),0,0);
00183               }
00184 
00185               btPlaneSpace1(normal[0], normal[1], normal[2]);
00186 
00187               for (int i=0;i<3;i++)
00188               {
00189                      new (&m_jac[i]) btJacobianEntry(
00190                             m_rbA.getCenterOfMassTransform().getBasis().transpose(),
00191                             m_rbB.getCenterOfMassTransform().getBasis().transpose(),
00192                             pivotAInW - m_rbA.getCenterOfMassPosition(),
00193                             pivotBInW - m_rbB.getCenterOfMassPosition(),
00194                             normal[i],
00195                             m_rbA.getInvInertiaDiagLocal(),
00196                             m_rbA.getInvMass(),
00197                             m_rbB.getInvInertiaDiagLocal(),
00198                             m_rbB.getInvMass());
00199               }
00200        }
00201 
00202        //calculate two perpendicular jointAxis, orthogonal to hingeAxis
00203        //these two jointAxis require equal angular velocities for both bodies
00204 
00205        //this is unused for now, it's a todo
00206        btVector3 jointAxis0local;
00207        btVector3 jointAxis1local;
00208        
00209        btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
00210 
00211        getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
00212        btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
00213        btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
00214        btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
00215               
00216        new (&m_jacAng[0])   btJacobianEntry(jointAxis0,
00217               m_rbA.getCenterOfMassTransform().getBasis().transpose(),
00218               m_rbB.getCenterOfMassTransform().getBasis().transpose(),
00219               m_rbA.getInvInertiaDiagLocal(),
00220               m_rbB.getInvInertiaDiagLocal());
00221 
00222        new (&m_jacAng[1])   btJacobianEntry(jointAxis1,
00223               m_rbA.getCenterOfMassTransform().getBasis().transpose(),
00224               m_rbB.getCenterOfMassTransform().getBasis().transpose(),
00225               m_rbA.getInvInertiaDiagLocal(),
00226               m_rbB.getInvInertiaDiagLocal());
00227 
00228        new (&m_jacAng[2])   btJacobianEntry(hingeAxisWorld,
00229               m_rbA.getCenterOfMassTransform().getBasis().transpose(),
00230               m_rbB.getCenterOfMassTransform().getBasis().transpose(),
00231               m_rbA.getInvInertiaDiagLocal(),
00232               m_rbB.getInvInertiaDiagLocal());
00233 
00234 
00235        // Compute limit information
00236        btScalar hingeAngle = getHingeAngle();  
00237 
00238        //set bias, sign, clear accumulator
00239        m_correction = btScalar(0.);
00240        m_limitSign = btScalar(0.);
00241        m_solveLimit = false;
00242        m_accLimitImpulse = btScalar(0.);
00243 
00244        if (m_lowerLimit < m_upperLimit)
00245        {
00246               if (hingeAngle <= m_lowerLimit*m_limitSoftness)
00247               {
00248                      m_correction = (m_lowerLimit - hingeAngle);
00249                      m_limitSign = 1.0f;
00250                      m_solveLimit = true;
00251               } 
00252               else if (hingeAngle >= m_upperLimit*m_limitSoftness)
00253               {
00254                      m_correction = m_upperLimit - hingeAngle;
00255                      m_limitSign = -1.0f;
00256                      m_solveLimit = true;
00257               }
00258        }
00259 
00260        //Compute K = J*W*J' for hinge axis
00261        btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
00262        m_kHinge =   1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
00263                                   getRigidBodyB().computeAngularImpulseDenominator(axisA));
00264 
00265 }
00266 
00267 void   btHingeConstraint::solveConstraint(btScalar      timeStep)
00268 {
00269 
00270        btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
00271        btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
00272 
00273        btScalar tau = btScalar(0.3);
00274 
00275        //linear part
00276        if (!m_angularOnly)
00277        {
00278               btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
00279               btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
00280 
00281               btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
00282               btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
00283               btVector3 vel = vel1 - vel2;
00284 
00285               for (int i=0;i<3;i++)
00286               {             
00287                      const btVector3& normal = m_jac[i].m_linearJointAxis;
00288                      btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
00289 
00290                      btScalar rel_vel;
00291                      rel_vel = normal.dot(vel);
00292                      //positional error (zeroth order error)
00293                      btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
00294                      btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
00295                      m_appliedImpulse += impulse;
00296                      btVector3 impulse_vector = normal * impulse;
00297                      m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
00298                      m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
00299               }
00300        }
00301 
00302        
00303        {
00305 
00306               // get axes in world space
00307               btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
00308               btVector3 axisB =  getRigidBodyB().getCenterOfMassTransform().getBasis() *  m_rbBFrame.getBasis().getColumn(2);
00309 
00310               const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
00311               const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
00312 
00313               btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
00314               btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);
00315 
00316               btVector3 angAorthog = angVelA - angVelAroundHingeAxisA;
00317               btVector3 angBorthog = angVelB - angVelAroundHingeAxisB;
00318               btVector3 velrelOrthog = angAorthog-angBorthog;
00319               {
00320                      //solve orthogonal angular velocity correction
00321                      btScalar relaxation = btScalar(1.);
00322                      btScalar len = velrelOrthog.length();
00323                      if (len > btScalar(0.00001))
00324                      {
00325                             btVector3 normal = velrelOrthog.normalized();
00326                             btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
00327                                    getRigidBodyB().computeAngularImpulseDenominator(normal);
00328                             // scale for mass and relaxation
00329                             //todo:  expose this 0.9 factor to developer
00330                             velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor;
00331                      }
00332 
00333                      //solve angular positional correction
00334                      btVector3 angularError = -axisA.cross(axisB) *(btScalar(1.)/timeStep);
00335                      btScalar len2 = angularError.length();
00336                      if (len2>btScalar(0.00001))
00337                      {
00338                             btVector3 normal2 = angularError.normalized();
00339                             btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
00340                                           getRigidBodyB().computeAngularImpulseDenominator(normal2);
00341                             angularError *= (btScalar(1.)/denom2) * relaxation;
00342                      }
00343 
00344                      m_rbA.applyTorqueImpulse(-velrelOrthog+angularError);
00345                      m_rbB.applyTorqueImpulse(velrelOrthog-angularError);
00346 
00347                      // solve limit
00348                      if (m_solveLimit)
00349                      {
00350                             btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor  ) * m_limitSign;
00351 
00352                             btScalar impulseMag = amplitude * m_kHinge;
00353 
00354                             // Clamp the accumulated impulse
00355                             btScalar temp = m_accLimitImpulse;
00356                             m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) );
00357                             impulseMag = m_accLimitImpulse - temp;
00358 
00359 
00360                             btVector3 impulse = axisA * impulseMag * m_limitSign;
00361                             m_rbA.applyTorqueImpulse(impulse);
00362                             m_rbB.applyTorqueImpulse(-impulse);
00363                      }
00364               }
00365 
00366               //apply motor
00367               if (m_enableAngularMotor) 
00368               {
00369                      //todo: add limits too
00370                      btVector3 angularLimit(0,0,0);
00371 
00372                      btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
00373                      btScalar projRelVel = velrel.dot(axisA);
00374 
00375                      btScalar desiredMotorVel = m_motorTargetVelocity;
00376                      btScalar motor_relvel = desiredMotorVel - projRelVel;
00377 
00378                      btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;;
00379                      //todo: should clip against accumulated impulse
00380                      btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse;
00381                      clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse;
00382                      btVector3 motorImp = clippedMotorImpulse * axisA;
00383 
00384                      m_rbA.applyTorqueImpulse(motorImp+angularLimit);
00385                      m_rbB.applyTorqueImpulse(-motorImp-angularLimit);
00386                      
00387               }
00388        }
00389 
00390 }
00391 
00392 void   btHingeConstraint::updateRHS(btScalar     timeStep)
00393 {
00394        (void)timeStep;
00395 
00396 }
00397 
00398 btScalar btHingeConstraint::getHingeAngle()
00399 {
00400        const btVector3 refAxis0  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0);
00401        const btVector3 refAxis1  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1);
00402        const btVector3 swingAxis = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(1);
00403 
00404        return btAtan2Fast( swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)  );
00405 }