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 #include "btGeneric6DofConstraint.h"
00023 #include "BulletDynamics/Dynamics/btRigidBody.h"
00024 #include "LinearMath/btTransformUtil.h"
00025 #include "LinearMath/btTransformUtil.h"
00026 #include <new>
00027 
00028 
00029 
00030 #define D6_USE_OBSOLETE_METHOD false
00031 #define D6_USE_FRAME_OFFSET true
00032 
00033 
00034 
00035 
00036 
00037 
00038 btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
00039 : btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB)
00040 , m_frameInA(frameInA)
00041 , m_frameInB(frameInB),
00042 m_useLinearReferenceFrameA(useLinearReferenceFrameA),
00043 m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
00044 m_flags(0),
00045 m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
00046 {
00047         calculateTransforms();
00048 }
00049 
00050 
00051 
00052 btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB)
00053         : btTypedConstraint(D6_CONSTRAINT_TYPE, getFixedBody(), rbB),
00054                 m_frameInB(frameInB),
00055                 m_useLinearReferenceFrameA(useLinearReferenceFrameB),
00056                 m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
00057                 m_flags(0),
00058                 m_useSolveConstraintObsolete(false)
00059 {
00061         m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB;
00062         calculateTransforms();
00063 }
00064 
00065 
00066 
00067 
00068 #define GENERIC_D6_DISABLE_WARMSTARTING 1
00069 
00070 
00071 
00072 btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
00073 btScalar btGetMatrixElem(const btMatrix3x3& mat, int index)
00074 {
00075         int i = index%3;
00076         int j = index/3;
00077         return mat[i][j];
00078 }
00079 
00080 
00081 
00083 bool    matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);
00084 bool    matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
00085 {
00086         //      // rot =  cy*cz          -cy*sz           sy
00087         //      //        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
00088         //      //       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
00089         //
00090 
00091         btScalar fi = btGetMatrixElem(mat,2);
00092         if (fi < btScalar(1.0f))
00093         {
00094                 if (fi > btScalar(-1.0f))
00095                 {
00096                         xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
00097                         xyz[1] = btAsin(btGetMatrixElem(mat,2));
00098                         xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
00099                         return true;
00100                 }
00101                 else
00102                 {
00103                         // WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)
00104                         xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
00105                         xyz[1] = -SIMD_HALF_PI;
00106                         xyz[2] = btScalar(0.0);
00107                         return false;
00108                 }
00109         }
00110         else
00111         {
00112                 // WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
00113                 xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
00114                 xyz[1] = SIMD_HALF_PI;
00115                 xyz[2] = 0.0;
00116         }
00117         return false;
00118 }
00119 
00121 
00122 int btRotationalLimitMotor::testLimitValue(btScalar test_value)
00123 {
00124         if(m_loLimit>m_hiLimit)
00125         {
00126                 m_currentLimit = 0;//Free from violation
00127                 return 0;
00128         }
00129         if (test_value < m_loLimit)
00130         {
00131                 m_currentLimit = 1;//low limit violation
00132                 m_currentLimitError =  test_value - m_loLimit;
00133                 if(m_currentLimitError>SIMD_PI) 
00134                         m_currentLimitError-=SIMD_2_PI;
00135                 else if(m_currentLimitError<-SIMD_PI) 
00136                         m_currentLimitError+=SIMD_2_PI;
00137                 return 1;
00138         }
00139         else if (test_value> m_hiLimit)
00140         {
00141                 m_currentLimit = 2;//High limit violation
00142                 m_currentLimitError = test_value - m_hiLimit;
00143                 if(m_currentLimitError>SIMD_PI) 
00144                         m_currentLimitError-=SIMD_2_PI;
00145                 else if(m_currentLimitError<-SIMD_PI) 
00146                         m_currentLimitError+=SIMD_2_PI;
00147                 return 2;
00148         };
00149 
00150         m_currentLimit = 0;//Free from violation
00151         return 0;
00152 
00153 }
00154 
00155 
00156 
00157 btScalar btRotationalLimitMotor::solveAngularLimits(
00158         btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
00159         btRigidBody * body0, btRigidBody * body1 )
00160 {
00161         if (needApplyTorques()==false) return 0.0f;
00162 
00163         btScalar target_velocity = m_targetVelocity;
00164         btScalar maxMotorForce = m_maxMotorForce;
00165 
00166         //current error correction
00167         if (m_currentLimit!=0)
00168         {
00169                 target_velocity = -m_stopERP*m_currentLimitError/(timeStep);
00170                 maxMotorForce = m_maxLimitForce;
00171         }
00172 
00173         maxMotorForce *= timeStep;
00174 
00175         // current velocity difference
00176 
00177         btVector3 angVelA = body0->getAngularVelocity();
00178         btVector3 angVelB = body1->getAngularVelocity();
00179 
00180         btVector3 vel_diff;
00181         vel_diff = angVelA-angVelB;
00182 
00183 
00184 
00185         btScalar rel_vel = axis.dot(vel_diff);
00186 
00187         // correction velocity
00188         btScalar motor_relvel = m_limitSoftness*(target_velocity  - m_damping*rel_vel);
00189 
00190 
00191         if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON  )
00192         {
00193                 return 0.0f;//no need for applying force
00194         }
00195 
00196 
00197         // correction impulse
00198         btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv;
00199 
00200         // clip correction impulse
00201         btScalar clippedMotorImpulse;
00202 
00204         if (unclippedMotorImpulse>0.0f)
00205         {
00206                 clippedMotorImpulse =  unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
00207         }
00208         else
00209         {
00210                 clippedMotorImpulse =  unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
00211         }
00212 
00213 
00214         // sort with accumulated impulses
00215         btScalar        lo = btScalar(-BT_LARGE_FLOAT);
00216         btScalar        hi = btScalar(BT_LARGE_FLOAT);
00217 
00218         btScalar oldaccumImpulse = m_accumulatedImpulse;
00219         btScalar sum = oldaccumImpulse + clippedMotorImpulse;
00220         m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
00221 
00222         clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
00223 
00224         btVector3 motorImp = clippedMotorImpulse * axis;
00225 
00226         body0->applyTorqueImpulse(motorImp);
00227         body1->applyTorqueImpulse(-motorImp);
00228 
00229         return clippedMotorImpulse;
00230 
00231 
00232 }
00233 
00235 
00236 
00237 
00238 
00240 
00241 
00242 int btTranslationalLimitMotor::testLimitValue(int limitIndex, btScalar test_value)
00243 {
00244         btScalar loLimit = m_lowerLimit[limitIndex];
00245         btScalar hiLimit = m_upperLimit[limitIndex];
00246         if(loLimit > hiLimit)
00247         {
00248                 m_currentLimit[limitIndex] = 0;//Free from violation
00249                 m_currentLimitError[limitIndex] = btScalar(0.f);
00250                 return 0;
00251         }
00252 
00253         if (test_value < loLimit)
00254         {
00255                 m_currentLimit[limitIndex] = 2;//low limit violation
00256                 m_currentLimitError[limitIndex] =  test_value - loLimit;
00257                 return 2;
00258         }
00259         else if (test_value> hiLimit)
00260         {
00261                 m_currentLimit[limitIndex] = 1;//High limit violation
00262                 m_currentLimitError[limitIndex] = test_value - hiLimit;
00263                 return 1;
00264         };
00265 
00266         m_currentLimit[limitIndex] = 0;//Free from violation
00267         m_currentLimitError[limitIndex] = btScalar(0.f);
00268         return 0;
00269 }
00270 
00271 
00272 
00273 btScalar btTranslationalLimitMotor::solveLinearAxis(
00274         btScalar timeStep,
00275         btScalar jacDiagABInv,
00276         btRigidBody& body1,const btVector3 &pointInA,
00277         btRigidBody& body2,const btVector3 &pointInB,
00278         int limit_index,
00279         const btVector3 & axis_normal_on_a,
00280         const btVector3 & anchorPos)
00281 {
00282 
00284         //    btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
00285         //    btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
00286         btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition();
00287         btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition();
00288 
00289         btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
00290         btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
00291         btVector3 vel = vel1 - vel2;
00292 
00293         btScalar rel_vel = axis_normal_on_a.dot(vel);
00294 
00295 
00296 
00298 
00299         //positional error (zeroth order error)
00300         btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a);
00301         btScalar        lo = btScalar(-BT_LARGE_FLOAT);
00302         btScalar        hi = btScalar(BT_LARGE_FLOAT);
00303 
00304         btScalar minLimit = m_lowerLimit[limit_index];
00305         btScalar maxLimit = m_upperLimit[limit_index];
00306 
00307         //handle the limits
00308         if (minLimit < maxLimit)
00309         {
00310                 {
00311                         if (depth > maxLimit)
00312                         {
00313                                 depth -= maxLimit;
00314                                 lo = btScalar(0.);
00315 
00316                         }
00317                         else
00318                         {
00319                                 if (depth < minLimit)
00320                                 {
00321                                         depth -= minLimit;
00322                                         hi = btScalar(0.);
00323                                 }
00324                                 else
00325                                 {
00326                                         return 0.0f;
00327                                 }
00328                         }
00329                 }
00330         }
00331 
00332         btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv;
00333 
00334 
00335 
00336 
00337         btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index];
00338         btScalar sum = oldNormalImpulse + normalImpulse;
00339         m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
00340         normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
00341 
00342         btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
00343         body1.applyImpulse( impulse_vector, rel_pos1);
00344         body2.applyImpulse(-impulse_vector, rel_pos2);
00345 
00346         
00347 
00348         return normalImpulse;
00349 }
00350 
00352 
00353 void btGeneric6DofConstraint::calculateAngleInfo()
00354 {
00355         btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis();
00356         matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff);
00357         // in euler angle mode we do not actually constrain the angular velocity
00358         // along the axes axis[0] and axis[2] (although we do use axis[1]) :
00359         //
00360         //    to get                    constrain w2-w1 along           ...not
00361         //    ------                    ---------------------           ------
00362         //    d(angle[0])/dt = 0        ax[1] x ax[2]                   ax[0]
00363         //    d(angle[1])/dt = 0        ax[1]
00364         //    d(angle[2])/dt = 0        ax[0] x ax[1]                   ax[2]
00365         //
00366         // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
00367         // to prove the result for angle[0], write the expression for angle[0] from
00368         // GetInfo1 then take the derivative. to prove this for angle[2] it is
00369         // easier to take the euler rate expression for d(angle[2])/dt with respect
00370         // to the components of w and set that to 0.
00371         btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
00372         btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
00373 
00374         m_calculatedAxis[1] = axis2.cross(axis0);
00375         m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
00376         m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
00377 
00378         m_calculatedAxis[0].normalize();
00379         m_calculatedAxis[1].normalize();
00380         m_calculatedAxis[2].normalize();
00381 
00382 }
00383 
00384 void btGeneric6DofConstraint::calculateTransforms()
00385 {
00386         calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
00387 }
00388 
00389 void btGeneric6DofConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB)
00390 {
00391         m_calculatedTransformA = transA * m_frameInA;
00392         m_calculatedTransformB = transB * m_frameInB;
00393         calculateLinearInfo();
00394         calculateAngleInfo();
00395         if(m_useOffsetForConstraintFrame)
00396         {       //  get weight factors depending on masses
00397                 btScalar miA = getRigidBodyA().getInvMass();
00398                 btScalar miB = getRigidBodyB().getInvMass();
00399                 m_hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
00400                 btScalar miS = miA + miB;
00401                 if(miS > btScalar(0.f))
00402                 {
00403                         m_factA = miB / miS;
00404                 }
00405                 else 
00406                 {
00407                         m_factA = btScalar(0.5f);
00408                 }
00409                 m_factB = btScalar(1.0f) - m_factA;
00410         }
00411 }
00412 
00413 
00414 
00415 void btGeneric6DofConstraint::buildLinearJacobian(
00416         btJacobianEntry & jacLinear,const btVector3 & normalWorld,
00417         const btVector3 & pivotAInW,const btVector3 & pivotBInW)
00418 {
00419         new (&jacLinear) btJacobianEntry(
00420         m_rbA.getCenterOfMassTransform().getBasis().transpose(),
00421         m_rbB.getCenterOfMassTransform().getBasis().transpose(),
00422         pivotAInW - m_rbA.getCenterOfMassPosition(),
00423         pivotBInW - m_rbB.getCenterOfMassPosition(),
00424         normalWorld,
00425         m_rbA.getInvInertiaDiagLocal(),
00426         m_rbA.getInvMass(),
00427         m_rbB.getInvInertiaDiagLocal(),
00428         m_rbB.getInvMass());
00429 }
00430 
00431 
00432 
00433 void btGeneric6DofConstraint::buildAngularJacobian(
00434         btJacobianEntry & jacAngular,const btVector3 & jointAxisW)
00435 {
00436          new (&jacAngular)      btJacobianEntry(jointAxisW,
00437                                       m_rbA.getCenterOfMassTransform().getBasis().transpose(),
00438                                       m_rbB.getCenterOfMassTransform().getBasis().transpose(),
00439                                       m_rbA.getInvInertiaDiagLocal(),
00440                                       m_rbB.getInvInertiaDiagLocal());
00441 
00442 }
00443 
00444 
00445 
00446 bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
00447 {
00448         btScalar angle = m_calculatedAxisAngleDiff[axis_index];
00449         angle = btAdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit);
00450         m_angularLimits[axis_index].m_currentPosition = angle;
00451         //test limits
00452         m_angularLimits[axis_index].testLimitValue(angle);
00453         return m_angularLimits[axis_index].needApplyTorques();
00454 }
00455 
00456 
00457 
00458 void btGeneric6DofConstraint::buildJacobian()
00459 {
00460 #ifndef __SPU__
00461         if (m_useSolveConstraintObsolete)
00462         {
00463 
00464                 // Clear accumulated impulses for the next simulation step
00465                 m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
00466                 int i;
00467                 for(i = 0; i < 3; i++)
00468                 {
00469                         m_angularLimits[i].m_accumulatedImpulse = btScalar(0.);
00470                 }
00471                 //calculates transform
00472                 calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
00473 
00474                 //  const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();
00475                 //  const btVector3& pivotBInW = m_calculatedTransformB.getOrigin();
00476                 calcAnchorPos();
00477                 btVector3 pivotAInW = m_AnchorPos;
00478                 btVector3 pivotBInW = m_AnchorPos;
00479 
00480                 // not used here
00481                 //    btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
00482                 //    btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
00483 
00484                 btVector3 normalWorld;
00485                 //linear part
00486                 for (i=0;i<3;i++)
00487                 {
00488                         if (m_linearLimits.isLimited(i))
00489                         {
00490                                 if (m_useLinearReferenceFrameA)
00491                                         normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
00492                                 else
00493                                         normalWorld = m_calculatedTransformB.getBasis().getColumn(i);
00494 
00495                                 buildLinearJacobian(
00496                                         m_jacLinear[i],normalWorld ,
00497                                         pivotAInW,pivotBInW);
00498 
00499                         }
00500                 }
00501 
00502                 // angular part
00503                 for (i=0;i<3;i++)
00504                 {
00505                         //calculates error angle
00506                         if (testAngularLimitMotor(i))
00507                         {
00508                                 normalWorld = this->getAxis(i);
00509                                 // Create angular atom
00510                                 buildAngularJacobian(m_jacAng[i],normalWorld);
00511                         }
00512                 }
00513 
00514         }
00515 #endif //__SPU__
00516 
00517 }
00518 
00519 
00520 void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info)
00521 {
00522         if (m_useSolveConstraintObsolete)
00523         {
00524                 info->m_numConstraintRows = 0;
00525                 info->nub = 0;
00526         } else
00527         {
00528                 //prepare constraint
00529                 calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
00530                 info->m_numConstraintRows = 0;
00531                 info->nub = 6;
00532                 int i;
00533                 //test linear limits
00534                 for(i = 0; i < 3; i++)
00535                 {
00536                         if(m_linearLimits.needApplyForce(i))
00537                         {
00538                                 info->m_numConstraintRows++;
00539                                 info->nub--;
00540                         }
00541                 }
00542                 //test angular limits
00543                 for (i=0;i<3 ;i++ )
00544                 {
00545                         if(testAngularLimitMotor(i))
00546                         {
00547                                 info->m_numConstraintRows++;
00548                                 info->nub--;
00549                         }
00550                 }
00551         }
00552 }
00553 
00554 void btGeneric6DofConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
00555 {
00556         if (m_useSolveConstraintObsolete)
00557         {
00558                 info->m_numConstraintRows = 0;
00559                 info->nub = 0;
00560         } else
00561         {
00562                 //pre-allocate all 6
00563                 info->m_numConstraintRows = 6;
00564                 info->nub = 0;
00565         }
00566 }
00567 
00568 
00569 void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info)
00570 {
00571         btAssert(!m_useSolveConstraintObsolete);
00572 
00573         const btTransform& transA = m_rbA.getCenterOfMassTransform();
00574         const btTransform& transB = m_rbB.getCenterOfMassTransform();
00575         const btVector3& linVelA = m_rbA.getLinearVelocity();
00576         const btVector3& linVelB = m_rbB.getLinearVelocity();
00577         const btVector3& angVelA = m_rbA.getAngularVelocity();
00578         const btVector3& angVelB = m_rbB.getAngularVelocity();
00579 
00580         if(m_useOffsetForConstraintFrame)
00581         { // for stability better to solve angular limits first
00582                 int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
00583                 setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
00584         }
00585         else
00586         { // leave old version for compatibility
00587                 int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
00588                 setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
00589         }
00590 
00591 }
00592 
00593 
00594 void btGeneric6DofConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
00595 {
00596         
00597         btAssert(!m_useSolveConstraintObsolete);
00598         //prepare constraint
00599         calculateTransforms(transA,transB);
00600 
00601         int i;
00602         for (i=0;i<3 ;i++ )
00603         {
00604                 testAngularLimitMotor(i);
00605         }
00606 
00607         if(m_useOffsetForConstraintFrame)
00608         { // for stability better to solve angular limits first
00609                 int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
00610                 setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
00611         }
00612         else
00613         { // leave old version for compatibility
00614                 int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
00615                 setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
00616         }
00617 }
00618 
00619 
00620 
00621 int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
00622 {
00623 //      int row = 0;
00624         //solve linear limits
00625         btRotationalLimitMotor limot;
00626         for (int i=0;i<3 ;i++ )
00627         {
00628                 if(m_linearLimits.needApplyForce(i))
00629                 { // re-use rotational motor code
00630                         limot.m_bounce = btScalar(0.f);
00631                         limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
00632                         limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i];
00633                         limot.m_currentLimitError  = m_linearLimits.m_currentLimitError[i];
00634                         limot.m_damping  = m_linearLimits.m_damping;
00635                         limot.m_enableMotor  = m_linearLimits.m_enableMotor[i];
00636                         limot.m_hiLimit  = m_linearLimits.m_upperLimit[i];
00637                         limot.m_limitSoftness  = m_linearLimits.m_limitSoftness;
00638                         limot.m_loLimit  = m_linearLimits.m_lowerLimit[i];
00639                         limot.m_maxLimitForce  = btScalar(0.f);
00640                         limot.m_maxMotorForce  = m_linearLimits.m_maxMotorForce[i];
00641                         limot.m_targetVelocity  = m_linearLimits.m_targetVelocity[i];
00642                         btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
00643                         int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT);
00644                         limot.m_normalCFM       = (flags & BT_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0];
00645                         limot.m_stopCFM         = (flags & BT_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
00646                         limot.m_stopERP         = (flags & BT_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp;
00647                         if(m_useOffsetForConstraintFrame)
00648                         {
00649                                 int indx1 = (i + 1) % 3;
00650                                 int indx2 = (i + 2) % 3;
00651                                 int rotAllowed = 1; // rotations around orthos to current axis
00652                                 if(m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit)
00653                                 {
00654                                         rotAllowed = 0;
00655                                 }
00656                                 row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed);
00657                         }
00658                         else
00659                         {
00660                                 row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0);
00661                         }
00662                 }
00663         }
00664         return row;
00665 }
00666 
00667 
00668 
00669 int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
00670 {
00671         btGeneric6DofConstraint * d6constraint = this;
00672         int row = row_offset;
00673         //solve angular limits
00674         for (int i=0;i<3 ;i++ )
00675         {
00676                 if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
00677                 {
00678                         btVector3 axis = d6constraint->getAxis(i);
00679                         int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT);
00680                         if(!(flags & BT_6DOF_FLAGS_CFM_NORM))
00681                         {
00682                                 m_angularLimits[i].m_normalCFM = info->cfm[0];
00683                         }
00684                         if(!(flags & BT_6DOF_FLAGS_CFM_STOP))
00685                         {
00686                                 m_angularLimits[i].m_stopCFM = info->cfm[0];
00687                         }
00688                         if(!(flags & BT_6DOF_FLAGS_ERP_STOP))
00689                         {
00690                                 m_angularLimits[i].m_stopERP = info->erp;
00691                         }
00692                         row += get_limit_motor_info2(d6constraint->getRotationalLimitMotor(i),
00693                                                                                                 transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1);
00694                 }
00695         }
00696 
00697         return row;
00698 }
00699 
00700 
00701 
00702 
00703 void    btGeneric6DofConstraint::updateRHS(btScalar     timeStep)
00704 {
00705         (void)timeStep;
00706 
00707 }
00708 
00709 
00710 void btGeneric6DofConstraint::setFrames(const btTransform& frameA, const btTransform& frameB)
00711 {
00712         m_frameInA = frameA;
00713         m_frameInB = frameB;
00714         buildJacobian();
00715         calculateTransforms();
00716 }
00717 
00718 
00719 
00720 btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
00721 {
00722         return m_calculatedAxis[axis_index];
00723 }
00724 
00725 
00726 btScalar        btGeneric6DofConstraint::getRelativePivotPosition(int axisIndex) const
00727 {
00728         return m_calculatedLinearDiff[axisIndex];
00729 }
00730 
00731 
00732 btScalar btGeneric6DofConstraint::getAngle(int axisIndex) const
00733 {
00734         return m_calculatedAxisAngleDiff[axisIndex];
00735 }
00736 
00737 
00738 
00739 void btGeneric6DofConstraint::calcAnchorPos(void)
00740 {
00741         btScalar imA = m_rbA.getInvMass();
00742         btScalar imB = m_rbB.getInvMass();
00743         btScalar weight;
00744         if(imB == btScalar(0.0))
00745         {
00746                 weight = btScalar(1.0);
00747         }
00748         else
00749         {
00750                 weight = imA / (imA + imB);
00751         }
00752         const btVector3& pA = m_calculatedTransformA.getOrigin();
00753         const btVector3& pB = m_calculatedTransformB.getOrigin();
00754         m_AnchorPos = pA * weight + pB * (btScalar(1.0) - weight);
00755         return;
00756 }
00757 
00758 
00759 
00760 void btGeneric6DofConstraint::calculateLinearInfo()
00761 {
00762         m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();
00763         m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;
00764         for(int i = 0; i < 3; i++)
00765         {
00766                 m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i];
00767                 m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
00768         }
00769 }
00770 
00771 
00772 
00773 int btGeneric6DofConstraint::get_limit_motor_info2(
00774         btRotationalLimitMotor * limot,
00775         const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
00776         btConstraintInfo2 *info, int row, btVector3& ax1, int rotational,int rotAllowed)
00777 {
00778     int srow = row * info->rowskip;
00779     int powered = limot->m_enableMotor;
00780     int limit = limot->m_currentLimit;
00781     if (powered || limit)
00782     {   // if the joint is powered, or has joint limits, add in the extra row
00783         btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
00784         btScalar *J2 = rotational ? info->m_J2angularAxis : 0;
00785         J1[srow+0] = ax1[0];
00786         J1[srow+1] = ax1[1];
00787         J1[srow+2] = ax1[2];
00788         if(rotational)
00789         {
00790             J2[srow+0] = -ax1[0];
00791             J2[srow+1] = -ax1[1];
00792             J2[srow+2] = -ax1[2];
00793         }
00794         if((!rotational))
00795         {
00796                         if (m_useOffsetForConstraintFrame)
00797                         {
00798                                 btVector3 tmpA, tmpB, relA, relB;
00799                                 // get vector from bodyB to frameB in WCS
00800                                 relB = m_calculatedTransformB.getOrigin() - transB.getOrigin();
00801                                 // get its projection to constraint axis
00802                                 btVector3 projB = ax1 * relB.dot(ax1);
00803                                 // get vector directed from bodyB to constraint axis (and orthogonal to it)
00804                                 btVector3 orthoB = relB - projB;
00805                                 // same for bodyA
00806                                 relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();
00807                                 btVector3 projA = ax1 * relA.dot(ax1);
00808                                 btVector3 orthoA = relA - projA;
00809                                 // get desired offset between frames A and B along constraint axis
00810                                 btScalar desiredOffs = limot->m_currentPosition - limot->m_currentLimitError;
00811                                 // desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis
00812                                 btVector3 totalDist = projA + ax1 * desiredOffs - projB;
00813                                 // get offset vectors relA and relB
00814                                 relA = orthoA + totalDist * m_factA;
00815                                 relB = orthoB - totalDist * m_factB;
00816                                 tmpA = relA.cross(ax1);
00817                                 tmpB = relB.cross(ax1);
00818                                 if(m_hasStaticBody && (!rotAllowed))
00819                                 {
00820                                         tmpA *= m_factA;
00821                                         tmpB *= m_factB;
00822                                 }
00823                                 int i;
00824                                 for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i];
00825                                 for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i];
00826                         } else
00827                         {
00828                                 btVector3 ltd;  // Linear Torque Decoupling vector
00829                                 btVector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin();
00830                                 ltd = c.cross(ax1);
00831                                 info->m_J1angularAxis[srow+0] = ltd[0];
00832                                 info->m_J1angularAxis[srow+1] = ltd[1];
00833                                 info->m_J1angularAxis[srow+2] = ltd[2];
00834 
00835                                 c = m_calculatedTransformB.getOrigin() - transB.getOrigin();
00836                                 ltd = -c.cross(ax1);
00837                                 info->m_J2angularAxis[srow+0] = ltd[0];
00838                                 info->m_J2angularAxis[srow+1] = ltd[1];
00839                                 info->m_J2angularAxis[srow+2] = ltd[2];
00840                         }
00841         }
00842         // if we're limited low and high simultaneously, the joint motor is
00843         // ineffective
00844         if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0;
00845         info->m_constraintError[srow] = btScalar(0.f);
00846         if (powered)
00847         {
00848                         info->cfm[srow] = limot->m_normalCFM;
00849             if(!limit)
00850             {
00851                                 btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
00852 
00853                                 btScalar mot_fact = getMotorFactor(     limot->m_currentPosition, 
00854                                                                                                         limot->m_loLimit,
00855                                                                                                         limot->m_hiLimit, 
00856                                                                                                         tag_vel, 
00857                                                                                                         info->fps * limot->m_stopERP);
00858                                 info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
00859                 info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
00860                 info->m_upperLimit[srow] = limot->m_maxMotorForce;
00861             }
00862         }
00863         if(limit)
00864         {
00865             btScalar k = info->fps * limot->m_stopERP;
00866                         if(!rotational)
00867                         {
00868                                 info->m_constraintError[srow] += k * limot->m_currentLimitError;
00869                         }
00870                         else
00871                         {
00872                                 info->m_constraintError[srow] += -k * limot->m_currentLimitError;
00873                         }
00874                         info->cfm[srow] = limot->m_stopCFM;
00875             if (limot->m_loLimit == limot->m_hiLimit)
00876             {   // limited low and high simultaneously
00877                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
00878                 info->m_upperLimit[srow] = SIMD_INFINITY;
00879             }
00880             else
00881             {
00882                 if (limit == 1)
00883                 {
00884                     info->m_lowerLimit[srow] = 0;
00885                     info->m_upperLimit[srow] = SIMD_INFINITY;
00886                 }
00887                 else
00888                 {
00889                     info->m_lowerLimit[srow] = -SIMD_INFINITY;
00890                     info->m_upperLimit[srow] = 0;
00891                 }
00892                 // deal with bounce
00893                 if (limot->m_bounce > 0)
00894                 {
00895                     // calculate joint velocity
00896                     btScalar vel;
00897                     if (rotational)
00898                     {
00899                         vel = angVelA.dot(ax1);
00900 //make sure that if no body -> angVelB == zero vec
00901 //                        if (body1)
00902                             vel -= angVelB.dot(ax1);
00903                     }
00904                     else
00905                     {
00906                         vel = linVelA.dot(ax1);
00907 //make sure that if no body -> angVelB == zero vec
00908 //                        if (body1)
00909                             vel -= linVelB.dot(ax1);
00910                     }
00911                     // only apply bounce if the velocity is incoming, and if the
00912                     // resulting c[] exceeds what we already have.
00913                     if (limit == 1)
00914                     {
00915                         if (vel < 0)
00916                         {
00917                             btScalar newc = -limot->m_bounce* vel;
00918                             if (newc > info->m_constraintError[srow]) 
00919                                                                 info->m_constraintError[srow] = newc;
00920                         }
00921                     }
00922                     else
00923                     {
00924                         if (vel > 0)
00925                         {
00926                             btScalar newc = -limot->m_bounce * vel;
00927                             if (newc < info->m_constraintError[srow]) 
00928                                                                 info->m_constraintError[srow] = newc;
00929                         }
00930                     }
00931                 }
00932             }
00933         }
00934         return 1;
00935     }
00936     else return 0;
00937 }
00938 
00939 
00940 
00941 
00942 
00943 
00946 void btGeneric6DofConstraint::setParam(int num, btScalar value, int axis)
00947 {
00948         if((axis >= 0) && (axis < 3))
00949         {
00950                 switch(num)
00951                 {
00952                         case BT_CONSTRAINT_STOP_ERP : 
00953                                 m_linearLimits.m_stopERP[axis] = value;
00954                                 m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
00955                                 break;
00956                         case BT_CONSTRAINT_STOP_CFM : 
00957                                 m_linearLimits.m_stopCFM[axis] = value;
00958                                 m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
00959                                 break;
00960                         case BT_CONSTRAINT_CFM : 
00961                                 m_linearLimits.m_normalCFM[axis] = value;
00962                                 m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
00963                                 break;
00964                         default : 
00965                                 btAssertConstrParams(0);
00966                 }
00967         }
00968         else if((axis >=3) && (axis < 6))
00969         {
00970                 switch(num)
00971                 {
00972                         case BT_CONSTRAINT_STOP_ERP : 
00973                                 m_angularLimits[axis - 3].m_stopERP = value;
00974                                 m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
00975                                 break;
00976                         case BT_CONSTRAINT_STOP_CFM : 
00977                                 m_angularLimits[axis - 3].m_stopCFM = value;
00978                                 m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
00979                                 break;
00980                         case BT_CONSTRAINT_CFM : 
00981                                 m_angularLimits[axis - 3].m_normalCFM = value;
00982                                 m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
00983                                 break;
00984                         default : 
00985                                 btAssertConstrParams(0);
00986                 }
00987         }
00988         else
00989         {
00990                 btAssertConstrParams(0);
00991         }
00992 }
00993 
00995 btScalar btGeneric6DofConstraint::getParam(int num, int axis) const 
00996 {
00997         btScalar retVal = 0;
00998         if((axis >= 0) && (axis < 3))
00999         {
01000                 switch(num)
01001                 {
01002                         case BT_CONSTRAINT_STOP_ERP : 
01003                                 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
01004                                 retVal = m_linearLimits.m_stopERP[axis];
01005                                 break;
01006                         case BT_CONSTRAINT_STOP_CFM : 
01007                                 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
01008                                 retVal = m_linearLimits.m_stopCFM[axis];
01009                                 break;
01010                         case BT_CONSTRAINT_CFM : 
01011                                 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
01012                                 retVal = m_linearLimits.m_normalCFM[axis];
01013                                 break;
01014                         default : 
01015                                 btAssertConstrParams(0);
01016                 }
01017         }
01018         else if((axis >=3) && (axis < 6))
01019         {
01020                 switch(num)
01021                 {
01022                         case BT_CONSTRAINT_STOP_ERP : 
01023                                 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
01024                                 retVal = m_angularLimits[axis - 3].m_stopERP;
01025                                 break;
01026                         case BT_CONSTRAINT_STOP_CFM : 
01027                                 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
01028                                 retVal = m_angularLimits[axis - 3].m_stopCFM;
01029                                 break;
01030                         case BT_CONSTRAINT_CFM : 
01031                                 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
01032                                 retVal = m_angularLimits[axis - 3].m_normalCFM;
01033                                 break;
01034                         default : 
01035                                 btAssertConstrParams(0);
01036                 }
01037         }
01038         else
01039         {
01040                 btAssertConstrParams(0);
01041         }
01042         return retVal;
01043 }
01044 
01045  
01046 
01047 void btGeneric6DofConstraint::setAxis(const btVector3& axis1,const btVector3& axis2)
01048 {
01049         btVector3 zAxis = axis1.normalized();
01050         btVector3 yAxis = axis2.normalized();
01051         btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
01052         
01053         btTransform frameInW;
01054         frameInW.setIdentity();
01055         frameInW.getBasis().setValue(   xAxis[0], yAxis[0], zAxis[0],   
01056                                         xAxis[1], yAxis[1], zAxis[1],
01057                                        xAxis[2], yAxis[2], zAxis[2]);
01058         
01059         // now get constraint frame in local coordinate systems
01060         m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW;
01061         m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;
01062         
01063         calculateTransforms();
01064 }