00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
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
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
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
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
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
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
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
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
00338 btScalar depth = -(pivotAInW - pivotBInW).dot(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
00351 if (m_bMotorEnabled)
00352 {
00353
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
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
00373 btVector3 omegaADes, omegaBDes;
00374
00375 btTransformUtil::calculateVelocity(trACur, trADes, timeStep, zerovec, omegaADes);
00376 btTransformUtil::calculateVelocity(trBCur, trBDes, timeStep, zerovec, omegaBDes);
00377
00378
00379 btVector3 dOmegaA = omegaADes - omegaA;
00380 btVector3 dOmegaB = omegaBDes - omegaB;
00381
00382
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)
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
00457 {
00459 btVector3 angVelA;
00460 bodyA.internalGetAngularVelocity(angVelA);
00461 btVector3 angVelB;
00462 bodyB.internalGetAngularVelocity(angVelB);
00463
00464
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
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
00481
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
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)
00502 amplitude += m_twistLimitRatio * relTwistVel * m_relaxationFactor;
00503 btScalar impulseMag = amplitude * m_kTwist;
00504
00505
00506 btScalar temp = m_accTwistLimitImpulse;
00507 m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) );
00508 impulseMag = m_accTwistLimitImpulse - temp;
00509
00510
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
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
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
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
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);
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
00631 if (m_bMotorEnabled && (!m_useSolveConstraintObsolete))
00632 {
00633
00634
00635
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
00660 btQuaternion qA = transA.getRotation() * m_rbAFrame.getRotation();
00661 btQuaternion qB = transB.getRotation() * m_rbBFrame.getRotation();
00662 btQuaternion qAB = qB.inverse() * qA;
00663
00664
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
00679
00680
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
00689 m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness);
00690
00691
00692 adjustSwingAxisToUseEllipseNormal(swingAxis);
00693
00694
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
00707
00708
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 {
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 {
00729
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 {
00752
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
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
00822
00823
00824 void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
00825 btScalar& swingAngle,
00826 btVector3& vSwingAxis,
00827 btScalar& swingLimit)
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
00836 btAssert(fabs(vSwingAxis.x()) <= SIMD_EPSILON));
00837 #endif
00838
00839
00840
00841
00842
00843
00844
00845
00846 btScalar xEllipse = vSwingAxis.y();
00847 btScalar yEllipse = -vSwingAxis.z();
00848
00849
00850
00851
00852
00853
00854
00855
00856 swingLimit = m_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
00867
00868
00869
00870
00871
00872
00873
00874
00875
00876
00877
00878 }
00879 else if (swingAngle < 0)
00880 {
00881
00882 #if 0
00883 btAssert(0);
00884 #endif
00885 }
00886 }
00887
00888 btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const
00889 {
00890
00891 btScalar xEllipse = btCos(fAngleInRadians);
00892 btScalar yEllipse = btSin(fAngleInRadians);
00893
00894
00895
00896
00897
00898
00899
00900
00901 float swingLimit = m_swingSpan1;
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
00912
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
00920
00921 void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist,
00922 btScalar& twistAngle,
00923 btVector3& vTwistAxis)
00924 {
00925 btQuaternion qMinTwist = qTwist;
00926 twistAngle = qTwist.getAngle();
00927
00928 if (twistAngle > SIMD_PI)
00929 {
00930 qMinTwist = -(qTwist);
00931 twistAngle = qMinTwist.getAngle();
00932 }
00933 if (twistAngle < 0)
00934 {
00935
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
00950
00951
00952
00953
00954
00955
00956 btScalar y = -vSwingAxis.z();
00957 btScalar z = vSwingAxis.y();
00958
00959
00960 if (fabs(z) > SIMD_EPSILON)
00961 {
00962
00963 btScalar grad = y/z;
00964 grad *= m_swingSpan2 / m_swingSpan1;
00965
00966
00967 if (y > 0)
00968 y = fabs(grad * z);
00969 else
00970 y = -fabs(grad * z);
00971
00972
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
00986
00987
00988
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
01000 {
01001 btScalar softness = 1.f;
01002
01003
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
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
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
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
01134 }
01135
01136
01137
01138