Back to index

supertuxkart  0.5+dfsg1
btOdeTypedJoint.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 #include "btOdeTypedJoint.h"
00016 #include "btOdeSolverBody.h"
00017 #include "btOdeMacros.h"
00018 #include <stdio.h>
00019 
00020 void btOdeTypedJoint::GetInfo1(Info1 *info)
00021 {
00022     int joint_type = m_constraint->getConstraintType();
00023     switch (joint_type)
00024     {
00025     case POINT2POINT_CONSTRAINT_TYPE:
00026     {
00027         OdeP2PJoint p2pjoint(m_constraint,m_index,m_swapBodies,m_body0,m_body1);
00028         p2pjoint.GetInfo1(info);
00029     }
00030     break;
00031     case D6_CONSTRAINT_TYPE:
00032     {
00033         OdeD6Joint d6joint(m_constraint,m_index,m_swapBodies,m_body0,m_body1);
00034         d6joint.GetInfo1(info);
00035     }
00036     break;
00037     };
00038 }
00039 
00040 void btOdeTypedJoint::GetInfo2(Info2 *info)
00041 {
00042     int joint_type = m_constraint->getConstraintType();
00043     switch (joint_type)
00044     {
00045     case POINT2POINT_CONSTRAINT_TYPE:
00046     {
00047         OdeP2PJoint p2pjoint(m_constraint,m_index,m_swapBodies,m_body0,m_body1);
00048         p2pjoint.GetInfo2(info);
00049     }
00050     break;
00051     case D6_CONSTRAINT_TYPE:
00052     {
00053         OdeD6Joint d6joint(m_constraint,m_index,m_swapBodies,m_body0,m_body1);
00054         d6joint.GetInfo2(info);
00055     }
00056     break;
00057     };
00058 }
00059 
00060 
00061 OdeP2PJoint::OdeP2PJoint(
00062     btTypedConstraint * constraint,
00063     int index,bool swap,btOdeSolverBody* body0,btOdeSolverBody* body1):
00064         btOdeTypedJoint(constraint,index,swap,body0,body1)
00065 {
00066 }
00067 
00068 
00069 void OdeP2PJoint::GetInfo1(Info1 *info)
00070 {
00071     info->m = 3;
00072     info->nub = 3;
00073 }
00074 
00075 
00076 void OdeP2PJoint::GetInfo2(Info2 *info)
00077 {
00078 
00079     btPoint2PointConstraint * p2pconstraint = this->getP2PConstraint();
00080 
00081     //retrieve matrices
00082     btTransform body0_trans;
00083     if (m_body0)
00084     {
00085         body0_trans = m_body0->m_originalBody->getCenterOfMassTransform();
00086     }
00087 //    btScalar body0_mat[12];
00088 //    body0_mat[0] = body0_trans.getBasis()[0][0];
00089 //    body0_mat[1] = body0_trans.getBasis()[0][1];
00090 //    body0_mat[2] = body0_trans.getBasis()[0][2];
00091 //    body0_mat[4] = body0_trans.getBasis()[1][0];
00092 //    body0_mat[5] = body0_trans.getBasis()[1][1];
00093 //    body0_mat[6] = body0_trans.getBasis()[1][2];
00094 //    body0_mat[8] = body0_trans.getBasis()[2][0];
00095 //    body0_mat[9] = body0_trans.getBasis()[2][1];
00096 //    body0_mat[10] = body0_trans.getBasis()[2][2];
00097 
00098     btTransform body1_trans;
00099 
00100     if (m_body1)
00101     {
00102         body1_trans = m_body1->m_originalBody->getCenterOfMassTransform();
00103     }
00104 //    btScalar body1_mat[12];
00105 //    body1_mat[0] = body1_trans.getBasis()[0][0];
00106 //    body1_mat[1] = body1_trans.getBasis()[0][1];
00107 //    body1_mat[2] = body1_trans.getBasis()[0][2];
00108 //    body1_mat[4] = body1_trans.getBasis()[1][0];
00109 //    body1_mat[5] = body1_trans.getBasis()[1][1];
00110 //    body1_mat[6] = body1_trans.getBasis()[1][2];
00111 //    body1_mat[8] = body1_trans.getBasis()[2][0];
00112 //    body1_mat[9] = body1_trans.getBasis()[2][1];
00113 //    body1_mat[10] = body1_trans.getBasis()[2][2];
00114 
00115 
00116 
00117 
00118     // anchor points in global coordinates with respect to body PORs.
00119 
00120 
00121     int s = info->rowskip;
00122 
00123     // set jacobian
00124     info->J1l[0] = 1;
00125     info->J1l[s+1] = 1;
00126     info->J1l[2*s+2] = 1;
00127 
00128 
00129     btVector3 a1,a2;
00130 
00131     a1 = body0_trans.getBasis()*p2pconstraint->getPivotInA();
00132     //dMULTIPLY0_331 (a1, body0_mat,m_constraint->m_pivotInA);
00133     dCROSSMAT (info->J1a,a1,s,-,+);
00134     if (m_body1)
00135     {
00136         info->J2l[0] = -1;
00137         info->J2l[s+1] = -1;
00138         info->J2l[2*s+2] = -1;
00139         a2 = body1_trans.getBasis()*p2pconstraint->getPivotInB();
00140         //dMULTIPLY0_331 (a2,body1_mat,m_constraint->m_pivotInB);
00141         dCROSSMAT (info->J2a,a2,s,+,-);
00142     }
00143 
00144 
00145     // set right hand side
00146     btScalar k = info->fps * info->erp;
00147     if (m_body1)
00148     {
00149         for (int j=0; j<3; j++)
00150         {
00151             info->c[j] = k * (a2[j] + body1_trans.getOrigin()[j] -
00152                               a1[j] - body0_trans.getOrigin()[j]);
00153         }
00154     }
00155     else
00156     {
00157         for (int j=0; j<3; j++)
00158         {
00159             info->c[j] = k * (p2pconstraint->getPivotInB()[j] - a1[j] -
00160                               body0_trans.getOrigin()[j]);
00161         }
00162     }
00163 }
00164 
00165 
00167 
00169 int bt_get_limit_motor_info2(
00170        btRotationalLimitMotor * limot,
00171        btRigidBody * body0, btRigidBody * body1,
00172        btOdeJoint::Info2 *info, int row, btVector3& ax1, int rotational)
00173 {
00174 
00175 
00176     int srow = row * info->rowskip;
00177 
00178     // if the joint is powered, or has joint limits, add in the extra row
00179     int powered = limot->m_enableMotor;
00180     int limit = limot->m_currentLimit;
00181 
00182     if (powered || limit)
00183     {
00184         btScalar *J1 = rotational ? info->J1a : info->J1l;
00185         btScalar *J2 = rotational ? info->J2a : info->J2l;
00186 
00187         J1[srow+0] = ax1[0];
00188         J1[srow+1] = ax1[1];
00189         J1[srow+2] = ax1[2];
00190         if (body1)
00191         {
00192             J2[srow+0] = -ax1[0];
00193             J2[srow+1] = -ax1[1];
00194             J2[srow+2] = -ax1[2];
00195         }
00196 
00197         // linear limot torque decoupling step:
00198         //
00199         // if this is a linear limot (e.g. from a slider), we have to be careful
00200         // that the linear constraint forces (+/- ax1) applied to the two bodies
00201         // do not create a torque couple. in other words, the points that the
00202         // constraint force is applied at must lie along the same ax1 axis.
00203         // a torque couple will result in powered or limited slider-jointed free
00204         // bodies from gaining angular momentum.
00205         // the solution used here is to apply the constraint forces at the point
00206         // halfway between the body centers. there is no penalty (other than an
00207         // extra tiny bit of computation) in doing this adjustment. note that we
00208         // only need to do this if the constraint connects two bodies.
00209 
00210         btVector3 ltd;      // Linear Torque Decoupling vector (a torque)
00211         if (!rotational && body1)
00212         {
00213             btVector3 c;
00214             c[0]=btScalar(0.5)*(body1->getCenterOfMassPosition()[0]
00215                                    -body0->getCenterOfMassPosition()[0]);
00216             c[1]=btScalar(0.5)*(body1->getCenterOfMassPosition()[1]
00217                                    -body0->getCenterOfMassPosition()[1]);
00218             c[2]=btScalar(0.5)*(body1->getCenterOfMassPosition()[2]
00219                                    -body0->getCenterOfMassPosition()[2]);
00220 
00221                      ltd = c.cross(ax1);
00222 
00223             info->J1a[srow+0] = ltd[0];
00224             info->J1a[srow+1] = ltd[1];
00225             info->J1a[srow+2] = ltd[2];
00226             info->J2a[srow+0] = ltd[0];
00227             info->J2a[srow+1] = ltd[1];
00228             info->J2a[srow+2] = ltd[2];
00229         }
00230 
00231         // if we're limited low and high simultaneously, the joint motor is
00232         // ineffective
00233 
00234         if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0;
00235 
00236         if (powered)
00237         {
00238             info->cfm[row] = 0.0f;//limot->m_normalCFM;
00239             if (! limit)
00240             {
00241                 info->c[row] = limot->m_targetVelocity;
00242                 info->lo[row] = -limot->m_maxMotorForce;
00243                 info->hi[row] = limot->m_maxMotorForce;
00244             }
00245         }
00246 
00247         if (limit)
00248         {
00249             btScalar k = info->fps * limot->m_ERP;
00250             info->c[row] = -k * limot->m_currentLimitError;
00251             info->cfm[row] = 0.0f;//limot->m_stopCFM;
00252 
00253             if (limot->m_loLimit == limot->m_hiLimit)
00254             {
00255                 // limited low and high simultaneously
00256                 info->lo[row] = -dInfinity;
00257                 info->hi[row] = dInfinity;
00258             }
00259             else
00260             {
00261                 if (limit == 1)
00262                 {
00263                     // low limit
00264                     info->lo[row] = 0;
00265                     info->hi[row] = SIMD_INFINITY;
00266                 }
00267                 else
00268                 {
00269                     // high limit
00270                     info->lo[row] = -SIMD_INFINITY;
00271                     info->hi[row] = 0;
00272                 }
00273 
00274                 // deal with bounce
00275                 if (limot->m_bounce > 0)
00276                 {
00277                     // calculate joint velocity
00278                     btScalar vel;
00279                     if (rotational)
00280                     {
00281                         vel = body0->getAngularVelocity().dot(ax1);
00282                         if (body1)
00283                             vel -= body1->getAngularVelocity().dot(ax1);
00284                     }
00285                     else
00286                     {
00287                         vel = body0->getLinearVelocity().dot(ax1);
00288                         if (body1)
00289                             vel -= body1->getLinearVelocity().dot(ax1);
00290                     }
00291 
00292                     // only apply bounce if the velocity is incoming, and if the
00293                     // resulting c[] exceeds what we already have.
00294                     if (limit == 1)
00295                     {
00296                         // low limit
00297                         if (vel < 0)
00298                         {
00299                             btScalar newc = -limot->m_bounce* vel;
00300                             if (newc > info->c[row]) info->c[row] = newc;
00301                         }
00302                     }
00303                     else
00304                     {
00305                         // high limit - all those computations are reversed
00306                         if (vel > 0)
00307                         {
00308                             btScalar newc = -limot->m_bounce * vel;
00309                             if (newc < info->c[row]) info->c[row] = newc;
00310                         }
00311                     }
00312                 }
00313             }
00314         }
00315         return 1;
00316     }
00317     else return 0;
00318 }
00319 
00320 
00322 
00323 
00324 
00325 
00326 
00327 OdeD6Joint::OdeD6Joint(
00328     btTypedConstraint * constraint,
00329     int index,bool swap,btOdeSolverBody* body0,btOdeSolverBody* body1):
00330         btOdeTypedJoint(constraint,index,swap,body0,body1)
00331 {
00332 }
00333 
00334 
00335 void OdeD6Joint::GetInfo1(Info1 *info)
00336 {
00337        btGeneric6DofConstraint * d6constraint = this->getD6Constraint();
00338        //prepare constraint
00339        d6constraint->calculateTransforms();
00340     info->m = 3;
00341     info->nub = 3;
00342 
00343     //test angular limits
00344     for (int i=0;i<3 ;i++ )
00345     {
00346        //if(i==2) continue;
00347               if(d6constraint->testAngularLimitMotor(i))
00348               {
00349                      info->m++;
00350               }
00351     }
00352 
00353 
00354 }
00355 
00356 
00357 int OdeD6Joint::setLinearLimits(Info2 *info)
00358 {
00359 
00360     btGeneric6DofConstraint * d6constraint = this->getD6Constraint();
00361 
00362     //retrieve matrices
00363     btTransform body0_trans;
00364     if (m_body0)
00365     {
00366         body0_trans = m_body0->m_originalBody->getCenterOfMassTransform();
00367     }
00368 
00369     btTransform body1_trans;
00370 
00371     if (m_body1)
00372     {
00373         body1_trans = m_body1->m_originalBody->getCenterOfMassTransform();
00374     }
00375 
00376     // anchor points in global coordinates with respect to body PORs.
00377 
00378     int s = info->rowskip;
00379 
00380     // set jacobian
00381     info->J1l[0] = 1;
00382     info->J1l[s+1] = 1;
00383     info->J1l[2*s+2] = 1;
00384 
00385 
00386     btVector3 a1,a2;
00387 
00388     a1 = body0_trans.getBasis()*d6constraint->getFrameOffsetA().getOrigin();
00389     //dMULTIPLY0_331 (a1, body0_mat,m_constraint->m_pivotInA);
00390     dCROSSMAT (info->J1a,a1,s,-,+);
00391     if (m_body1)
00392     {
00393         info->J2l[0] = -1;
00394         info->J2l[s+1] = -1;
00395         info->J2l[2*s+2] = -1;
00396         a2 = body1_trans.getBasis()*d6constraint->getFrameOffsetB().getOrigin();
00397 
00398         //dMULTIPLY0_331 (a2,body1_mat,m_constraint->m_pivotInB);
00399         dCROSSMAT (info->J2a,a2,s,+,-);
00400     }
00401 
00402 
00403     // set right hand side
00404     btScalar k = info->fps * info->erp;
00405     if (m_body1)
00406     {
00407         for (int j=0; j<3; j++)
00408         {
00409             info->c[j] = k * (a2[j] + body1_trans.getOrigin()[j] -
00410                               a1[j] - body0_trans.getOrigin()[j]);
00411         }
00412     }
00413     else
00414     {
00415         for (int j=0; j<3; j++)
00416         {
00417             info->c[j] = k * (d6constraint->getCalculatedTransformB().getOrigin()[j] - a1[j] -
00418                               body0_trans.getOrigin()[j]);
00419         }
00420     }
00421 
00422     return 3;
00423 
00424 }
00425 
00426 int OdeD6Joint::setAngularLimits(Info2 *info, int row_offset)
00427 {
00428        btGeneric6DofConstraint * d6constraint = this->getD6Constraint();
00429        int row = row_offset;
00430        //solve angular limits
00431     for (int i=0;i<3 ;i++ )
00432     {
00433        //if(i==2) continue;
00434               if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
00435               {
00436                      btVector3 axis = d6constraint->getAxis(i);
00437                      row += bt_get_limit_motor_info2(
00438                             d6constraint->getRotationalLimitMotor(i),
00439                             m_body0->m_originalBody,m_body1->m_originalBody,info,row,axis,1);
00440               }
00441     }
00442 
00443     return row;
00444 }
00445 
00446 void OdeD6Joint::GetInfo2(Info2 *info)
00447 {
00448     int row = setLinearLimits(info);
00449     setAngularLimits(info, row);
00450 }
00451