Back to index

supertuxkart  0.5+dfsg1
btGeneric6DofConstraint.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 2007-09-09
00017 Refactored by Francisco León
00018 email: projectileman@yahoo.com
00019 http://gimpact.sf.net
00020 */
00021 
00022 
00023 #include "btGeneric6DofConstraint.h"
00024 #include "BulletDynamics/Dynamics/btRigidBody.h"
00025 #include "LinearMath/btTransformUtil.h"
00026 #include <new>
00027 
00028 
00029 static const btScalar kSign[] = { btScalar(1.0), btScalar(-1.0), btScalar(1.0) };
00030 static const int kAxisA[] = { 1, 0, 0 };
00031 static const int kAxisB[] = { 2, 2, 1 };
00032 #define GENERIC_D6_DISABLE_WARMSTARTING 1
00033 
00034 btScalar btGetMatrixElem(const btMatrix3x3& mat, int index)
00035 {
00036        int i = index%3;
00037        int j = index/3;
00038        return mat[i][j];
00039 }
00040 
00042 bool   matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
00043 {
00044 //     // rot =  cy*cz          -cy*sz           sy
00045 //     //        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
00046 //     //       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
00047 //
00048 
00049               if (btGetMatrixElem(mat,2) < btScalar(1.0))
00050               {
00051                      if (btGetMatrixElem(mat,2) > btScalar(-1.0))
00052                      {
00053                             xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
00054                             xyz[1] = btAsin(btGetMatrixElem(mat,2));
00055                             xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
00056                             return true;
00057                      }
00058                      else
00059                      {
00060                             // WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)
00061                             xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
00062                             xyz[1] = -SIMD_HALF_PI;
00063                             xyz[2] = btScalar(0.0);
00064                             return false;
00065                      }
00066               }
00067               else
00068               {
00069                      // WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
00070                      xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
00071                      xyz[1] = SIMD_HALF_PI;
00072                      xyz[2] = 0.0;
00073 
00074               }
00075 
00076 
00077        return false;
00078 }
00079 
00080 
00081 
00083 
00084 
00085 int btRotationalLimitMotor::testLimitValue(btScalar test_value)
00086 {
00087        if(m_loLimit>m_hiLimit)
00088        {
00089               m_currentLimit = 0;//Free from violation
00090               return 0;
00091        }
00092 
00093        if (test_value < m_loLimit)
00094        {
00095               m_currentLimit = 1;//low limit violation
00096               m_currentLimitError =  test_value - m_loLimit;
00097               return 1;
00098        }
00099        else if (test_value> m_hiLimit)
00100        {
00101               m_currentLimit = 2;//High limit violation
00102               m_currentLimitError = test_value - m_hiLimit;
00103               return 2;
00104        }
00105        else
00106        {
00107               m_currentLimit = 0;//Free from violation
00108               return 0;
00109        }
00110        return 0;
00111 }
00112 
00113 
00114 btScalar btRotationalLimitMotor::solveAngularLimits(
00115               btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
00116               btRigidBody * body0, btRigidBody * body1)
00117 {
00118     if (needApplyTorques()==false) return 0.0f;
00119 
00120     btScalar target_velocity = m_targetVelocity;
00121     btScalar maxMotorForce = m_maxMotorForce;
00122 
00123        //current error correction
00124     if (m_currentLimit!=0)
00125     {
00126         target_velocity = -m_ERP*m_currentLimitError/(timeStep);
00127         maxMotorForce = m_maxLimitForce;
00128     }
00129 
00130     maxMotorForce *= timeStep;
00131 
00132     // current velocity difference
00133     btVector3 vel_diff = body0->getAngularVelocity();
00134     if (body1)
00135     {
00136         vel_diff -= body1->getAngularVelocity();
00137     }
00138 
00139 
00140 
00141     btScalar rel_vel = axis.dot(vel_diff);
00142 
00143        // correction velocity
00144     btScalar motor_relvel = m_limitSoftness*(target_velocity  - m_damping*rel_vel);
00145 
00146 
00147     if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON  )
00148     {
00149         return 0.0f;//no need for applying force
00150     }
00151 
00152 
00153        // correction impulse
00154     btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv;
00155 
00156        // clip correction impulse
00157     btScalar clippedMotorImpulse;
00158 
00159     //todo: should clip against accumulated impulse
00160     if (unclippedMotorImpulse>0.0f)
00161     {
00162         clippedMotorImpulse =  unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
00163     }
00164     else
00165     {
00166         clippedMotorImpulse =  unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
00167     }
00168 
00169 
00170        // sort with accumulated impulses
00171     btScalar  lo = btScalar(-1e30);
00172     btScalar  hi = btScalar(1e30);
00173 
00174     btScalar oldaccumImpulse = m_accumulatedImpulse;
00175     btScalar sum = oldaccumImpulse + clippedMotorImpulse;
00176     m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
00177 
00178     clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
00179 
00180 
00181 
00182     btVector3 motorImp = clippedMotorImpulse * axis;
00183 
00184 
00185     body0->applyTorqueImpulse(motorImp);
00186     if (body1) body1->applyTorqueImpulse(-motorImp);
00187 
00188     return clippedMotorImpulse;
00189 
00190 
00191 }
00192 
00194 
00196 btScalar btTranslationalLimitMotor::solveLinearAxis(
00197               btScalar timeStep,
00198         btScalar jacDiagABInv,
00199         btRigidBody& body1,const btVector3 &pointInA,
00200         btRigidBody& body2,const btVector3 &pointInB,
00201         int limit_index,
00202         const btVector3 & axis_normal_on_a)
00203 {
00204 
00206     btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
00207     btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
00208 
00209     btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
00210     btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
00211     btVector3 vel = vel1 - vel2;
00212 
00213     btScalar rel_vel = axis_normal_on_a.dot(vel);
00214 
00215 
00216 
00218 
00219 //positional error (zeroth order error)
00220     btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a);
00221     btScalar  lo = btScalar(-1e30);
00222     btScalar  hi = btScalar(1e30);
00223 
00224     btScalar minLimit = m_lowerLimit[limit_index];
00225     btScalar maxLimit = m_upperLimit[limit_index];
00226 
00227     //handle the limits
00228     if (minLimit < maxLimit)
00229     {
00230         {
00231             if (depth > maxLimit)
00232             {
00233                 depth -= maxLimit;
00234                 lo = btScalar(0.);
00235 
00236             }
00237             else
00238             {
00239                 if (depth < minLimit)
00240                 {
00241                     depth -= minLimit;
00242                     hi = btScalar(0.);
00243                 }
00244                 else
00245                 {
00246                     return 0.0f;
00247                 }
00248             }
00249         }
00250     }
00251 
00252     btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv;
00253 
00254 
00255 
00256 
00257     btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index];
00258     btScalar sum = oldNormalImpulse + normalImpulse;
00259     m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
00260     normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
00261 
00262     btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
00263     body1.applyImpulse( impulse_vector, rel_pos1);
00264     body2.applyImpulse(-impulse_vector, rel_pos2);
00265     return normalImpulse;
00266 }
00267 
00269 
00270 
00271 btGeneric6DofConstraint::btGeneric6DofConstraint()
00272         :btTypedConstraint(D6_CONSTRAINT_TYPE),
00273               m_useLinearReferenceFrameA(true)
00274 {
00275 }
00276 
00277 btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
00278         : btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB)
00279         , m_frameInA(frameInA)
00280         , m_frameInB(frameInB),
00281               m_useLinearReferenceFrameA(useLinearReferenceFrameA)
00282 {
00283 
00284 }
00285 
00286 
00287 
00288 
00289 
00290 void btGeneric6DofConstraint::calculateAngleInfo()
00291 {
00292        btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis();
00293 
00294        matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff);
00295 
00296 
00297 
00298        // in euler angle mode we do not actually constrain the angular velocity
00299   // along the axes axis[0] and axis[2] (although we do use axis[1]) :
00300   //
00301   //    to get                     constrain w2-w1 along              ...not
00302   //    ------                     ---------------------              ------
00303   //    d(angle[0])/dt = 0  ax[1] x ax[2]               ax[0]
00304   //    d(angle[1])/dt = 0  ax[1]
00305   //    d(angle[2])/dt = 0  ax[0] x ax[1]               ax[2]
00306   //
00307   // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
00308   // to prove the result for angle[0], write the expression for angle[0] from
00309   // GetInfo1 then take the derivative. to prove this for angle[2] it is
00310   // easier to take the euler rate expression for d(angle[2])/dt with respect
00311   // to the components of w and set that to 0.
00312 
00313        btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
00314        btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
00315 
00316        m_calculatedAxis[1] = axis2.cross(axis0);
00317        m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
00318        m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
00319 
00320 
00321 //    if(m_debugDrawer)
00322 //    {
00323 //
00324 //     char buff[300];
00325 //            sprintf(buff,"\n X: %.2f ; Y: %.2f ; Z: %.2f ",
00326 //            m_calculatedAxisAngleDiff[0],
00327 //            m_calculatedAxisAngleDiff[1],
00328 //            m_calculatedAxisAngleDiff[2]);
00329 //     m_debugDrawer->reportErrorWarning(buff);
00330 //    }
00331 
00332 }
00333 
00334 void btGeneric6DofConstraint::calculateTransforms()
00335 {
00336     m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
00337     m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB;
00338 
00339     calculateAngleInfo();
00340 }
00341 
00342 
00343 void btGeneric6DofConstraint::buildLinearJacobian(
00344     btJacobianEntry & jacLinear,const btVector3 & normalWorld,
00345     const btVector3 & pivotAInW,const btVector3 & pivotBInW)
00346 {
00347     new (&jacLinear) btJacobianEntry(
00348         m_rbA.getCenterOfMassTransform().getBasis().transpose(),
00349         m_rbB.getCenterOfMassTransform().getBasis().transpose(),
00350         pivotAInW - m_rbA.getCenterOfMassPosition(),
00351         pivotBInW - m_rbB.getCenterOfMassPosition(),
00352         normalWorld,
00353         m_rbA.getInvInertiaDiagLocal(),
00354         m_rbA.getInvMass(),
00355         m_rbB.getInvInertiaDiagLocal(),
00356         m_rbB.getInvMass());
00357 
00358 }
00359 
00360 void btGeneric6DofConstraint::buildAngularJacobian(
00361     btJacobianEntry & jacAngular,const btVector3 & jointAxisW)
00362 {
00363     new (&jacAngular)       btJacobianEntry(jointAxisW,
00364                                       m_rbA.getCenterOfMassTransform().getBasis().transpose(),
00365                                       m_rbB.getCenterOfMassTransform().getBasis().transpose(),
00366                                       m_rbA.getInvInertiaDiagLocal(),
00367                                       m_rbB.getInvInertiaDiagLocal());
00368 
00369 }
00370 
00371 bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
00372 {
00373     btScalar angle = m_calculatedAxisAngleDiff[axis_index];
00374 
00375     //test limits
00376     m_angularLimits[axis_index].testLimitValue(angle);
00377     return m_angularLimits[axis_index].needApplyTorques();
00378 }
00379 
00380 void btGeneric6DofConstraint::buildJacobian()
00381 {
00382 
00383        // Clear accumulated impulses for the next simulation step
00384     m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
00385     int i;
00386     for(i = 0; i < 3; i++)
00387     {
00388         m_angularLimits[i].m_accumulatedImpulse = btScalar(0.);
00389     }
00390     //calculates transform
00391     calculateTransforms();
00392 
00393     const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();
00394     const btVector3& pivotBInW = m_calculatedTransformB.getOrigin();
00395 
00396 
00397     btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
00398     btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
00399 
00400     btVector3 normalWorld;
00401     //linear part
00402     for (i=0;i<3;i++)
00403     {
00404         if (m_linearLimits.isLimited(i))
00405         {
00406                      if (m_useLinearReferenceFrameA)
00407                    normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
00408                      else
00409                    normalWorld = m_calculatedTransformB.getBasis().getColumn(i);
00410 
00411             buildLinearJacobian(
00412                 m_jacLinear[i],normalWorld ,
00413                 pivotAInW,pivotBInW);
00414 
00415         }
00416     }
00417 
00418     // angular part
00419     for (i=0;i<3;i++)
00420     {
00421         //calculates error angle
00422         if (testAngularLimitMotor(i))
00423         {
00424             normalWorld = this->getAxis(i);
00425             // Create angular atom
00426             buildAngularJacobian(m_jacAng[i],normalWorld);
00427         }
00428     }
00429 
00430 
00431 }
00432 
00433 
00434 void btGeneric6DofConstraint::solveConstraint(btScalar  timeStep)
00435 {
00436     m_timeStep = timeStep;
00437 
00438     //calculateTransforms();
00439 
00440     int i;
00441 
00442     // linear
00443 
00444     btVector3 pointInA = m_calculatedTransformA.getOrigin();
00445        btVector3 pointInB = m_calculatedTransformB.getOrigin();
00446 
00447        btScalar jacDiagABInv;
00448        btVector3 linear_axis;
00449     for (i=0;i<3;i++)
00450     {
00451         if (m_linearLimits.isLimited(i))
00452         {
00453             jacDiagABInv = btScalar(1.) / m_jacLinear[i].getDiagonal();
00454 
00455                      if (m_useLinearReferenceFrameA)
00456                    linear_axis = m_calculatedTransformA.getBasis().getColumn(i);
00457                      else
00458                    linear_axis = m_calculatedTransformB.getBasis().getColumn(i);
00459 
00460             m_linearLimits.solveLinearAxis(
00461               m_timeStep,
00462               jacDiagABInv,
00463               m_rbA,pointInA,
00464                 m_rbB,pointInB,
00465                 i,linear_axis);
00466 
00467         }
00468     }
00469 
00470     // angular
00471     btVector3 angular_axis;
00472     btScalar angularJacDiagABInv;
00473     for (i=0;i<3;i++)
00474     {
00475         if (m_angularLimits[i].needApplyTorques())
00476         {
00477 
00478                      // get axis
00479                      angular_axis = getAxis(i);
00480 
00481                      angularJacDiagABInv = btScalar(1.) / m_jacAng[i].getDiagonal();
00482 
00483                      m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, &m_rbA,&m_rbB);
00484         }
00485     }
00486 }
00487 
00488 void   btGeneric6DofConstraint::updateRHS(btScalar      timeStep)
00489 {
00490     (void)timeStep;
00491 
00492 }
00493 
00494 btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
00495 {
00496     return m_calculatedAxis[axis_index];
00497 }
00498 
00499 btScalar btGeneric6DofConstraint::getAngle(int axis_index) const
00500 {
00501     return m_calculatedAxisAngleDiff[axis_index];
00502 }
00503 
00504