btHingeConstraint.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 #include "btHingeConstraint.h"
00018 #include "BulletDynamics/Dynamics/btRigidBody.h"
00019 #include "LinearMath/btTransformUtil.h"
00020 #include "LinearMath/btMinMax.h"
00021 #include <new>
00022 #include "btSolverBody.h"
00023 
00024 
00025 
00026 //#define HINGE_USE_OBSOLETE_SOLVER false
00027 #define HINGE_USE_OBSOLETE_SOLVER false
00028 
00029 #define HINGE_USE_FRAME_OFFSET true
00030 
00031 #ifndef __SPU__
00032 
00033 
00034 
00035 
00036 
00037 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
00038                                                                          const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA)
00039                                                                          :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
00040 #ifdef _BT_USE_CENTER_LIMIT_
00041                                                                          m_limit(),
00042 #endif
00043                                                                          m_angularOnly(false),
00044                                                                          m_enableAngularMotor(false),
00045                                                                          m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
00046                                                                          m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
00047                                                                          m_useReferenceFrameA(useReferenceFrameA),
00048                                                                          m_flags(0)
00049 {
00050         m_rbAFrame.getOrigin() = pivotInA;
00051         
00052         // since no frame is given, assume this to be zero angle and just pick rb transform axis
00053         btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);
00054 
00055         btVector3 rbAxisA2;
00056         btScalar projection = axisInA.dot(rbAxisA1);
00057         if (projection >= 1.0f - SIMD_EPSILON) {
00058                 rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2);
00059                 rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
00060         } else if (projection <= -1.0f + SIMD_EPSILON) {
00061                 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2);
00062                 rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);      
00063         } else {
00064                 rbAxisA2 = axisInA.cross(rbAxisA1);
00065                 rbAxisA1 = rbAxisA2.cross(axisInA);
00066         }
00067 
00068         m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
00069                                                                         rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
00070                                                                         rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
00071 
00072         btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
00073         btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
00074         btVector3 rbAxisB2 =  axisInB.cross(rbAxisB1);  
00075         
00076         m_rbBFrame.getOrigin() = pivotInB;
00077         m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
00078                                                                         rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
00079                                                                         rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
00080         
00081 #ifndef _BT_USE_CENTER_LIMIT_
00082         //start with free
00083         m_lowerLimit = btScalar(1.0f);
00084         m_upperLimit = btScalar(-1.0f);
00085         m_biasFactor = 0.3f;
00086         m_relaxationFactor = 1.0f;
00087         m_limitSoftness = 0.9f;
00088         m_solveLimit = false;
00089 #endif
00090         m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
00091 }
00092 
00093 
00094 
00095 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA)
00096 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),
00097 #ifdef _BT_USE_CENTER_LIMIT_
00098 m_limit(),
00099 #endif
00100 m_angularOnly(false), m_enableAngularMotor(false), 
00101 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
00102 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
00103 m_useReferenceFrameA(useReferenceFrameA),
00104 m_flags(0)
00105 {
00106 
00107         // since no frame is given, assume this to be zero angle and just pick rb transform axis
00108         // fixed axis in worldspace
00109         btVector3 rbAxisA1, rbAxisA2;
00110         btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2);
00111 
00112         m_rbAFrame.getOrigin() = pivotInA;
00113         m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
00114                                                                         rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
00115                                                                         rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
00116 
00117         btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA;
00118 
00119         btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
00120         btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
00121         btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
00122 
00123 
00124         m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA);
00125         m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
00126                                                                         rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
00127                                                                         rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
00128         
00129 #ifndef _BT_USE_CENTER_LIMIT_
00130         //start with free
00131         m_lowerLimit = btScalar(1.0f);
00132         m_upperLimit = btScalar(-1.0f);
00133         m_biasFactor = 0.3f;
00134         m_relaxationFactor = 1.0f;
00135         m_limitSoftness = 0.9f;
00136         m_solveLimit = false;
00137 #endif
00138         m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
00139 }
00140 
00141 
00142 
00143 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, 
00144                                                                      const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA)
00145 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
00146 #ifdef _BT_USE_CENTER_LIMIT_
00147 m_limit(),
00148 #endif
00149 m_angularOnly(false),
00150 m_enableAngularMotor(false),
00151 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
00152 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
00153 m_useReferenceFrameA(useReferenceFrameA),
00154 m_flags(0)
00155 {
00156 #ifndef _BT_USE_CENTER_LIMIT_
00157         //start with free
00158         m_lowerLimit = btScalar(1.0f);
00159         m_upperLimit = btScalar(-1.0f);
00160         m_biasFactor = 0.3f;
00161         m_relaxationFactor = 1.0f;
00162         m_limitSoftness = 0.9f;
00163         m_solveLimit = false;
00164 #endif
00165         m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
00166 }                       
00167 
00168 
00169 
00170 btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA)
00171 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame),
00172 #ifdef _BT_USE_CENTER_LIMIT_
00173 m_limit(),
00174 #endif
00175 m_angularOnly(false),
00176 m_enableAngularMotor(false),
00177 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
00178 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
00179 m_useReferenceFrameA(useReferenceFrameA),
00180 m_flags(0)
00181 {
00183 
00184         m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin());
00185 #ifndef _BT_USE_CENTER_LIMIT_
00186         //start with free
00187         m_lowerLimit = btScalar(1.0f);
00188         m_upperLimit = btScalar(-1.0f);
00189         m_biasFactor = 0.3f;
00190         m_relaxationFactor = 1.0f;
00191         m_limitSoftness = 0.9f;
00192         m_solveLimit = false;
00193 #endif
00194         m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
00195 }
00196 
00197 
00198 
00199 void    btHingeConstraint::buildJacobian()
00200 {
00201         if (m_useSolveConstraintObsolete)
00202         {
00203                 m_appliedImpulse = btScalar(0.);
00204                 m_accMotorImpulse = btScalar(0.);
00205 
00206                 if (!m_angularOnly)
00207                 {
00208                         btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
00209                         btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
00210                         btVector3 relPos = pivotBInW - pivotAInW;
00211 
00212                         btVector3 normal[3];
00213                         if (relPos.length2() > SIMD_EPSILON)
00214                         {
00215                                 normal[0] = relPos.normalized();
00216                         }
00217                         else
00218                         {
00219                                 normal[0].setValue(btScalar(1.0),0,0);
00220                         }
00221 
00222                         btPlaneSpace1(normal[0], normal[1], normal[2]);
00223 
00224                         for (int i=0;i<3;i++)
00225                         {
00226                                 new (&m_jac[i]) btJacobianEntry(
00227                                 m_rbA.getCenterOfMassTransform().getBasis().transpose(),
00228                                 m_rbB.getCenterOfMassTransform().getBasis().transpose(),
00229                                 pivotAInW - m_rbA.getCenterOfMassPosition(),
00230                                 pivotBInW - m_rbB.getCenterOfMassPosition(),
00231                                 normal[i],
00232                                 m_rbA.getInvInertiaDiagLocal(),
00233                                 m_rbA.getInvMass(),
00234                                 m_rbB.getInvInertiaDiagLocal(),
00235                                 m_rbB.getInvMass());
00236                         }
00237                 }
00238 
00239                 //calculate two perpendicular jointAxis, orthogonal to hingeAxis
00240                 //these two jointAxis require equal angular velocities for both bodies
00241 
00242                 //this is unused for now, it's a todo
00243                 btVector3 jointAxis0local;
00244                 btVector3 jointAxis1local;
00245                 
00246                 btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
00247 
00248                 btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
00249                 btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
00250                 btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
00251                         
00252                 new (&m_jacAng[0])      btJacobianEntry(jointAxis0,
00253                         m_rbA.getCenterOfMassTransform().getBasis().transpose(),
00254                         m_rbB.getCenterOfMassTransform().getBasis().transpose(),
00255                         m_rbA.getInvInertiaDiagLocal(),
00256                         m_rbB.getInvInertiaDiagLocal());
00257 
00258                 new (&m_jacAng[1])      btJacobianEntry(jointAxis1,
00259                         m_rbA.getCenterOfMassTransform().getBasis().transpose(),
00260                         m_rbB.getCenterOfMassTransform().getBasis().transpose(),
00261                         m_rbA.getInvInertiaDiagLocal(),
00262                         m_rbB.getInvInertiaDiagLocal());
00263 
00264                 new (&m_jacAng[2])      btJacobianEntry(hingeAxisWorld,
00265                         m_rbA.getCenterOfMassTransform().getBasis().transpose(),
00266                         m_rbB.getCenterOfMassTransform().getBasis().transpose(),
00267                         m_rbA.getInvInertiaDiagLocal(),
00268                         m_rbB.getInvInertiaDiagLocal());
00269 
00270                         // clear accumulator
00271                         m_accLimitImpulse = btScalar(0.);
00272 
00273                         // test angular limit
00274                         testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
00275 
00276                 //Compute K = J*W*J' for hinge axis
00277                 btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
00278                 m_kHinge =   1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
00279                                                          getRigidBodyB().computeAngularImpulseDenominator(axisA));
00280 
00281         }
00282 }
00283 
00284 
00285 #endif //__SPU__
00286 
00287 
00288 void btHingeConstraint::getInfo1(btConstraintInfo1* info)
00289 {
00290         if (m_useSolveConstraintObsolete)
00291         {
00292                 info->m_numConstraintRows = 0;
00293                 info->nub = 0;
00294         }
00295         else
00296         {
00297                 info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular
00298                 info->nub = 1; 
00299                 //always add the row, to avoid computation (data is not available yet)
00300                 //prepare constraint
00301                 testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
00302                 if(getSolveLimit() || getEnableAngularMotor())
00303                 {
00304                         info->m_numConstraintRows++; // limit 3rd anguar as well
00305                         info->nub--; 
00306                 }
00307 
00308         }
00309 }
00310 
00311 void btHingeConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
00312 {
00313         if (m_useSolveConstraintObsolete)
00314         {
00315                 info->m_numConstraintRows = 0;
00316                 info->nub = 0;
00317         }
00318         else
00319         {
00320                 //always add the 'limit' row, to avoid computation (data is not available yet)
00321                 info->m_numConstraintRows = 6; // Fixed 3 linear + 2 angular
00322                 info->nub = 0; 
00323         }
00324 }
00325 
00326 void btHingeConstraint::getInfo2 (btConstraintInfo2* info)
00327 {
00328         if(m_useOffsetForConstraintFrame)
00329         {
00330                 getInfo2InternalUsingFrameOffset(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity());
00331         }
00332         else
00333         {
00334                 getInfo2Internal(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity());
00335         }
00336 }
00337 
00338 
00339 void    btHingeConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
00340 {
00342         testLimit(transA,transB);
00343 
00344         getInfo2Internal(info,transA,transB,angVelA,angVelB);
00345 }
00346 
00347 
00348 void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
00349 {
00350 
00351         btAssert(!m_useSolveConstraintObsolete);
00352         int i, skip = info->rowskip;
00353         // transforms in world space
00354         btTransform trA = transA*m_rbAFrame;
00355         btTransform trB = transB*m_rbBFrame;
00356         // pivot point
00357         btVector3 pivotAInW = trA.getOrigin();
00358         btVector3 pivotBInW = trB.getOrigin();
00359 #if 0
00360         if (0)
00361         {
00362                 for (i=0;i<6;i++)
00363                 {
00364                         info->m_J1linearAxis[i*skip]=0;
00365                         info->m_J1linearAxis[i*skip+1]=0;
00366                         info->m_J1linearAxis[i*skip+2]=0;
00367 
00368                         info->m_J1angularAxis[i*skip]=0;
00369                         info->m_J1angularAxis[i*skip+1]=0;
00370                         info->m_J1angularAxis[i*skip+2]=0;
00371 
00372                         info->m_J2angularAxis[i*skip]=0;
00373                         info->m_J2angularAxis[i*skip+1]=0;
00374                         info->m_J2angularAxis[i*skip+2]=0;
00375 
00376                         info->m_constraintError[i*skip]=0.f;
00377                 }
00378         }
00379 #endif //#if 0
00380         // linear (all fixed)
00381 
00382         if (!m_angularOnly)
00383         {
00384                 info->m_J1linearAxis[0] = 1;
00385                 info->m_J1linearAxis[skip + 1] = 1;
00386                 info->m_J1linearAxis[2 * skip + 2] = 1;
00387         }       
00388 
00389 
00390 
00391 
00392         btVector3 a1 = pivotAInW - transA.getOrigin();
00393         {
00394                 btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
00395                 btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + skip);
00396                 btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * skip);
00397                 btVector3 a1neg = -a1;
00398                 a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
00399         }
00400         btVector3 a2 = pivotBInW - transB.getOrigin();
00401         {
00402                 btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
00403                 btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + skip);
00404                 btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * skip);
00405                 a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
00406         }
00407         // linear RHS
00408     btScalar k = info->fps * info->erp;
00409         if (!m_angularOnly)
00410         {
00411                 for(i = 0; i < 3; i++)
00412                 {
00413                         info->m_constraintError[i * skip] = k * (pivotBInW[i] - pivotAInW[i]);
00414                 }
00415         }
00416         // make rotations around X and Y equal
00417         // the hinge axis should be the only unconstrained
00418         // rotational axis, the angular velocity of the two bodies perpendicular to
00419         // the hinge axis should be equal. thus the constraint equations are
00420         //    p*w1 - p*w2 = 0
00421         //    q*w1 - q*w2 = 0
00422         // where p and q are unit vectors normal to the hinge axis, and w1 and w2
00423         // are the angular velocity vectors of the two bodies.
00424         // get hinge axis (Z)
00425         btVector3 ax1 = trA.getBasis().getColumn(2);
00426         // get 2 orthos to hinge axis (X, Y)
00427         btVector3 p = trA.getBasis().getColumn(0);
00428         btVector3 q = trA.getBasis().getColumn(1);
00429         // set the two hinge angular rows 
00430     int s3 = 3 * info->rowskip;
00431     int s4 = 4 * info->rowskip;
00432 
00433         info->m_J1angularAxis[s3 + 0] = p[0];
00434         info->m_J1angularAxis[s3 + 1] = p[1];
00435         info->m_J1angularAxis[s3 + 2] = p[2];
00436         info->m_J1angularAxis[s4 + 0] = q[0];
00437         info->m_J1angularAxis[s4 + 1] = q[1];
00438         info->m_J1angularAxis[s4 + 2] = q[2];
00439 
00440         info->m_J2angularAxis[s3 + 0] = -p[0];
00441         info->m_J2angularAxis[s3 + 1] = -p[1];
00442         info->m_J2angularAxis[s3 + 2] = -p[2];
00443         info->m_J2angularAxis[s4 + 0] = -q[0];
00444         info->m_J2angularAxis[s4 + 1] = -q[1];
00445         info->m_J2angularAxis[s4 + 2] = -q[2];
00446     // compute the right hand side of the constraint equation. set relative
00447     // body velocities along p and q to bring the hinge back into alignment.
00448     // if ax1,ax2 are the unit length hinge axes as computed from body1 and
00449     // body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
00450     // if `theta' is the angle between ax1 and ax2, we need an angular velocity
00451     // along u to cover angle erp*theta in one step :
00452     //   |angular_velocity| = angle/time = erp*theta / stepsize
00453     //                      = (erp*fps) * theta
00454     //    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
00455     //                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
00456     // ...as ax1 and ax2 are unit length. if theta is smallish,
00457     // theta ~= sin(theta), so
00458     //    angular_velocity  = (erp*fps) * (ax1 x ax2)
00459     // ax1 x ax2 is in the plane space of ax1, so we project the angular
00460     // velocity to p and q to find the right hand side.
00461     btVector3 ax2 = trB.getBasis().getColumn(2);
00462         btVector3 u = ax1.cross(ax2);
00463         info->m_constraintError[s3] = k * u.dot(p);
00464         info->m_constraintError[s4] = k * u.dot(q);
00465         // check angular limits
00466         int nrow = 4; // last filled row
00467         int srow;
00468         btScalar limit_err = btScalar(0.0);
00469         int limit = 0;
00470         if(getSolveLimit())
00471         {
00472 #ifdef  _BT_USE_CENTER_LIMIT_
00473         limit_err = m_limit.getCorrection() * m_referenceSign;
00474 #else
00475         limit_err = m_correction * m_referenceSign;
00476 #endif
00477         limit = (limit_err > btScalar(0.0)) ? 1 : 2;
00478 
00479         }
00480         // if the hinge has joint limits or motor, add in the extra row
00481         int powered = 0;
00482         if(getEnableAngularMotor())
00483         {
00484                 powered = 1;
00485         }
00486         if(limit || powered) 
00487         {
00488                 nrow++;
00489                 srow = nrow * info->rowskip;
00490                 info->m_J1angularAxis[srow+0] = ax1[0];
00491                 info->m_J1angularAxis[srow+1] = ax1[1];
00492                 info->m_J1angularAxis[srow+2] = ax1[2];
00493 
00494                 info->m_J2angularAxis[srow+0] = -ax1[0];
00495                 info->m_J2angularAxis[srow+1] = -ax1[1];
00496                 info->m_J2angularAxis[srow+2] = -ax1[2];
00497 
00498                 btScalar lostop = getLowerLimit();
00499                 btScalar histop = getUpperLimit();
00500                 if(limit && (lostop == histop))
00501                 {  // the joint motor is ineffective
00502                         powered = 0;
00503                 }
00504                 info->m_constraintError[srow] = btScalar(0.0f);
00505                 btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp;
00506                 if(powered)
00507                 {
00508                         if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
00509                         {
00510                                 info->cfm[srow] = m_normalCFM;
00511                         }
00512                         btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
00513                         info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
00514                         info->m_lowerLimit[srow] = - m_maxMotorImpulse;
00515                         info->m_upperLimit[srow] =   m_maxMotorImpulse;
00516                 }
00517                 if(limit)
00518                 {
00519                         k = info->fps * currERP;
00520                         info->m_constraintError[srow] += k * limit_err;
00521                         if(m_flags & BT_HINGE_FLAGS_CFM_STOP)
00522                         {
00523                                 info->cfm[srow] = m_stopCFM;
00524                         }
00525                         if(lostop == histop) 
00526                         {
00527                                 // limited low and high simultaneously
00528                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
00529                                 info->m_upperLimit[srow] = SIMD_INFINITY;
00530                         }
00531                         else if(limit == 1) 
00532                         { // low limit
00533                                 info->m_lowerLimit[srow] = 0;
00534                                 info->m_upperLimit[srow] = SIMD_INFINITY;
00535                         }
00536                         else 
00537                         { // high limit
00538                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
00539                                 info->m_upperLimit[srow] = 0;
00540                         }
00541                         // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
00542 #ifdef  _BT_USE_CENTER_LIMIT_
00543                         btScalar bounce = m_limit.getRelaxationFactor();
00544 #else
00545                         btScalar bounce = m_relaxationFactor;
00546 #endif
00547                         if(bounce > btScalar(0.0))
00548                         {
00549                                 btScalar vel = angVelA.dot(ax1);
00550                                 vel -= angVelB.dot(ax1);
00551                                 // only apply bounce if the velocity is incoming, and if the
00552                                 // resulting c[] exceeds what we already have.
00553                                 if(limit == 1)
00554                                 {       // low limit
00555                                         if(vel < 0)
00556                                         {
00557                                                 btScalar newc = -bounce * vel;
00558                                                 if(newc > info->m_constraintError[srow])
00559                                                 {
00560                                                         info->m_constraintError[srow] = newc;
00561                                                 }
00562                                         }
00563                                 }
00564                                 else
00565                                 {       // high limit - all those computations are reversed
00566                                         if(vel > 0)
00567                                         {
00568                                                 btScalar newc = -bounce * vel;
00569                                                 if(newc < info->m_constraintError[srow])
00570                                                 {
00571                                                         info->m_constraintError[srow] = newc;
00572                                                 }
00573                                         }
00574                                 }
00575                         }
00576 #ifdef  _BT_USE_CENTER_LIMIT_
00577                         info->m_constraintError[srow] *= m_limit.getBiasFactor();
00578 #else
00579                         info->m_constraintError[srow] *= m_biasFactor;
00580 #endif
00581                 } // if(limit)
00582         } // if angular limit or powered
00583 }
00584 
00585 
00586 void btHingeConstraint::setFrames(const btTransform & frameA, const btTransform & frameB)
00587 {
00588         m_rbAFrame = frameA;
00589         m_rbBFrame = frameB;
00590         buildJacobian();
00591 }
00592 
00593 
00594 void    btHingeConstraint::updateRHS(btScalar   timeStep)
00595 {
00596         (void)timeStep;
00597 
00598 }
00599 
00600 
00601 btScalar btHingeConstraint::getHingeAngle()
00602 {
00603         return getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
00604 }
00605 
00606 btScalar btHingeConstraint::getHingeAngle(const btTransform& transA,const btTransform& transB)
00607 {
00608         const btVector3 refAxis0  = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
00609         const btVector3 refAxis1  = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
00610         const btVector3 swingAxis = transB.getBasis() * m_rbBFrame.getBasis().getColumn(1);
00611 //      btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
00612         btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
00613         return m_referenceSign * angle;
00614 }
00615 
00616 
00617 
00618 void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& transB)
00619 {
00620         // Compute limit information
00621         m_hingeAngle = getHingeAngle(transA,transB);
00622 #ifdef  _BT_USE_CENTER_LIMIT_
00623         m_limit.test(m_hingeAngle);
00624 #else
00625         m_correction = btScalar(0.);
00626         m_limitSign = btScalar(0.);
00627         m_solveLimit = false;
00628         if (m_lowerLimit <= m_upperLimit)
00629         {
00630                 m_hingeAngle = btAdjustAngleToLimits(m_hingeAngle, m_lowerLimit, m_upperLimit);
00631                 if (m_hingeAngle <= m_lowerLimit)
00632                 {
00633                         m_correction = (m_lowerLimit - m_hingeAngle);
00634                         m_limitSign = 1.0f;
00635                         m_solveLimit = true;
00636                 } 
00637                 else if (m_hingeAngle >= m_upperLimit)
00638                 {
00639                         m_correction = m_upperLimit - m_hingeAngle;
00640                         m_limitSign = -1.0f;
00641                         m_solveLimit = true;
00642                 }
00643         }
00644 #endif
00645         return;
00646 }
00647 
00648 
00649 static btVector3 vHinge(0, 0, btScalar(1));
00650 
00651 void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt)
00652 {
00653         // convert target from body to constraint space
00654         btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * qAinB * m_rbAFrame.getRotation();
00655         qConstraint.normalize();
00656 
00657         // extract "pure" hinge component
00658         btVector3 vNoHinge = quatRotate(qConstraint, vHinge); vNoHinge.normalize();
00659         btQuaternion qNoHinge = shortestArcQuat(vHinge, vNoHinge);
00660         btQuaternion qHinge = qNoHinge.inverse() * qConstraint;
00661         qHinge.normalize();
00662 
00663         // compute angular target, clamped to limits
00664         btScalar targetAngle = qHinge.getAngle();
00665         if (targetAngle > SIMD_PI) // long way around. flip quat and recalculate.
00666         {
00667                 qHinge = -(qHinge);
00668                 targetAngle = qHinge.getAngle();
00669         }
00670         if (qHinge.getZ() < 0)
00671                 targetAngle = -targetAngle;
00672 
00673         setMotorTarget(targetAngle, dt);
00674 }
00675 
00676 void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt)
00677 {
00678 #ifdef  _BT_USE_CENTER_LIMIT_
00679         m_limit.fit(targetAngle);
00680 #else
00681         if (m_lowerLimit < m_upperLimit)
00682         {
00683                 if (targetAngle < m_lowerLimit)
00684                         targetAngle = m_lowerLimit;
00685                 else if (targetAngle > m_upperLimit)
00686                         targetAngle = m_upperLimit;
00687         }
00688 #endif
00689         // compute angular velocity
00690         btScalar curAngle  = getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
00691         btScalar dAngle = targetAngle - curAngle;
00692         m_motorTargetVelocity = dAngle / dt;
00693 }
00694 
00695 
00696 
00697 void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
00698 {
00699         btAssert(!m_useSolveConstraintObsolete);
00700         int i, s = info->rowskip;
00701         // transforms in world space
00702         btTransform trA = transA*m_rbAFrame;
00703         btTransform trB = transB*m_rbBFrame;
00704         // pivot point
00705 //      btVector3 pivotAInW = trA.getOrigin();
00706 //      btVector3 pivotBInW = trB.getOrigin();
00707 #if 1
00708         // difference between frames in WCS
00709         btVector3 ofs = trB.getOrigin() - trA.getOrigin();
00710         // now get weight factors depending on masses
00711         btScalar miA = getRigidBodyA().getInvMass();
00712         btScalar miB = getRigidBodyB().getInvMass();
00713         bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
00714         btScalar miS = miA + miB;
00715         btScalar factA, factB;
00716         if(miS > btScalar(0.f))
00717         {
00718                 factA = miB / miS;
00719         }
00720         else 
00721         {
00722                 factA = btScalar(0.5f);
00723         }
00724         factB = btScalar(1.0f) - factA;
00725         // get the desired direction of hinge axis
00726         // as weighted sum of Z-orthos of frameA and frameB in WCS
00727         btVector3 ax1A = trA.getBasis().getColumn(2);
00728         btVector3 ax1B = trB.getBasis().getColumn(2);
00729         btVector3 ax1 = ax1A * factA + ax1B * factB;
00730         ax1.normalize();
00731         // fill first 3 rows 
00732         // we want: velA + wA x relA == velB + wB x relB
00733         btTransform bodyA_trans = transA;
00734         btTransform bodyB_trans = transB;
00735         int s0 = 0;
00736         int s1 = s;
00737         int s2 = s * 2;
00738         int nrow = 2; // last filled row
00739         btVector3 tmpA, tmpB, relA, relB, p, q;
00740         // get vector from bodyB to frameB in WCS
00741         relB = trB.getOrigin() - bodyB_trans.getOrigin();
00742         // get its projection to hinge axis
00743         btVector3 projB = ax1 * relB.dot(ax1);
00744         // get vector directed from bodyB to hinge axis (and orthogonal to it)
00745         btVector3 orthoB = relB - projB;
00746         // same for bodyA
00747         relA = trA.getOrigin() - bodyA_trans.getOrigin();
00748         btVector3 projA = ax1 * relA.dot(ax1);
00749         btVector3 orthoA = relA - projA;
00750         btVector3 totalDist = projA - projB;
00751         // get offset vectors relA and relB
00752         relA = orthoA + totalDist * factA;
00753         relB = orthoB - totalDist * factB;
00754         // now choose average ortho to hinge axis
00755         p = orthoB * factA + orthoA * factB;
00756         btScalar len2 = p.length2();
00757         if(len2 > SIMD_EPSILON)
00758         {
00759                 p /= btSqrt(len2);
00760         }
00761         else
00762         {
00763                 p = trA.getBasis().getColumn(1);
00764         }
00765         // make one more ortho
00766         q = ax1.cross(p);
00767         // fill three rows
00768         tmpA = relA.cross(p);
00769         tmpB = relB.cross(p);
00770     for (i=0; i<3; i++) info->m_J1angularAxis[s0+i] = tmpA[i];
00771     for (i=0; i<3; i++) info->m_J2angularAxis[s0+i] = -tmpB[i];
00772         tmpA = relA.cross(q);
00773         tmpB = relB.cross(q);
00774         if(hasStaticBody && getSolveLimit())
00775         { // to make constraint between static and dynamic objects more rigid
00776                 // remove wA (or wB) from equation if angular limit is hit
00777                 tmpB *= factB;
00778                 tmpA *= factA;
00779         }
00780         for (i=0; i<3; i++) info->m_J1angularAxis[s1+i] = tmpA[i];
00781     for (i=0; i<3; i++) info->m_J2angularAxis[s1+i] = -tmpB[i];
00782         tmpA = relA.cross(ax1);
00783         tmpB = relB.cross(ax1);
00784         if(hasStaticBody)
00785         { // to make constraint between static and dynamic objects more rigid
00786                 // remove wA (or wB) from equation
00787                 tmpB *= factB;
00788                 tmpA *= factA;
00789         }
00790         for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i];
00791     for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i];
00792 
00793         btScalar k = info->fps * info->erp;
00794 
00795         if (!m_angularOnly)
00796         {
00797                 for (i=0; i<3; i++) info->m_J1linearAxis[s0+i] = p[i];
00798                 for (i=0; i<3; i++) info->m_J1linearAxis[s1+i] = q[i];
00799                 for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = ax1[i];
00800         
00801         // compute three elements of right hand side
00802         
00803                 btScalar rhs = k * p.dot(ofs);
00804                 info->m_constraintError[s0] = rhs;
00805                 rhs = k * q.dot(ofs);
00806                 info->m_constraintError[s1] = rhs;
00807                 rhs = k * ax1.dot(ofs);
00808                 info->m_constraintError[s2] = rhs;
00809         }
00810         // the hinge axis should be the only unconstrained
00811         // rotational axis, the angular velocity of the two bodies perpendicular to
00812         // the hinge axis should be equal. thus the constraint equations are
00813         //    p*w1 - p*w2 = 0
00814         //    q*w1 - q*w2 = 0
00815         // where p and q are unit vectors normal to the hinge axis, and w1 and w2
00816         // are the angular velocity vectors of the two bodies.
00817         int s3 = 3 * s;
00818         int s4 = 4 * s;
00819         info->m_J1angularAxis[s3 + 0] = p[0];
00820         info->m_J1angularAxis[s3 + 1] = p[1];
00821         info->m_J1angularAxis[s3 + 2] = p[2];
00822         info->m_J1angularAxis[s4 + 0] = q[0];
00823         info->m_J1angularAxis[s4 + 1] = q[1];
00824         info->m_J1angularAxis[s4 + 2] = q[2];
00825 
00826         info->m_J2angularAxis[s3 + 0] = -p[0];
00827         info->m_J2angularAxis[s3 + 1] = -p[1];
00828         info->m_J2angularAxis[s3 + 2] = -p[2];
00829         info->m_J2angularAxis[s4 + 0] = -q[0];
00830         info->m_J2angularAxis[s4 + 1] = -q[1];
00831         info->m_J2angularAxis[s4 + 2] = -q[2];
00832         // compute the right hand side of the constraint equation. set relative
00833         // body velocities along p and q to bring the hinge back into alignment.
00834         // if ax1A,ax1B are the unit length hinge axes as computed from bodyA and
00835         // bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2).
00836         // if "theta" is the angle between ax1 and ax2, we need an angular velocity
00837         // along u to cover angle erp*theta in one step :
00838         //   |angular_velocity| = angle/time = erp*theta / stepsize
00839         //                      = (erp*fps) * theta
00840         //    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
00841         //                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
00842         // ...as ax1 and ax2 are unit length. if theta is smallish,
00843         // theta ~= sin(theta), so
00844         //    angular_velocity  = (erp*fps) * (ax1 x ax2)
00845         // ax1 x ax2 is in the plane space of ax1, so we project the angular
00846         // velocity to p and q to find the right hand side.
00847         k = info->fps * info->erp;
00848         btVector3 u = ax1A.cross(ax1B);
00849         info->m_constraintError[s3] = k * u.dot(p);
00850         info->m_constraintError[s4] = k * u.dot(q);
00851 #endif
00852         // check angular limits
00853         nrow = 4; // last filled row
00854         int srow;
00855         btScalar limit_err = btScalar(0.0);
00856         int limit = 0;
00857         if(getSolveLimit())
00858         {
00859 #ifdef  _BT_USE_CENTER_LIMIT_
00860         limit_err = m_limit.getCorrection() * m_referenceSign;
00861 #else
00862         limit_err = m_correction * m_referenceSign;
00863 #endif
00864         limit = (limit_err > btScalar(0.0)) ? 1 : 2;
00865 
00866         }
00867         // if the hinge has joint limits or motor, add in the extra row
00868         int powered = 0;
00869         if(getEnableAngularMotor())
00870         {
00871                 powered = 1;
00872         }
00873         if(limit || powered) 
00874         {
00875                 nrow++;
00876                 srow = nrow * info->rowskip;
00877                 info->m_J1angularAxis[srow+0] = ax1[0];
00878                 info->m_J1angularAxis[srow+1] = ax1[1];
00879                 info->m_J1angularAxis[srow+2] = ax1[2];
00880 
00881                 info->m_J2angularAxis[srow+0] = -ax1[0];
00882                 info->m_J2angularAxis[srow+1] = -ax1[1];
00883                 info->m_J2angularAxis[srow+2] = -ax1[2];
00884 
00885                 btScalar lostop = getLowerLimit();
00886                 btScalar histop = getUpperLimit();
00887                 if(limit && (lostop == histop))
00888                 {  // the joint motor is ineffective
00889                         powered = 0;
00890                 }
00891                 info->m_constraintError[srow] = btScalar(0.0f);
00892                 btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp;
00893                 if(powered)
00894                 {
00895                         if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
00896                         {
00897                                 info->cfm[srow] = m_normalCFM;
00898                         }
00899                         btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
00900                         info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
00901                         info->m_lowerLimit[srow] = - m_maxMotorImpulse;
00902                         info->m_upperLimit[srow] =   m_maxMotorImpulse;
00903                 }
00904                 if(limit)
00905                 {
00906                         k = info->fps * currERP;
00907                         info->m_constraintError[srow] += k * limit_err;
00908                         if(m_flags & BT_HINGE_FLAGS_CFM_STOP)
00909                         {
00910                                 info->cfm[srow] = m_stopCFM;
00911                         }
00912                         if(lostop == histop) 
00913                         {
00914                                 // limited low and high simultaneously
00915                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
00916                                 info->m_upperLimit[srow] = SIMD_INFINITY;
00917                         }
00918                         else if(limit == 1) 
00919                         { // low limit
00920                                 info->m_lowerLimit[srow] = 0;
00921                                 info->m_upperLimit[srow] = SIMD_INFINITY;
00922                         }
00923                         else 
00924                         { // high limit
00925                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
00926                                 info->m_upperLimit[srow] = 0;
00927                         }
00928                         // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
00929 #ifdef  _BT_USE_CENTER_LIMIT_
00930                         btScalar bounce = m_limit.getRelaxationFactor();
00931 #else
00932                         btScalar bounce = m_relaxationFactor;
00933 #endif
00934                         if(bounce > btScalar(0.0))
00935                         {
00936                                 btScalar vel = angVelA.dot(ax1);
00937                                 vel -= angVelB.dot(ax1);
00938                                 // only apply bounce if the velocity is incoming, and if the
00939                                 // resulting c[] exceeds what we already have.
00940                                 if(limit == 1)
00941                                 {       // low limit
00942                                         if(vel < 0)
00943                                         {
00944                                                 btScalar newc = -bounce * vel;
00945                                                 if(newc > info->m_constraintError[srow])
00946                                                 {
00947                                                         info->m_constraintError[srow] = newc;
00948                                                 }
00949                                         }
00950                                 }
00951                                 else
00952                                 {       // high limit - all those computations are reversed
00953                                         if(vel > 0)
00954                                         {
00955                                                 btScalar newc = -bounce * vel;
00956                                                 if(newc < info->m_constraintError[srow])
00957                                                 {
00958                                                         info->m_constraintError[srow] = newc;
00959                                                 }
00960                                         }
00961                                 }
00962                         }
00963 #ifdef  _BT_USE_CENTER_LIMIT_
00964                         info->m_constraintError[srow] *= m_limit.getBiasFactor();
00965 #else
00966                         info->m_constraintError[srow] *= m_biasFactor;
00967 #endif
00968                 } // if(limit)
00969         } // if angular limit or powered
00970 }
00971 
00972 
00975 void btHingeConstraint::setParam(int num, btScalar value, int axis)
00976 {
00977         if((axis == -1) || (axis == 5))
00978         {
00979                 switch(num)
00980                 {       
00981                         case BT_CONSTRAINT_STOP_ERP :
00982                                 m_stopERP = value;
00983                                 m_flags |= BT_HINGE_FLAGS_ERP_STOP;
00984                                 break;
00985                         case BT_CONSTRAINT_STOP_CFM :
00986                                 m_stopCFM = value;
00987                                 m_flags |= BT_HINGE_FLAGS_CFM_STOP;
00988                                 break;
00989                         case BT_CONSTRAINT_CFM :
00990                                 m_normalCFM = value;
00991                                 m_flags |= BT_HINGE_FLAGS_CFM_NORM;
00992                                 break;
00993                         default : 
00994                                 btAssertConstrParams(0);
00995                 }
00996         }
00997         else
00998         {
00999                 btAssertConstrParams(0);
01000         }
01001 }
01002 
01004 btScalar btHingeConstraint::getParam(int num, int axis) const 
01005 {
01006         btScalar retVal = 0;
01007         if((axis == -1) || (axis == 5))
01008         {
01009                 switch(num)
01010                 {       
01011                         case BT_CONSTRAINT_STOP_ERP :
01012                                 btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_STOP);
01013                                 retVal = m_stopERP;
01014                                 break;
01015                         case BT_CONSTRAINT_STOP_CFM :
01016                                 btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_STOP);
01017                                 retVal = m_stopCFM;
01018                                 break;
01019                         case BT_CONSTRAINT_CFM :
01020                                 btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_NORM);
01021                                 retVal = m_normalCFM;
01022                                 break;
01023                         default : 
01024                                 btAssertConstrParams(0);
01025                 }
01026         }
01027         else
01028         {
01029                 btAssertConstrParams(0);
01030         }
01031         return retVal;
01032 }
01033 
01034