Back to index

supertuxkart  0.5+dfsg1
btSliderConstraint.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 Added by Roman Ponomarev (rponom@gmail.com)
00018 April 04, 2008
00019 */
00020 
00021 //-----------------------------------------------------------------------------
00022 
00023 #include "btSliderConstraint.h"
00024 #include "BulletDynamics/Dynamics/btRigidBody.h"
00025 #include "LinearMath/btTransformUtil.h"
00026 #include <new>
00027 
00028 //-----------------------------------------------------------------------------
00029 
00030 void btSliderConstraint::initParams()
00031 {
00032     m_lowerLinLimit = btScalar(1.0);
00033     m_upperLinLimit = btScalar(-1.0);
00034     m_lowerAngLimit = btScalar(0.);
00035     m_upperAngLimit = btScalar(0.);
00036        m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
00037        m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
00038        m_dampingDirLin = btScalar(0.);
00039        m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
00040        m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
00041        m_dampingDirAng = btScalar(0.);
00042        m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
00043        m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
00044        m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING;
00045        m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
00046        m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
00047        m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING;
00048        m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
00049        m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
00050        m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING;
00051        m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
00052        m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
00053        m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING;
00054 } // btSliderConstraint::initParams()
00055 
00056 //-----------------------------------------------------------------------------
00057 
00058 btSliderConstraint::btSliderConstraint()
00059         :btTypedConstraint(SLIDER_CONSTRAINT_TYPE),
00060               m_useLinearReferenceFrameA(true)
00061 {
00062        initParams();
00063 } // btSliderConstraint::btSliderConstraint()
00064 
00065 //-----------------------------------------------------------------------------
00066 
00067 btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
00068         : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB)
00069         , m_frameInA(frameInA)
00070         , m_frameInB(frameInB),
00071               m_useLinearReferenceFrameA(useLinearReferenceFrameA)
00072 {
00073        initParams();
00074 } // btSliderConstraint::btSliderConstraint()
00075 
00076 //-----------------------------------------------------------------------------
00077 
00078 void btSliderConstraint::buildJacobian()
00079 {
00080        if(m_useLinearReferenceFrameA)
00081        {
00082               buildJacobianInt(m_rbA, m_rbB, m_frameInA, m_frameInB);
00083        }
00084        else
00085        {
00086               buildJacobianInt(m_rbB, m_rbA, m_frameInB, m_frameInA);
00087        }
00088 } // btSliderConstraint::buildJacobian()
00089 
00090 //-----------------------------------------------------------------------------
00091 
00092 void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB)
00093 {
00094        //calculate transforms
00095     m_calculatedTransformA = rbA.getCenterOfMassTransform() * frameInA;
00096     m_calculatedTransformB = rbB.getCenterOfMassTransform() * frameInB;
00097        m_realPivotAInW = m_calculatedTransformA.getOrigin();
00098        m_realPivotBInW = m_calculatedTransformB.getOrigin();
00099        m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X
00100        m_delta = m_realPivotBInW - m_realPivotAInW;
00101        m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
00102        m_relPosA = m_projPivotInW - rbA.getCenterOfMassPosition();
00103        m_relPosB = m_realPivotBInW - rbB.getCenterOfMassPosition();
00104     btVector3 normalWorld;
00105     int i;
00106     //linear part
00107     for(i = 0; i < 3; i++)
00108     {
00109               normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
00110               new (&m_jacLin[i]) btJacobianEntry(
00111                      rbA.getCenterOfMassTransform().getBasis().transpose(),
00112                      rbB.getCenterOfMassTransform().getBasis().transpose(),
00113                      m_relPosA,
00114                      m_relPosB,
00115                      normalWorld,
00116                      rbA.getInvInertiaDiagLocal(),
00117                      rbA.getInvMass(),
00118                      rbB.getInvInertiaDiagLocal(),
00119                      rbB.getInvMass()
00120                      );
00121               m_jacLinDiagABInv[i] = btScalar(1.) / m_jacLin[i].getDiagonal();
00122               m_depth[i] = m_delta.dot(normalWorld);
00123     }
00124        m_solveLinLim = false;
00125        if(m_lowerLinLimit <= m_upperLinLimit)
00126        {
00127               if(m_depth[0] > m_upperLinLimit)
00128               {
00129                      m_depth[0] -= m_upperLinLimit;
00130                      m_solveLinLim = true;
00131               }
00132               else if(m_depth[0] < m_lowerLinLimit)
00133               {
00134                      m_depth[0] -= m_lowerLinLimit;
00135                      m_solveLinLim = true;
00136               }
00137               else
00138               {
00139                      m_depth[0] = btScalar(0.);
00140               }
00141        }
00142        else
00143        {
00144               m_depth[0] = btScalar(0.);
00145        }
00146     // angular part
00147     for(i = 0; i < 3; i++)
00148     {
00149               normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
00150               new (&m_jacAng[i])   btJacobianEntry(
00151                      normalWorld,
00152             rbA.getCenterOfMassTransform().getBasis().transpose(),
00153             rbB.getCenterOfMassTransform().getBasis().transpose(),
00154             rbA.getInvInertiaDiagLocal(),
00155             rbB.getInvInertiaDiagLocal()
00156                      );
00157        }
00158        m_angDepth = btScalar(0.);
00159        m_solveAngLim = false;
00160        if(m_lowerAngLimit <= m_upperAngLimit)
00161        {
00162               const btVector3 axisA0 = m_calculatedTransformA.getBasis().getColumn(1);
00163               const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2);
00164               const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1);
00165               btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0));  
00166               if(rot < m_lowerAngLimit)
00167               {
00168                      m_angDepth = rot - m_lowerAngLimit;
00169                      m_solveAngLim = true;
00170               } 
00171               else if(rot > m_upperAngLimit)
00172               {
00173                      m_angDepth = rot - m_upperAngLimit;
00174                      m_solveAngLim = true;
00175               }
00176        }
00177        btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0);
00178        m_kAngle = btScalar(1.0 )/ (rbA.computeAngularImpulseDenominator(axisA) + rbB.computeAngularImpulseDenominator(axisA));
00179 } // btSliderConstraint::buildJacobianInt()
00180 
00181 //-----------------------------------------------------------------------------
00182 
00183 void btSliderConstraint::solveConstraint(btScalar timeStep)
00184 {
00185     m_timeStep = timeStep;
00186        if(m_useLinearReferenceFrameA)
00187        {
00188               solveConstraintInt(m_rbA, m_rbB);
00189        }
00190        else
00191        {
00192               solveConstraintInt(m_rbB, m_rbA);
00193        }
00194 } // btSliderConstraint::solveConstraint()
00195 
00196 //-----------------------------------------------------------------------------
00197 
00198 void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB)
00199 {
00200     int i;
00201     // linear
00202     btVector3 velA = rbA.getVelocityInLocalPoint(m_relPosA);
00203     btVector3 velB = rbB.getVelocityInLocalPoint(m_relPosB);
00204     btVector3 vel = velA - velB;
00205        for(i = 0; i < 3; i++)
00206     {
00207               const btVector3& normal = m_jacLin[i].m_linearJointAxis;
00208               btScalar rel_vel = normal.dot(vel);
00209               // calculate positional error
00210               btScalar depth = m_depth[i];
00211               // get parameters
00212               btScalar softness = (i) ? m_softnessOrthoLin : (m_solveLinLim ? m_softnessLimLin : m_softnessDirLin);
00213               btScalar restitution = (i) ? m_restitutionOrthoLin : (m_solveLinLim ? m_restitutionLimLin : m_restitutionDirLin);
00214               btScalar damping = (i) ? m_dampingOrthoLin : (m_solveLinLim ? m_dampingLimLin : m_dampingDirLin);
00215               // calcutate and apply impulse
00216               btScalar normalImpulse = softness * (restitution * depth / m_timeStep - damping * rel_vel) * m_jacLinDiagABInv[i];
00217               btVector3 impulse_vector = normal * normalImpulse;
00218               rbA.applyImpulse( impulse_vector, m_relPosA);
00219               rbB.applyImpulse(-impulse_vector, m_relPosB);
00220     }
00221        // angular 
00222        // get axes in world space
00223        btVector3 axisA =  m_calculatedTransformA.getBasis().getColumn(0);
00224        btVector3 axisB =  m_calculatedTransformB.getBasis().getColumn(0);
00225 
00226        const btVector3& angVelA = rbA.getAngularVelocity();
00227        const btVector3& angVelB = rbB.getAngularVelocity();
00228 
00229        btVector3 angVelAroundAxisA = axisA * axisA.dot(angVelA);
00230        btVector3 angVelAroundAxisB = axisB * axisB.dot(angVelB);
00231 
00232        btVector3 angAorthog = angVelA - angVelAroundAxisA;
00233        btVector3 angBorthog = angVelB - angVelAroundAxisB;
00234        btVector3 velrelOrthog = angAorthog-angBorthog;
00235        //solve orthogonal angular velocity correction
00236        btScalar len = velrelOrthog.length();
00237        if (len > btScalar(0.00001))
00238        {
00239               btVector3 normal = velrelOrthog.normalized();
00240               btScalar denom = rbA.computeAngularImpulseDenominator(normal) + rbB.computeAngularImpulseDenominator(normal);
00241               velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
00242        }
00243        //solve angular positional correction
00244        btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/m_timeStep);
00245        btScalar len2 = angularError.length();
00246        if (len2>btScalar(0.00001))
00247        {
00248               btVector3 normal2 = angularError.normalized();
00249               btScalar denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2);
00250               angularError *= (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
00251        }
00252        // apply impulse
00253        rbA.applyTorqueImpulse(-velrelOrthog+angularError);
00254        rbB.applyTorqueImpulse(velrelOrthog-angularError);
00255        btScalar impulseMag;
00256        //solve angular limits
00257        if(m_solveAngLim)
00258        {
00259               impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingLimAng + m_angDepth * m_restitutionLimAng / m_timeStep;
00260               impulseMag *= m_kAngle * m_softnessLimAng;
00261        }
00262        else
00263        {
00264               impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingDirAng + m_angDepth * m_restitutionDirAng / m_timeStep;
00265               impulseMag *= m_kAngle * m_softnessDirAng;
00266        }
00267        btVector3 impulse = axisA * impulseMag;
00268        rbA.applyTorqueImpulse(impulse);
00269        rbB.applyTorqueImpulse(-impulse);
00270 } // btSliderConstraint::solveConstraint()
00271 
00272 //-----------------------------------------------------------------------------
00273