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