Blender V2.61 - r43446

btConeTwistConstraint.cpp

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 btConeTwistConstraint is Copyright (c) 2007 Starbreeze Studios
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 Written by: Marcus Hennix
00016 */
00017 
00018 
00019 #include "btConeTwistConstraint.h"
00020 #include "BulletDynamics/Dynamics/btRigidBody.h"
00021 #include "LinearMath/btTransformUtil.h"
00022 #include "LinearMath/btMinMax.h"
00023 #include <new>
00024 
00025 
00026 
00027 //#define CONETWIST_USE_OBSOLETE_SOLVER true
00028 #define CONETWIST_USE_OBSOLETE_SOLVER false
00029 #define CONETWIST_DEF_FIX_THRESH btScalar(.05f)
00030 
00031 
00032 SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis, const btMatrix3x3& invInertiaWorld)
00033 {
00034     btVector3 vec = axis * invInertiaWorld;
00035     return axis.dot(vec);
00036 }
00037 
00038 
00039 
00040 
00041 btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB, 
00042                                              const btTransform& rbAFrame,const btTransform& rbBFrame)
00043                                              :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
00044                                              m_angularOnly(false),
00045                                              m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
00046 {
00047     init();
00048 }
00049 
00050 btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame)
00051                                             :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame),
00052                                              m_angularOnly(false),
00053                                              m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
00054 {
00055     m_rbBFrame = m_rbAFrame;
00056     init(); 
00057 }
00058 
00059 
00060 void btConeTwistConstraint::init()
00061 {
00062     m_angularOnly = false;
00063     m_solveTwistLimit = false;
00064     m_solveSwingLimit = false;
00065     m_bMotorEnabled = false;
00066     m_maxMotorImpulse = btScalar(-1);
00067 
00068     setLimit(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
00069     m_damping = btScalar(0.01);
00070     m_fixThresh = CONETWIST_DEF_FIX_THRESH;
00071     m_flags = 0;
00072     m_linCFM = btScalar(0.f);
00073     m_linERP = btScalar(0.7f);
00074     m_angCFM = btScalar(0.f);
00075 }
00076 
00077 
00078 void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
00079 {
00080     if (m_useSolveConstraintObsolete)
00081     {
00082         info->m_numConstraintRows = 0;
00083         info->nub = 0;
00084     } 
00085     else
00086     {
00087         info->m_numConstraintRows = 3;
00088         info->nub = 3;
00089         calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
00090         if(m_solveSwingLimit)
00091         {
00092             info->m_numConstraintRows++;
00093             info->nub--;
00094             if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
00095             {
00096                 info->m_numConstraintRows++;
00097                 info->nub--;
00098             }
00099         }
00100         if(m_solveTwistLimit)
00101         {
00102             info->m_numConstraintRows++;
00103             info->nub--;
00104         }
00105     }
00106 }
00107 
00108 void btConeTwistConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
00109 {
00110     //always reserve 6 rows: object transform is not available on SPU
00111     info->m_numConstraintRows = 6;
00112     info->nub = 0;
00113         
00114 }
00115     
00116 
00117 void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
00118 {
00119     getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
00120 }
00121 
00122 void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB)
00123 {
00124     calcAngleInfo2(transA,transB,invInertiaWorldA,invInertiaWorldB);
00125     
00126     btAssert(!m_useSolveConstraintObsolete);
00127     // set jacobian
00128     info->m_J1linearAxis[0] = 1;
00129     info->m_J1linearAxis[info->rowskip+1] = 1;
00130     info->m_J1linearAxis[2*info->rowskip+2] = 1;
00131     btVector3 a1 = transA.getBasis() * m_rbAFrame.getOrigin();
00132     {
00133         btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
00134         btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
00135         btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
00136         btVector3 a1neg = -a1;
00137         a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
00138     }
00139     btVector3 a2 = transB.getBasis() * m_rbBFrame.getOrigin();
00140     {
00141         btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
00142         btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
00143         btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
00144         a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
00145     }
00146     // set right hand side
00147     btScalar linERP = (m_flags & BT_CONETWIST_FLAGS_LIN_ERP) ? m_linERP : info->erp;
00148     btScalar k = info->fps * linERP;
00149     int j;
00150     for (j=0; j<3; j++)
00151     {
00152         info->m_constraintError[j*info->rowskip] = k * (a2[j] + transB.getOrigin()[j] - a1[j] - transA.getOrigin()[j]);
00153         info->m_lowerLimit[j*info->rowskip] = -SIMD_INFINITY;
00154         info->m_upperLimit[j*info->rowskip] = SIMD_INFINITY;
00155         if(m_flags & BT_CONETWIST_FLAGS_LIN_CFM)
00156         {
00157             info->cfm[j*info->rowskip] = m_linCFM;
00158         }
00159     }
00160     int row = 3;
00161     int srow = row * info->rowskip;
00162     btVector3 ax1;
00163     // angular limits
00164     if(m_solveSwingLimit)
00165     {
00166         btScalar *J1 = info->m_J1angularAxis;
00167         btScalar *J2 = info->m_J2angularAxis;
00168         if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
00169         {
00170             btTransform trA = transA*m_rbAFrame;
00171             btVector3 p = trA.getBasis().getColumn(1);
00172             btVector3 q = trA.getBasis().getColumn(2);
00173             int srow1 = srow + info->rowskip;
00174             J1[srow+0] = p[0];
00175             J1[srow+1] = p[1];
00176             J1[srow+2] = p[2];
00177             J1[srow1+0] = q[0];
00178             J1[srow1+1] = q[1];
00179             J1[srow1+2] = q[2];
00180             J2[srow+0] = -p[0];
00181             J2[srow+1] = -p[1];
00182             J2[srow+2] = -p[2];
00183             J2[srow1+0] = -q[0];
00184             J2[srow1+1] = -q[1];
00185             J2[srow1+2] = -q[2];
00186             btScalar fact = info->fps * m_relaxationFactor;
00187             info->m_constraintError[srow] =   fact * m_swingAxis.dot(p);
00188             info->m_constraintError[srow1] =  fact * m_swingAxis.dot(q);
00189             info->m_lowerLimit[srow] = -SIMD_INFINITY;
00190             info->m_upperLimit[srow] = SIMD_INFINITY;
00191             info->m_lowerLimit[srow1] = -SIMD_INFINITY;
00192             info->m_upperLimit[srow1] = SIMD_INFINITY;
00193             srow = srow1 + info->rowskip;
00194         }
00195         else
00196         {
00197             ax1 = m_swingAxis * m_relaxationFactor * m_relaxationFactor;
00198             J1[srow+0] = ax1[0];
00199             J1[srow+1] = ax1[1];
00200             J1[srow+2] = ax1[2];
00201             J2[srow+0] = -ax1[0];
00202             J2[srow+1] = -ax1[1];
00203             J2[srow+2] = -ax1[2];
00204             btScalar k = info->fps * m_biasFactor;
00205 
00206             info->m_constraintError[srow] = k * m_swingCorrection;
00207             if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
00208             {
00209                 info->cfm[srow] = m_angCFM;
00210             }
00211             // m_swingCorrection is always positive or 0
00212             info->m_lowerLimit[srow] = 0;
00213             info->m_upperLimit[srow] = SIMD_INFINITY;
00214             srow += info->rowskip;
00215         }
00216     }
00217     if(m_solveTwistLimit)
00218     {
00219         ax1 = m_twistAxis * m_relaxationFactor * m_relaxationFactor;
00220         btScalar *J1 = info->m_J1angularAxis;
00221         btScalar *J2 = info->m_J2angularAxis;
00222         J1[srow+0] = ax1[0];
00223         J1[srow+1] = ax1[1];
00224         J1[srow+2] = ax1[2];
00225         J2[srow+0] = -ax1[0];
00226         J2[srow+1] = -ax1[1];
00227         J2[srow+2] = -ax1[2];
00228         btScalar k = info->fps * m_biasFactor;
00229         info->m_constraintError[srow] = k * m_twistCorrection;
00230         if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
00231         {
00232             info->cfm[srow] = m_angCFM;
00233         }
00234         if(m_twistSpan > 0.0f)
00235         {
00236 
00237             if(m_twistCorrection > 0.0f)
00238             {
00239                 info->m_lowerLimit[srow] = 0;
00240                 info->m_upperLimit[srow] = SIMD_INFINITY;
00241             } 
00242             else
00243             {
00244                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
00245                 info->m_upperLimit[srow] = 0;
00246             } 
00247         }
00248         else
00249         {
00250             info->m_lowerLimit[srow] = -SIMD_INFINITY;
00251             info->m_upperLimit[srow] = SIMD_INFINITY;
00252         }
00253         srow += info->rowskip;
00254     }
00255 }
00256     
00257 
00258 
00259 void    btConeTwistConstraint::buildJacobian()
00260 {
00261     if (m_useSolveConstraintObsolete)
00262     {
00263         m_appliedImpulse = btScalar(0.);
00264         m_accTwistLimitImpulse = btScalar(0.);
00265         m_accSwingLimitImpulse = btScalar(0.);
00266         m_accMotorImpulse = btVector3(0.,0.,0.);
00267 
00268         if (!m_angularOnly)
00269         {
00270             btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
00271             btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
00272             btVector3 relPos = pivotBInW - pivotAInW;
00273 
00274             btVector3 normal[3];
00275             if (relPos.length2() > SIMD_EPSILON)
00276             {
00277                 normal[0] = relPos.normalized();
00278             }
00279             else
00280             {
00281                 normal[0].setValue(btScalar(1.0),0,0);
00282             }
00283 
00284             btPlaneSpace1(normal[0], normal[1], normal[2]);
00285 
00286             for (int i=0;i<3;i++)
00287             {
00288                 new (&m_jac[i]) btJacobianEntry(
00289                 m_rbA.getCenterOfMassTransform().getBasis().transpose(),
00290                 m_rbB.getCenterOfMassTransform().getBasis().transpose(),
00291                 pivotAInW - m_rbA.getCenterOfMassPosition(),
00292                 pivotBInW - m_rbB.getCenterOfMassPosition(),
00293                 normal[i],
00294                 m_rbA.getInvInertiaDiagLocal(),
00295                 m_rbA.getInvMass(),
00296                 m_rbB.getInvInertiaDiagLocal(),
00297                 m_rbB.getInvMass());
00298             }
00299         }
00300 
00301         calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
00302     }
00303 }
00304 
00305 
00306 
00307 void    btConeTwistConstraint::solveConstraintObsolete(btRigidBody& bodyA,btRigidBody& bodyB,btScalar   timeStep)
00308 {
00309     #ifndef __SPU__
00310     if (m_useSolveConstraintObsolete)
00311     {
00312         btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
00313         btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
00314 
00315         btScalar tau = btScalar(0.3);
00316 
00317         //linear part
00318         if (!m_angularOnly)
00319         {
00320             btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
00321             btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
00322 
00323             btVector3 vel1;
00324             bodyA.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1);
00325             btVector3 vel2;
00326             bodyB.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2);
00327             btVector3 vel = vel1 - vel2;
00328 
00329             for (int i=0;i<3;i++)
00330             {       
00331                 const btVector3& normal = m_jac[i].m_linearJointAxis;
00332                 btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
00333 
00334                 btScalar rel_vel;
00335                 rel_vel = normal.dot(vel);
00336                 //positional error (zeroth order error)
00337                 btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
00338                 btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
00339                 m_appliedImpulse += impulse;
00340                 
00341                 btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
00342                 btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
00343                 bodyA.internalApplyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
00344                 bodyB.internalApplyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
00345         
00346             }
00347         }
00348 
00349         // apply motor
00350         if (m_bMotorEnabled)
00351         {
00352             // compute current and predicted transforms
00353             btTransform trACur = m_rbA.getCenterOfMassTransform();
00354             btTransform trBCur = m_rbB.getCenterOfMassTransform();
00355             btVector3 omegaA; bodyA.internalGetAngularVelocity(omegaA);
00356             btVector3 omegaB; bodyB.internalGetAngularVelocity(omegaB);
00357             btTransform trAPred; trAPred.setIdentity(); 
00358             btVector3 zerovec(0,0,0);
00359             btTransformUtil::integrateTransform(
00360                 trACur, zerovec, omegaA, timeStep, trAPred);
00361             btTransform trBPred; trBPred.setIdentity(); 
00362             btTransformUtil::integrateTransform(
00363                 trBCur, zerovec, omegaB, timeStep, trBPred);
00364 
00365             // compute desired transforms in world
00366             btTransform trPose(m_qTarget);
00367             btTransform trABDes = m_rbBFrame * trPose * m_rbAFrame.inverse();
00368             btTransform trADes = trBPred * trABDes;
00369             btTransform trBDes = trAPred * trABDes.inverse();
00370 
00371             // compute desired omegas in world
00372             btVector3 omegaADes, omegaBDes;
00373             
00374             btTransformUtil::calculateVelocity(trACur, trADes, timeStep, zerovec, omegaADes);
00375             btTransformUtil::calculateVelocity(trBCur, trBDes, timeStep, zerovec, omegaBDes);
00376 
00377             // compute delta omegas
00378             btVector3 dOmegaA = omegaADes - omegaA;
00379             btVector3 dOmegaB = omegaBDes - omegaB;
00380 
00381             // compute weighted avg axis of dOmega (weighting based on inertias)
00382             btVector3 axisA, axisB;
00383             btScalar kAxisAInv = 0, kAxisBInv = 0;
00384 
00385             if (dOmegaA.length2() > SIMD_EPSILON)
00386             {
00387                 axisA = dOmegaA.normalized();
00388                 kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(axisA);
00389             }
00390 
00391             if (dOmegaB.length2() > SIMD_EPSILON)
00392             {
00393                 axisB = dOmegaB.normalized();
00394                 kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(axisB);
00395             }
00396 
00397             btVector3 avgAxis = kAxisAInv * axisA + kAxisBInv * axisB;
00398 
00399             static bool bDoTorque = true;
00400             if (bDoTorque && avgAxis.length2() > SIMD_EPSILON)
00401             {
00402                 avgAxis.normalize();
00403                 kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(avgAxis);
00404                 kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(avgAxis);
00405                 btScalar kInvCombined = kAxisAInv + kAxisBInv;
00406 
00407                 btVector3 impulse = (kAxisAInv * dOmegaA - kAxisBInv * dOmegaB) /
00408                                     (kInvCombined * kInvCombined);
00409 
00410                 if (m_maxMotorImpulse >= 0)
00411                 {
00412                     btScalar fMaxImpulse = m_maxMotorImpulse;
00413                     if (m_bNormalizedMotorStrength)
00414                         fMaxImpulse = fMaxImpulse/kAxisAInv;
00415 
00416                     btVector3 newUnclampedAccImpulse = m_accMotorImpulse + impulse;
00417                     btScalar  newUnclampedMag = newUnclampedAccImpulse.length();
00418                     if (newUnclampedMag > fMaxImpulse)
00419                     {
00420                         newUnclampedAccImpulse.normalize();
00421                         newUnclampedAccImpulse *= fMaxImpulse;
00422                         impulse = newUnclampedAccImpulse - m_accMotorImpulse;
00423                     }
00424                     m_accMotorImpulse += impulse;
00425                 }
00426 
00427                 btScalar  impulseMag  = impulse.length();
00428                 btVector3 impulseAxis =  impulse / impulseMag;
00429 
00430                 bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
00431                 bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
00432 
00433             }
00434         }
00435         else if (m_damping > SIMD_EPSILON) // no motor: do a little damping
00436         {
00437             btVector3 angVelA; bodyA.internalGetAngularVelocity(angVelA);
00438             btVector3 angVelB; bodyB.internalGetAngularVelocity(angVelB);
00439             btVector3 relVel = angVelB - angVelA;
00440             if (relVel.length2() > SIMD_EPSILON)
00441             {
00442                 btVector3 relVelAxis = relVel.normalized();
00443                 btScalar m_kDamping =  btScalar(1.) /
00444                     (getRigidBodyA().computeAngularImpulseDenominator(relVelAxis) +
00445                      getRigidBodyB().computeAngularImpulseDenominator(relVelAxis));
00446                 btVector3 impulse = m_damping * m_kDamping * relVel;
00447 
00448                 btScalar  impulseMag  = impulse.length();
00449                 btVector3 impulseAxis = impulse / impulseMag;
00450                 bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
00451                 bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
00452             }
00453         }
00454 
00455         // joint limits
00456         {
00458             btVector3 angVelA;
00459             bodyA.internalGetAngularVelocity(angVelA);
00460             btVector3 angVelB;
00461             bodyB.internalGetAngularVelocity(angVelB);
00462 
00463             // solve swing limit
00464             if (m_solveSwingLimit)
00465             {
00466                 btScalar amplitude = m_swingLimitRatio * m_swingCorrection*m_biasFactor/timeStep;
00467                 btScalar relSwingVel = (angVelB - angVelA).dot(m_swingAxis);
00468                 if (relSwingVel > 0)
00469                     amplitude += m_swingLimitRatio * relSwingVel * m_relaxationFactor;
00470                 btScalar impulseMag = amplitude * m_kSwing;
00471 
00472                 // Clamp the accumulated impulse
00473                 btScalar temp = m_accSwingLimitImpulse;
00474                 m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) );
00475                 impulseMag = m_accSwingLimitImpulse - temp;
00476 
00477                 btVector3 impulse = m_swingAxis * impulseMag;
00478 
00479                 // don't let cone response affect twist
00480                 // (this can happen since body A's twist doesn't match body B's AND we use an elliptical cone limit)
00481                 {
00482                     btVector3 impulseTwistCouple = impulse.dot(m_twistAxisA) * m_twistAxisA;
00483                     btVector3 impulseNoTwistCouple = impulse - impulseTwistCouple;
00484                     impulse = impulseNoTwistCouple;
00485                 }
00486 
00487                 impulseMag = impulse.length();
00488                 btVector3 noTwistSwingAxis = impulse / impulseMag;
00489 
00490                 bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag);
00491                 bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag);
00492             }
00493 
00494 
00495             // solve twist limit
00496             if (m_solveTwistLimit)
00497             {
00498                 btScalar amplitude = m_twistLimitRatio * m_twistCorrection*m_biasFactor/timeStep;
00499                 btScalar relTwistVel = (angVelB - angVelA).dot( m_twistAxis );
00500                 if (relTwistVel > 0) // only damp when moving towards limit (m_twistAxis flipping is important)
00501                     amplitude += m_twistLimitRatio * relTwistVel * m_relaxationFactor;
00502                 btScalar impulseMag = amplitude * m_kTwist;
00503 
00504                 // Clamp the accumulated impulse
00505                 btScalar temp = m_accTwistLimitImpulse;
00506                 m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) );
00507                 impulseMag = m_accTwistLimitImpulse - temp;
00508 
00509                 btVector3 impulse = m_twistAxis * impulseMag;
00510 
00511                 bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag);
00512                 bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag);
00513             }       
00514         }
00515     }
00516 #else
00517 btAssert(0);
00518 #endif //__SPU__
00519 }
00520 
00521 
00522 
00523 
00524 void    btConeTwistConstraint::updateRHS(btScalar   timeStep)
00525 {
00526     (void)timeStep;
00527 
00528 }
00529 
00530 
00531 #ifndef __SPU__
00532 void btConeTwistConstraint::calcAngleInfo()
00533 {
00534     m_swingCorrection = btScalar(0.);
00535     m_twistLimitSign = btScalar(0.);
00536     m_solveTwistLimit = false;
00537     m_solveSwingLimit = false;
00538 
00539     btVector3 b1Axis1,b1Axis2,b1Axis3;
00540     btVector3 b2Axis1,b2Axis2;
00541 
00542     b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0);
00543     b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0);
00544 
00545     btScalar swing1=btScalar(0.),swing2 = btScalar(0.);
00546 
00547     btScalar swx=btScalar(0.),swy = btScalar(0.);
00548     btScalar thresh = btScalar(10.);
00549     btScalar fact;
00550 
00551     // Get Frame into world space
00552     if (m_swingSpan1 >= btScalar(0.05f))
00553     {
00554         b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1);
00555         swx = b2Axis1.dot(b1Axis1);
00556         swy = b2Axis1.dot(b1Axis2);
00557         swing1  = btAtan2Fast(swy, swx);
00558         fact = (swy*swy + swx*swx) * thresh * thresh;
00559         fact = fact / (fact + btScalar(1.0));
00560         swing1 *= fact; 
00561     }
00562 
00563     if (m_swingSpan2 >= btScalar(0.05f))
00564     {
00565         b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);         
00566         swx = b2Axis1.dot(b1Axis1);
00567         swy = b2Axis1.dot(b1Axis3);
00568         swing2  = btAtan2Fast(swy, swx);
00569         fact = (swy*swy + swx*swx) * thresh * thresh;
00570         fact = fact / (fact + btScalar(1.0));
00571         swing2 *= fact; 
00572     }
00573 
00574     btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1);     
00575     btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2); 
00576     btScalar EllipseAngle = btFabs(swing1*swing1)* RMaxAngle1Sq + btFabs(swing2*swing2) * RMaxAngle2Sq;
00577 
00578     if (EllipseAngle > 1.0f)
00579     {
00580         m_swingCorrection = EllipseAngle-1.0f;
00581         m_solveSwingLimit = true;
00582         // Calculate necessary axis & factors
00583         m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3));
00584         m_swingAxis.normalize();
00585         btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
00586         m_swingAxis *= swingAxisSign;
00587     }
00588 
00589     // Twist limits
00590     if (m_twistSpan >= btScalar(0.))
00591     {
00592         btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1);
00593         btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1);
00594         btVector3 TwistRef = quatRotate(rotationArc,b2Axis2); 
00595         btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) );
00596         m_twistAngle = twist;
00597 
00598 //      btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
00599         btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? btScalar(1.0f) : btScalar(0.);
00600         if (twist <= -m_twistSpan*lockedFreeFactor)
00601         {
00602             m_twistCorrection = -(twist + m_twistSpan);
00603             m_solveTwistLimit = true;
00604             m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
00605             m_twistAxis.normalize();
00606             m_twistAxis *= -1.0f;
00607         }
00608         else if (twist >  m_twistSpan*lockedFreeFactor)
00609         {
00610             m_twistCorrection = (twist - m_twistSpan);
00611             m_solveTwistLimit = true;
00612             m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
00613             m_twistAxis.normalize();
00614         }
00615     }
00616 }
00617 #endif //__SPU__
00618 
00619 static btVector3 vTwist(1,0,0); // twist axis in constraint's space
00620 
00621 
00622 
00623 void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB)
00624 {
00625     m_swingCorrection = btScalar(0.);
00626     m_twistLimitSign = btScalar(0.);
00627     m_solveTwistLimit = false;
00628     m_solveSwingLimit = false;
00629     // compute rotation of A wrt B (in constraint space)
00630     if (m_bMotorEnabled && (!m_useSolveConstraintObsolete))
00631     {   // it is assumed that setMotorTarget() was alredy called 
00632         // and motor target m_qTarget is within constraint limits
00633         // TODO : split rotation to pure swing and pure twist
00634         // compute desired transforms in world
00635         btTransform trPose(m_qTarget);
00636         btTransform trA = transA * m_rbAFrame;
00637         btTransform trB = transB * m_rbBFrame;
00638         btTransform trDeltaAB = trB * trPose * trA.inverse();
00639         btQuaternion qDeltaAB = trDeltaAB.getRotation();
00640         btVector3 swingAxis =   btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z());
00641         m_swingAxis = swingAxis;
00642         m_swingAxis.normalize();
00643         m_swingCorrection = qDeltaAB.getAngle();
00644         if(!btFuzzyZero(m_swingCorrection))
00645         {
00646             m_solveSwingLimit = true;
00647         }
00648         return;
00649     }
00650 
00651 
00652     {
00653         // compute rotation of A wrt B (in constraint space)
00654         btQuaternion qA = transA.getRotation() * m_rbAFrame.getRotation();
00655         btQuaternion qB = transB.getRotation() * m_rbBFrame.getRotation();
00656         btQuaternion qAB = qB.inverse() * qA;
00657         // split rotation into cone and twist
00658         // (all this is done from B's perspective. Maybe I should be averaging axes...)
00659         btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize();
00660         btQuaternion qABCone  = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize();
00661         btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize();
00662 
00663         if (m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh)
00664         {
00665             btScalar swingAngle, swingLimit = 0; btVector3 swingAxis;
00666             computeConeLimitInfo(qABCone, swingAngle, swingAxis, swingLimit);
00667 
00668             if (swingAngle > swingLimit * m_limitSoftness)
00669             {
00670                 m_solveSwingLimit = true;
00671 
00672                 // compute limit ratio: 0->1, where
00673                 // 0 == beginning of soft limit
00674                 // 1 == hard/real limit
00675                 m_swingLimitRatio = 1.f;
00676                 if (swingAngle < swingLimit && m_limitSoftness < 1.f - SIMD_EPSILON)
00677                 {
00678                     m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness)/
00679                                         (swingLimit - swingLimit * m_limitSoftness);
00680                 }               
00681 
00682                 // swing correction tries to get back to soft limit
00683                 m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness);
00684 
00685                 // adjustment of swing axis (based on ellipse normal)
00686                 adjustSwingAxisToUseEllipseNormal(swingAxis);
00687 
00688                 // Calculate necessary axis & factors       
00689                 m_swingAxis = quatRotate(qB, -swingAxis);
00690 
00691                 m_twistAxisA.setValue(0,0,0);
00692 
00693                 m_kSwing =  btScalar(1.) /
00694                     (computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldA) +
00695                      computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldB));
00696             }
00697         }
00698         else
00699         {
00700             // you haven't set any limits;
00701             // or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?)
00702             // anyway, we have either hinge or fixed joint
00703             btVector3 ivA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
00704             btVector3 jvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
00705             btVector3 kvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(2);
00706             btVector3 ivB = transB.getBasis() * m_rbBFrame.getBasis().getColumn(0);
00707             btVector3 target;
00708             btScalar x = ivB.dot(ivA);
00709             btScalar y = ivB.dot(jvA);
00710             btScalar z = ivB.dot(kvA);
00711             if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
00712             { // fixed. We'll need to add one more row to constraint
00713                 if((!btFuzzyZero(y)) || (!(btFuzzyZero(z))))
00714                 {
00715                     m_solveSwingLimit = true;
00716                     m_swingAxis = -ivB.cross(ivA);
00717                 }
00718             }
00719             else
00720             {
00721                 if(m_swingSpan1 < m_fixThresh)
00722                 { // hinge around Y axis
00723                     if(!(btFuzzyZero(y)))
00724                     {
00725                         m_solveSwingLimit = true;
00726                         if(m_swingSpan2 >= m_fixThresh)
00727                         {
00728                             y = btScalar(0.f);
00729                             btScalar span2 = btAtan2(z, x);
00730                             if(span2 > m_swingSpan2)
00731                             {
00732                                 x = btCos(m_swingSpan2);
00733                                 z = btSin(m_swingSpan2);
00734                             }
00735                             else if(span2 < -m_swingSpan2)
00736                             {
00737                                 x =  btCos(m_swingSpan2);
00738                                 z = -btSin(m_swingSpan2);
00739                             }
00740                         }
00741                     }
00742                 }
00743                 else
00744                 { // hinge around Z axis
00745                     if(!btFuzzyZero(z))
00746                     {
00747                         m_solveSwingLimit = true;
00748                         if(m_swingSpan1 >= m_fixThresh)
00749                         {
00750                             z = btScalar(0.f);
00751                             btScalar span1 = btAtan2(y, x);
00752                             if(span1 > m_swingSpan1)
00753                             {
00754                                 x = btCos(m_swingSpan1);
00755                                 y = btSin(m_swingSpan1);
00756                             }
00757                             else if(span1 < -m_swingSpan1)
00758                             {
00759                                 x =  btCos(m_swingSpan1);
00760                                 y = -btSin(m_swingSpan1);
00761                             }
00762                         }
00763                     }
00764                 }
00765                 target[0] = x * ivA[0] + y * jvA[0] + z * kvA[0];
00766                 target[1] = x * ivA[1] + y * jvA[1] + z * kvA[1];
00767                 target[2] = x * ivA[2] + y * jvA[2] + z * kvA[2];
00768                 target.normalize();
00769                 m_swingAxis = -ivB.cross(target);
00770                 m_swingCorrection = m_swingAxis.length();
00771                 m_swingAxis.normalize();
00772             }
00773         }
00774 
00775         if (m_twistSpan >= btScalar(0.f))
00776         {
00777             btVector3 twistAxis;
00778             computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis);
00779 
00780             if (m_twistAngle > m_twistSpan*m_limitSoftness)
00781             {
00782                 m_solveTwistLimit = true;
00783 
00784                 m_twistLimitRatio = 1.f;
00785                 if (m_twistAngle < m_twistSpan && m_limitSoftness < 1.f - SIMD_EPSILON)
00786                 {
00787                     m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness)/
00788                                         (m_twistSpan  - m_twistSpan * m_limitSoftness);
00789                 }
00790 
00791                 // twist correction tries to get back to soft limit
00792                 m_twistCorrection = m_twistAngle - (m_twistSpan * m_limitSoftness);
00793 
00794                 m_twistAxis = quatRotate(qB, -twistAxis);
00795 
00796                 m_kTwist = btScalar(1.) /
00797                     (computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldA) +
00798                      computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldB));
00799             }
00800 
00801             if (m_solveSwingLimit)
00802                 m_twistAxisA = quatRotate(qA, -twistAxis);
00803         }
00804         else
00805         {
00806             m_twistAngle = btScalar(0.f);
00807         }
00808     }
00809 }
00810 
00811 
00812 
00813 // given a cone rotation in constraint space, (pre: twist must already be removed)
00814 // this method computes its corresponding swing angle and axis.
00815 // more interestingly, it computes the cone/swing limit (angle) for this cone "pose".
00816 void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
00817                                                  btScalar& swingAngle, // out
00818                                                  btVector3& vSwingAxis, // out
00819                                                  btScalar& swingLimit) // out
00820 {
00821     swingAngle = qCone.getAngle();
00822     if (swingAngle > SIMD_EPSILON)
00823     {
00824         vSwingAxis = btVector3(qCone.x(), qCone.y(), qCone.z());
00825         vSwingAxis.normalize();
00826         if (fabs(vSwingAxis.x()) > SIMD_EPSILON)
00827         {
00828             // non-zero twist?! this should never happen.
00829             int wtf = 0; wtf = wtf;
00830         }
00831 
00832         // Compute limit for given swing. tricky:
00833         // Given a swing axis, we're looking for the intersection with the bounding cone ellipse.
00834         // (Since we're dealing with angles, this ellipse is embedded on the surface of a sphere.)
00835 
00836         // For starters, compute the direction from center to surface of ellipse.
00837         // This is just the perpendicular (ie. rotate 2D vector by PI/2) of the swing axis.
00838         // (vSwingAxis is the cone rotation (in z,y); change vars and rotate to (x,y) coords.)
00839         btScalar xEllipse =  vSwingAxis.y();
00840         btScalar yEllipse = -vSwingAxis.z();
00841 
00842         // Now, we use the slope of the vector (using x/yEllipse) and find the length
00843         // of the line that intersects the ellipse:
00844         //  x^2   y^2
00845         //  --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
00846         //  a^2   b^2
00847         // Do the math and it should be clear.
00848 
00849         swingLimit = m_swingSpan1; // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1
00850         if (fabs(xEllipse) > SIMD_EPSILON)
00851         {
00852             btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
00853             btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
00854             norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
00855             btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
00856             swingLimit = sqrt(swingLimit2);
00857         }
00858 
00859         // test!
00860         /*swingLimit = m_swingSpan2;
00861         if (fabs(vSwingAxis.z()) > SIMD_EPSILON)
00862         {
00863         btScalar mag_2 = m_swingSpan1*m_swingSpan1 + m_swingSpan2*m_swingSpan2;
00864         btScalar sinphi = m_swingSpan2 / sqrt(mag_2);
00865         btScalar phi = asin(sinphi);
00866         btScalar theta = atan2(fabs(vSwingAxis.y()),fabs(vSwingAxis.z()));
00867         btScalar alpha = 3.14159f - theta - phi;
00868         btScalar sinalpha = sin(alpha);
00869         swingLimit = m_swingSpan1 * sinphi/sinalpha;
00870         }*/
00871     }
00872     else if (swingAngle < 0)
00873     {
00874         // this should never happen!
00875         int wtf = 0; wtf = wtf;
00876     }
00877 }
00878 
00879 btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const
00880 {
00881     // compute x/y in ellipse using cone angle (0 -> 2*PI along surface of cone)
00882     btScalar xEllipse = btCos(fAngleInRadians);
00883     btScalar yEllipse = btSin(fAngleInRadians);
00884 
00885     // Use the slope of the vector (using x/yEllipse) and find the length
00886     // of the line that intersects the ellipse:
00887     //  x^2   y^2
00888     //  --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
00889     //  a^2   b^2
00890     // Do the math and it should be clear.
00891 
00892     float swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1)
00893     if (fabs(xEllipse) > SIMD_EPSILON)
00894     {
00895         btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
00896         btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
00897         norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
00898         btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
00899         swingLimit = sqrt(swingLimit2);
00900     }
00901 
00902     // convert into point in constraint space:
00903     // note: twist is x-axis, swing 1 and 2 are along the z and y axes respectively
00904     btVector3 vSwingAxis(0, xEllipse, -yEllipse);
00905     btQuaternion qSwing(vSwingAxis, swingLimit);
00906     btVector3 vPointInConstraintSpace(fLength,0,0);
00907     return quatRotate(qSwing, vPointInConstraintSpace);
00908 }
00909 
00910 // given a twist rotation in constraint space, (pre: cone must already be removed)
00911 // this method computes its corresponding angle and axis.
00912 void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist,
00913                                                   btScalar& twistAngle, // out
00914                                                   btVector3& vTwistAxis) // out
00915 {
00916     btQuaternion qMinTwist = qTwist;
00917     twistAngle = qTwist.getAngle();
00918 
00919     if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
00920     {
00921         qMinTwist = operator-(qTwist);
00922         twistAngle = qMinTwist.getAngle();
00923     }
00924     if (twistAngle < 0)
00925     {
00926         // this should never happen
00927         int wtf = 0; wtf = wtf;         
00928     }
00929 
00930     vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
00931     if (twistAngle > SIMD_EPSILON)
00932         vTwistAxis.normalize();
00933 }
00934 
00935 
00936 void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const
00937 {
00938     // the swing axis is computed as the "twist-free" cone rotation,
00939     // but the cone limit is not circular, but elliptical (if swingspan1 != swingspan2).
00940     // so, if we're outside the limits, the closest way back inside the cone isn't 
00941     // along the vector back to the center. better (and more stable) to use the ellipse normal.
00942 
00943     // convert swing axis to direction from center to surface of ellipse
00944     // (ie. rotate 2D vector by PI/2)
00945     btScalar y = -vSwingAxis.z();
00946     btScalar z =  vSwingAxis.y();
00947 
00948     // do the math...
00949     if (fabs(z) > SIMD_EPSILON) // avoid division by 0. and we don't need an update if z == 0.
00950     {
00951         // compute gradient/normal of ellipse surface at current "point"
00952         btScalar grad = y/z;
00953         grad *= m_swingSpan2 / m_swingSpan1;
00954 
00955         // adjust y/z to represent normal at point (instead of vector to point)
00956         if (y > 0)
00957             y =  fabs(grad * z);
00958         else
00959             y = -fabs(grad * z);
00960 
00961         // convert ellipse direction back to swing axis
00962         vSwingAxis.setZ(-y);
00963         vSwingAxis.setY( z);
00964         vSwingAxis.normalize();
00965     }
00966 }
00967 
00968 
00969 
00970 void btConeTwistConstraint::setMotorTarget(const btQuaternion &q)
00971 {
00972     btTransform trACur = m_rbA.getCenterOfMassTransform();
00973     btTransform trBCur = m_rbB.getCenterOfMassTransform();
00974     btTransform trABCur = trBCur.inverse() * trACur;
00975     btQuaternion qABCur = trABCur.getRotation();
00976     btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame);
00977     btQuaternion qConstraintCur = trConstraintCur.getRotation();
00978 
00979     btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * q * m_rbAFrame.getRotation();
00980     setMotorTargetInConstraintSpace(qConstraint);
00981 }
00982 
00983 
00984 void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &q)
00985 {
00986     m_qTarget = q;
00987 
00988     // clamp motor target to within limits
00989     {
00990         btScalar softness = 1.f;//m_limitSoftness;
00991 
00992         // split into twist and cone
00993         btVector3 vTwisted = quatRotate(m_qTarget, vTwist);
00994         btQuaternion qTargetCone  = shortestArcQuat(vTwist, vTwisted); qTargetCone.normalize();
00995         btQuaternion qTargetTwist = qTargetCone.inverse() * m_qTarget; qTargetTwist.normalize();
00996 
00997         // clamp cone
00998         if (m_swingSpan1 >= btScalar(0.05f) && m_swingSpan2 >= btScalar(0.05f))
00999         {
01000             btScalar swingAngle, swingLimit; btVector3 swingAxis;
01001             computeConeLimitInfo(qTargetCone, swingAngle, swingAxis, swingLimit);
01002 
01003             if (fabs(swingAngle) > SIMD_EPSILON)
01004             {
01005                 if (swingAngle > swingLimit*softness)
01006                     swingAngle = swingLimit*softness;
01007                 else if (swingAngle < -swingLimit*softness)
01008                     swingAngle = -swingLimit*softness;
01009                 qTargetCone = btQuaternion(swingAxis, swingAngle);
01010             }
01011         }
01012 
01013         // clamp twist
01014         if (m_twistSpan >= btScalar(0.05f))
01015         {
01016             btScalar twistAngle; btVector3 twistAxis;
01017             computeTwistLimitInfo(qTargetTwist, twistAngle, twistAxis);
01018 
01019             if (fabs(twistAngle) > SIMD_EPSILON)
01020             {
01021                 // eddy todo: limitSoftness used here???
01022                 if (twistAngle > m_twistSpan*softness)
01023                     twistAngle = m_twistSpan*softness;
01024                 else if (twistAngle < -m_twistSpan*softness)
01025                     twistAngle = -m_twistSpan*softness;
01026                 qTargetTwist = btQuaternion(twistAxis, twistAngle);
01027             }
01028         }
01029 
01030         m_qTarget = qTargetCone * qTargetTwist;
01031     }
01032 }
01033 
01036 void btConeTwistConstraint::setParam(int num, btScalar value, int axis)
01037 {
01038     switch(num)
01039     {
01040         case BT_CONSTRAINT_ERP :
01041         case BT_CONSTRAINT_STOP_ERP :
01042             if((axis >= 0) && (axis < 3)) 
01043             {
01044                 m_linERP = value;
01045                 m_flags |= BT_CONETWIST_FLAGS_LIN_ERP;
01046             }
01047             else
01048             {
01049                 m_biasFactor = value;
01050             }
01051             break;
01052         case BT_CONSTRAINT_CFM :
01053         case BT_CONSTRAINT_STOP_CFM :
01054             if((axis >= 0) && (axis < 3)) 
01055             {
01056                 m_linCFM = value;
01057                 m_flags |= BT_CONETWIST_FLAGS_LIN_CFM;
01058             }
01059             else
01060             {
01061                 m_angCFM = value;
01062                 m_flags |= BT_CONETWIST_FLAGS_ANG_CFM;
01063             }
01064             break;
01065         default:
01066             btAssertConstrParams(0);
01067             break;
01068     }
01069 }
01070 
01072 btScalar btConeTwistConstraint::getParam(int num, int axis) const 
01073 {
01074     btScalar retVal = 0;
01075     switch(num)
01076     {
01077         case BT_CONSTRAINT_ERP :
01078         case BT_CONSTRAINT_STOP_ERP :
01079             if((axis >= 0) && (axis < 3)) 
01080             {
01081                 btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_ERP);
01082                 retVal = m_linERP;
01083             }
01084             else if((axis >= 3) && (axis < 6)) 
01085             {
01086                 retVal = m_biasFactor;
01087             }
01088             else
01089             {
01090                 btAssertConstrParams(0);
01091             }
01092             break;
01093         case BT_CONSTRAINT_CFM :
01094         case BT_CONSTRAINT_STOP_CFM :
01095             if((axis >= 0) && (axis < 3)) 
01096             {
01097                 btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_CFM);
01098                 retVal = m_linCFM;
01099             }
01100             else if((axis >= 3) && (axis < 6)) 
01101             {
01102                 btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_ANG_CFM);
01103                 retVal = m_angCFM;
01104             }
01105             else
01106             {
01107                 btAssertConstrParams(0);
01108             }
01109             break;
01110         default : 
01111             btAssertConstrParams(0);
01112     }
01113     return retVal;
01114 }
01115 
01116 
01117 void btConeTwistConstraint::setFrames(const btTransform & frameA, const btTransform & frameB)
01118 {
01119     m_rbAFrame = frameA;
01120     m_rbBFrame = frameB;
01121     buildJacobian();
01122     //calculateTransforms();
01123 }
01124 
01125  
01126 
01127