Back to index

supertuxkart  0.5+dfsg1
btConeTwistConstraint.cpp
Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 btConeTwistConstraint is Copyright (c) 2007 Starbreeze Studios
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 Written by: Marcus Hennix
00016 */
00017 
00018 
00019 #include "btConeTwistConstraint.h"
00020 #include "BulletDynamics/Dynamics/btRigidBody.h"
00021 #include "LinearMath/btTransformUtil.h"
00022 #include "LinearMath/btMinMax.h"
00023 #include <new>
00024 
00025 btConeTwistConstraint::btConeTwistConstraint()
00026 :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE)
00027 {
00028 }
00029 
00030 
00031 btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB, 
00032                                                                               const btTransform& rbAFrame,const btTransform& rbBFrame)
00033                                                                               :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
00034                                                                               m_angularOnly(false)
00035 {
00036        m_swingSpan1 = btScalar(1e30);
00037        m_swingSpan2 = btScalar(1e30);
00038        m_twistSpan  = btScalar(1e30);
00039        m_biasFactor = 0.3f;
00040        m_relaxationFactor = 1.0f;
00041 
00042        m_solveTwistLimit = false;
00043        m_solveSwingLimit = false;
00044 
00045 }
00046 
00047 btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame)
00048                                                                              :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame),
00049                                                                               m_angularOnly(false)
00050 {
00051        m_rbBFrame = m_rbAFrame;
00052        
00053        m_swingSpan1 = btScalar(1e30);
00054        m_swingSpan2 = btScalar(1e30);
00055        m_twistSpan  = btScalar(1e30);
00056        m_biasFactor = 0.3f;
00057        m_relaxationFactor = 1.0f;
00058 
00059        m_solveTwistLimit = false;
00060        m_solveSwingLimit = false;
00061        
00062 }                    
00063 
00064 void   btConeTwistConstraint::buildJacobian()
00065 {
00066        m_appliedImpulse = btScalar(0.);
00067 
00068        //set bias, sign, clear accumulator
00069        m_swingCorrection = btScalar(0.);
00070        m_twistLimitSign = btScalar(0.);
00071        m_solveTwistLimit = false;
00072        m_solveSwingLimit = false;
00073        m_accTwistLimitImpulse = btScalar(0.);
00074        m_accSwingLimitImpulse = btScalar(0.);
00075 
00076        if (!m_angularOnly)
00077        {
00078               btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
00079               btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
00080               btVector3 relPos = pivotBInW - pivotAInW;
00081 
00082               btVector3 normal[3];
00083               if (relPos.length2() > SIMD_EPSILON)
00084               {
00085                      normal[0] = relPos.normalized();
00086               }
00087               else
00088               {
00089                      normal[0].setValue(btScalar(1.0),0,0);
00090               }
00091 
00092               btPlaneSpace1(normal[0], normal[1], normal[2]);
00093 
00094               for (int i=0;i<3;i++)
00095               {
00096                      new (&m_jac[i]) btJacobianEntry(
00097                             m_rbA.getCenterOfMassTransform().getBasis().transpose(),
00098                             m_rbB.getCenterOfMassTransform().getBasis().transpose(),
00099                             pivotAInW - m_rbA.getCenterOfMassPosition(),
00100                             pivotBInW - m_rbB.getCenterOfMassPosition(),
00101                             normal[i],
00102                             m_rbA.getInvInertiaDiagLocal(),
00103                             m_rbA.getInvMass(),
00104                             m_rbB.getInvInertiaDiagLocal(),
00105                             m_rbB.getInvMass());
00106               }
00107        }
00108 
00109        btVector3 b1Axis1,b1Axis2,b1Axis3;
00110        btVector3 b2Axis1,b2Axis2;
00111 
00112        b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0);
00113        b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0);
00114 
00115        btScalar swing1=btScalar(0.),swing2 = btScalar(0.);
00116 
00117        // Get Frame into world space
00118        if (m_swingSpan1 >= btScalar(0.05f))
00119        {
00120               b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1);
00121               swing1  = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) );
00122        }
00123 
00124        if (m_swingSpan2 >= btScalar(0.05f))
00125        {
00126               b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);                   
00127               swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) );
00128        }
00129 
00130        btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1);           
00131        btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2);    
00132        btScalar EllipseAngle = btFabs(swing1*swing1)* RMaxAngle1Sq + btFabs(swing2*swing2) * RMaxAngle2Sq;
00133 
00134        if (EllipseAngle > 1.0f)
00135        {
00136               m_swingCorrection = EllipseAngle-1.0f;
00137               m_solveSwingLimit = true;
00138               
00139               // Calculate necessary axis & factors
00140               m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3));
00141               m_swingAxis.normalize();
00142 
00143               btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
00144               m_swingAxis *= swingAxisSign;
00145 
00146               m_kSwing =  btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) +
00147                      getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis));
00148 
00149        }
00150 
00151        // Twist limits
00152        if (m_twistSpan >= btScalar(0.))
00153        {
00154               btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1);
00155               btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1);
00156               btVector3 TwistRef = quatRotate(rotationArc,b2Axis2); 
00157               btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) );
00158 
00159               btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
00160               if (twist <= -m_twistSpan*lockedFreeFactor)
00161               {
00162                      m_twistCorrection = -(twist + m_twistSpan);
00163                      m_solveTwistLimit = true;
00164 
00165                      m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
00166                      m_twistAxis.normalize();
00167                      m_twistAxis *= -1.0f;
00168 
00169                      m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
00170                             getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
00171 
00172               }      else
00173                      if (twist >  m_twistSpan*lockedFreeFactor)
00174                      {
00175                             m_twistCorrection = (twist - m_twistSpan);
00176                             m_solveTwistLimit = true;
00177 
00178                             m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
00179                             m_twistAxis.normalize();
00180 
00181                             m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
00182                                    getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
00183 
00184                      }
00185        }
00186 }
00187 
00188 void   btConeTwistConstraint::solveConstraint(btScalar  timeStep)
00189 {
00190 
00191        btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
00192        btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
00193 
00194        btScalar tau = btScalar(0.3);
00195 
00196        //linear part
00197        if (!m_angularOnly)
00198        {
00199               btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
00200               btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
00201 
00202               btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
00203               btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
00204               btVector3 vel = vel1 - vel2;
00205 
00206               for (int i=0;i<3;i++)
00207               {             
00208                      const btVector3& normal = m_jac[i].m_linearJointAxis;
00209                      btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
00210 
00211                      btScalar rel_vel;
00212                      rel_vel = normal.dot(vel);
00213                      //positional error (zeroth order error)
00214                      btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
00215                      btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
00216                      m_appliedImpulse += impulse;
00217                      btVector3 impulse_vector = normal * impulse;
00218                      m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
00219                      m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
00220               }
00221        }
00222        
00223        {
00225               const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
00226               const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
00227 
00228               // solve swing limit
00229               if (m_solveSwingLimit)
00230               {
00231                      btScalar amplitude = ((angVelB - angVelA).dot( m_swingAxis )*m_relaxationFactor*m_relaxationFactor + m_swingCorrection*(btScalar(1.)/timeStep)*m_biasFactor);
00232                      btScalar impulseMag = amplitude * m_kSwing;
00233 
00234                      // Clamp the accumulated impulse
00235                      btScalar temp = m_accSwingLimitImpulse;
00236                      m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) );
00237                      impulseMag = m_accSwingLimitImpulse - temp;
00238 
00239                      btVector3 impulse = m_swingAxis * impulseMag;
00240 
00241                      m_rbA.applyTorqueImpulse(impulse);
00242                      m_rbB.applyTorqueImpulse(-impulse);
00243 
00244               }
00245 
00246               // solve twist limit
00247               if (m_solveTwistLimit)
00248               {
00249                      btScalar amplitude = ((angVelB - angVelA).dot( m_twistAxis )*m_relaxationFactor*m_relaxationFactor + m_twistCorrection*(btScalar(1.)/timeStep)*m_biasFactor );
00250                      btScalar impulseMag = amplitude * m_kTwist;
00251 
00252                      // Clamp the accumulated impulse
00253                      btScalar temp = m_accTwistLimitImpulse;
00254                      m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) );
00255                      impulseMag = m_accTwistLimitImpulse - temp;
00256 
00257                      btVector3 impulse = m_twistAxis * impulseMag;
00258 
00259                      m_rbA.applyTorqueImpulse(impulse);
00260                      m_rbB.applyTorqueImpulse(-impulse);
00261 
00262               }
00263        
00264        }
00265 
00266 }
00267 
00268 void   btConeTwistConstraint::updateRHS(btScalar timeStep)
00269 {
00270        (void)timeStep;
00271 
00272 }