1 /* 2 Bullet Continuous Collision Detection and Physics Library 3 btConeTwistConstraint is Copyright (c) 2007 Starbreeze Studios 4 5 This software is provided 'as-is', without any express or implied warranty. 6 In no event will the authors be held liable for any damages arising from the use of this software. 7 Permission is granted to anyone to use this software for any purpose, 8 including commercial applications, and to alter it and redistribute it freely, 9 subject to the following restrictions: 10 11 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. 12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 13 3. This notice may not be removed or altered from any source distribution. 14 15 Written by: Marcus Hennix 16 */ 17 18 19 #include "btConeTwistConstraint.h" 20 #include "BulletDynamics/Dynamics/btRigidBody.h" 21 #include "LinearMath/btTransformUtil.h" 22 #include "LinearMath/btMinMax.h" 23 #include <new> 24 25 26 27 //#define CONETWIST_USE_OBSOLETE_SOLVER true 28 #define CONETWIST_USE_OBSOLETE_SOLVER false 29 #define CONETWIST_DEF_FIX_THRESH btScalar(.05f) 30 31 32 SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis, const btMatrix3x3& invInertiaWorld) 33 { 34 btVector3 vec = axis * invInertiaWorld; 35 return axis.dot(vec); 36 } 37 38 39 40 41 btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB, 42 const btTransform& rbAFrame,const btTransform& rbBFrame) 43 :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame), 44 m_angularOnly(false), 45 m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER) 46 { 47 init(); 48 } 49 50 btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame) 51 :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame), 52 m_angularOnly(false), 53 m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER) 54 { 55 m_rbBFrame = m_rbAFrame; 56 m_rbBFrame.setOrigin(btVector3(0., 0., 0.)); 57 init(); 58 } 59 60 61 void btConeTwistConstraint::init() 62 { 63 m_angularOnly = false; 64 m_solveTwistLimit = false; 65 m_solveSwingLimit = false; 66 m_bMotorEnabled = false; 67 m_maxMotorImpulse = btScalar(-1); 68 69 setLimit(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT)); 70 m_damping = btScalar(0.01); 71 m_fixThresh = CONETWIST_DEF_FIX_THRESH; 72 m_flags = 0; 73 m_linCFM = btScalar(0.f); 74 m_linERP = btScalar(0.7f); 75 m_angCFM = btScalar(0.f); 76 } 77 78 79 void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info) 80 { 81 if (m_useSolveConstraintObsolete) 82 { 83 info->m_numConstraintRows = 0; 84 info->nub = 0; 85 } 86 else 87 { 88 info->m_numConstraintRows = 3; 89 info->nub = 3; 90 calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld()); 91 if(m_solveSwingLimit) 92 { 93 info->m_numConstraintRows++; 94 info->nub--; 95 if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) 96 { 97 info->m_numConstraintRows++; 98 info->nub--; 99 } 100 } 101 if(m_solveTwistLimit) 102 { 103 info->m_numConstraintRows++; 104 info->nub--; 105 } 106 } 107 } 108 109 void btConeTwistConstraint::getInfo1NonVirtual (btConstraintInfo1* info) 110 { 111 //always reserve 6 rows: object transform is not available on SPU 112 info->m_numConstraintRows = 6; 113 info->nub = 0; 114 115 } 116 117 118 void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info) 119 { 120 getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld()); 121 } 122 123 void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB) 124 { 125 calcAngleInfo2(transA,transB,invInertiaWorldA,invInertiaWorldB); 126 127 btAssert(!m_useSolveConstraintObsolete); 128 // set jacobian 129 info->m_J1linearAxis[0] = 1; 130 info->m_J1linearAxis[info->rowskip+1] = 1; 131 info->m_J1linearAxis[2*info->rowskip+2] = 1; 132 btVector3 a1 = transA.getBasis() * m_rbAFrame.getOrigin(); 133 { 134 btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); 135 btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip); 136 btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip); 137 btVector3 a1neg = -a1; 138 a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); 139 } 140 info->m_J2linearAxis[0] = -1; 141 info->m_J2linearAxis[info->rowskip+1] = -1; 142 info->m_J2linearAxis[2*info->rowskip+2] = -1; 143 btVector3 a2 = transB.getBasis() * m_rbBFrame.getOrigin(); 144 { 145 btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); 146 btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip); 147 btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip); 148 a2.getSkewSymmetricMatrix(angular0,angular1,angular2); 149 } 150 // set right hand side 151 btScalar linERP = (m_flags & BT_CONETWIST_FLAGS_LIN_ERP) ? m_linERP : info->erp; 152 btScalar k = info->fps * linERP; 153 int j; 154 for (j=0; j<3; j++) 155 { 156 info->m_constraintError[j*info->rowskip] = k * (a2[j] + transB.getOrigin()[j] - a1[j] - transA.getOrigin()[j]); 157 info->m_lowerLimit[j*info->rowskip] = -SIMD_INFINITY; 158 info->m_upperLimit[j*info->rowskip] = SIMD_INFINITY; 159 if(m_flags & BT_CONETWIST_FLAGS_LIN_CFM) 160 { 161 info->cfm[j*info->rowskip] = m_linCFM; 162 } 163 } 164 int row = 3; 165 int srow = row * info->rowskip; 166 btVector3 ax1; 167 // angular limits 168 if(m_solveSwingLimit) 169 { 170 btScalar *J1 = info->m_J1angularAxis; 171 btScalar *J2 = info->m_J2angularAxis; 172 if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) 173 { 174 btTransform trA = transA*m_rbAFrame; 175 btVector3 p = trA.getBasis().getColumn(1); 176 btVector3 q = trA.getBasis().getColumn(2); 177 int srow1 = srow + info->rowskip; 178 J1[srow+0] = p[0]; 179 J1[srow+1] = p[1]; 180 J1[srow+2] = p[2]; 181 J1[srow1+0] = q[0]; 182 J1[srow1+1] = q[1]; 183 J1[srow1+2] = q[2]; 184 J2[srow+0] = -p[0]; 185 J2[srow+1] = -p[1]; 186 J2[srow+2] = -p[2]; 187 J2[srow1+0] = -q[0]; 188 J2[srow1+1] = -q[1]; 189 J2[srow1+2] = -q[2]; 190 btScalar fact = info->fps * m_relaxationFactor; 191 info->m_constraintError[srow] = fact * m_swingAxis.dot(p); 192 info->m_constraintError[srow1] = fact * m_swingAxis.dot(q); 193 info->m_lowerLimit[srow] = -SIMD_INFINITY; 194 info->m_upperLimit[srow] = SIMD_INFINITY; 195 info->m_lowerLimit[srow1] = -SIMD_INFINITY; 196 info->m_upperLimit[srow1] = SIMD_INFINITY; 197 srow = srow1 + info->rowskip; 198 } 199 else 200 { 201 ax1 = m_swingAxis * m_relaxationFactor * m_relaxationFactor; 202 J1[srow+0] = ax1[0]; 203 J1[srow+1] = ax1[1]; 204 J1[srow+2] = ax1[2]; 205 J2[srow+0] = -ax1[0]; 206 J2[srow+1] = -ax1[1]; 207 J2[srow+2] = -ax1[2]; 208 btScalar k = info->fps * m_biasFactor; 209 210 info->m_constraintError[srow] = k * m_swingCorrection; 211 if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM) 212 { 213 info->cfm[srow] = m_angCFM; 214 } 215 // m_swingCorrection is always positive or 0 216 info->m_lowerLimit[srow] = 0; 217 info->m_upperLimit[srow] = SIMD_INFINITY; 218 srow += info->rowskip; 219 } 220 } 221 if(m_solveTwistLimit) 222 { 223 ax1 = m_twistAxis * m_relaxationFactor * m_relaxationFactor; 224 btScalar *J1 = info->m_J1angularAxis; 225 btScalar *J2 = info->m_J2angularAxis; 226 J1[srow+0] = ax1[0]; 227 J1[srow+1] = ax1[1]; 228 J1[srow+2] = ax1[2]; 229 J2[srow+0] = -ax1[0]; 230 J2[srow+1] = -ax1[1]; 231 J2[srow+2] = -ax1[2]; 232 btScalar k = info->fps * m_biasFactor; 233 info->m_constraintError[srow] = k * m_twistCorrection; 234 if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM) 235 { 236 info->cfm[srow] = m_angCFM; 237 } 238 if(m_twistSpan > 0.0f) 239 { 240 241 if(m_twistCorrection > 0.0f) 242 { 243 info->m_lowerLimit[srow] = 0; 244 info->m_upperLimit[srow] = SIMD_INFINITY; 245 } 246 else 247 { 248 info->m_lowerLimit[srow] = -SIMD_INFINITY; 249 info->m_upperLimit[srow] = 0; 250 } 251 } 252 else 253 { 254 info->m_lowerLimit[srow] = -SIMD_INFINITY; 255 info->m_upperLimit[srow] = SIMD_INFINITY; 256 } 257 srow += info->rowskip; 258 } 259 } 260 261 262 263 void btConeTwistConstraint::buildJacobian() 264 { 265 if (m_useSolveConstraintObsolete) 266 { 267 m_appliedImpulse = btScalar(0.); 268 m_accTwistLimitImpulse = btScalar(0.); 269 m_accSwingLimitImpulse = btScalar(0.); 270 m_accMotorImpulse = btVector3(0.,0.,0.); 271 272 if (!m_angularOnly) 273 { 274 btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); 275 btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); 276 btVector3 relPos = pivotBInW - pivotAInW; 277 278 btVector3 normal[3]; 279 if (relPos.length2() > SIMD_EPSILON) 280 { 281 normal[0] = relPos.normalized(); 282 } 283 else 284 { 285 normal[0].setValue(btScalar(1.0),0,0); 286 } 287 288 btPlaneSpace1(normal[0], normal[1], normal[2]); 289 290 for (int i=0;i<3;i++) 291 { 292 new (&m_jac[i]) btJacobianEntry( 293 m_rbA.getCenterOfMassTransform().getBasis().transpose(), 294 m_rbB.getCenterOfMassTransform().getBasis().transpose(), 295 pivotAInW - m_rbA.getCenterOfMassPosition(), 296 pivotBInW - m_rbB.getCenterOfMassPosition(), 297 normal[i], 298 m_rbA.getInvInertiaDiagLocal(), 299 m_rbA.getInvMass(), 300 m_rbB.getInvInertiaDiagLocal(), 301 m_rbB.getInvMass()); 302 } 303 } 304 305 calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld()); 306 } 307 } 308 309 310 311 void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) 312 { 313 #ifndef __SPU__ 314 if (m_useSolveConstraintObsolete) 315 { 316 btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); 317 btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); 318 319 btScalar tau = btScalar(0.3); 320 321 //linear part 322 if (!m_angularOnly) 323 { 324 btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 325 btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); 326 327 btVector3 vel1; 328 bodyA.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1); 329 btVector3 vel2; 330 bodyB.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2); 331 btVector3 vel = vel1 - vel2; 332 333 for (int i=0;i<3;i++) 334 { 335 const btVector3& normal = m_jac[i].m_linearJointAxis; 336 btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); 337 338 btScalar rel_vel; 339 rel_vel = normal.dot(vel); 340 //positional error (zeroth order error) 341 btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal 342 btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv; 343 m_appliedImpulse += impulse; 344 345 btVector3 ftorqueAxis1 = rel_pos1.cross(normal); 346 btVector3 ftorqueAxis2 = rel_pos2.cross(normal); 347 bodyA.internalApplyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse); 348 bodyB.internalApplyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse); 349 350 } 351 } 352 353 // apply motor 354 if (m_bMotorEnabled) 355 { 356 // compute current and predicted transforms 357 btTransform trACur = m_rbA.getCenterOfMassTransform(); 358 btTransform trBCur = m_rbB.getCenterOfMassTransform(); 359 btVector3 omegaA; bodyA.internalGetAngularVelocity(omegaA); 360 btVector3 omegaB; bodyB.internalGetAngularVelocity(omegaB); 361 btTransform trAPred; trAPred.setIdentity(); 362 btVector3 zerovec(0,0,0); 363 btTransformUtil::integrateTransform( 364 trACur, zerovec, omegaA, timeStep, trAPred); 365 btTransform trBPred; trBPred.setIdentity(); 366 btTransformUtil::integrateTransform( 367 trBCur, zerovec, omegaB, timeStep, trBPred); 368 369 // compute desired transforms in world 370 btTransform trPose(m_qTarget); 371 btTransform trABDes = m_rbBFrame * trPose * m_rbAFrame.inverse(); 372 btTransform trADes = trBPred * trABDes; 373 btTransform trBDes = trAPred * trABDes.inverse(); 374 375 // compute desired omegas in world 376 btVector3 omegaADes, omegaBDes; 377 378 btTransformUtil::calculateVelocity(trACur, trADes, timeStep, zerovec, omegaADes); 379 btTransformUtil::calculateVelocity(trBCur, trBDes, timeStep, zerovec, omegaBDes); 380 381 // compute delta omegas 382 btVector3 dOmegaA = omegaADes - omegaA; 383 btVector3 dOmegaB = omegaBDes - omegaB; 384 385 // compute weighted avg axis of dOmega (weighting based on inertias) 386 btVector3 axisA, axisB; 387 btScalar kAxisAInv = 0, kAxisBInv = 0; 388 389 if (dOmegaA.length2() > SIMD_EPSILON) 390 { 391 axisA = dOmegaA.normalized(); 392 kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(axisA); 393 } 394 395 if (dOmegaB.length2() > SIMD_EPSILON) 396 { 397 axisB = dOmegaB.normalized(); 398 kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(axisB); 399 } 400 401 btVector3 avgAxis = kAxisAInv * axisA + kAxisBInv * axisB; 402 403 static bool bDoTorque = true; 404 if (bDoTorque && avgAxis.length2() > SIMD_EPSILON) 405 { 406 avgAxis.normalize(); 407 kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(avgAxis); 408 kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(avgAxis); 409 btScalar kInvCombined = kAxisAInv + kAxisBInv; 410 411 btVector3 impulse = (kAxisAInv * dOmegaA - kAxisBInv * dOmegaB) / 412 (kInvCombined * kInvCombined); 413 414 if (m_maxMotorImpulse >= 0) 415 { 416 btScalar fMaxImpulse = m_maxMotorImpulse; 417 if (m_bNormalizedMotorStrength) 418 fMaxImpulse = fMaxImpulse/kAxisAInv; 419 420 btVector3 newUnclampedAccImpulse = m_accMotorImpulse + impulse; 421 btScalar newUnclampedMag = newUnclampedAccImpulse.length(); 422 if (newUnclampedMag > fMaxImpulse) 423 { 424 newUnclampedAccImpulse.normalize(); 425 newUnclampedAccImpulse *= fMaxImpulse; 426 impulse = newUnclampedAccImpulse - m_accMotorImpulse; 427 } 428 m_accMotorImpulse += impulse; 429 } 430 431 btScalar impulseMag = impulse.length(); 432 btVector3 impulseAxis = impulse / impulseMag; 433 434 bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag); 435 bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag); 436 437 } 438 } 439 else if (m_damping > SIMD_EPSILON) // no motor: do a little damping 440 { 441 btVector3 angVelA; bodyA.internalGetAngularVelocity(angVelA); 442 btVector3 angVelB; bodyB.internalGetAngularVelocity(angVelB); 443 btVector3 relVel = angVelB - angVelA; 444 if (relVel.length2() > SIMD_EPSILON) 445 { 446 btVector3 relVelAxis = relVel.normalized(); 447 btScalar m_kDamping = btScalar(1.) / 448 (getRigidBodyA().computeAngularImpulseDenominator(relVelAxis) + 449 getRigidBodyB().computeAngularImpulseDenominator(relVelAxis)); 450 btVector3 impulse = m_damping * m_kDamping * relVel; 451 452 btScalar impulseMag = impulse.length(); 453 btVector3 impulseAxis = impulse / impulseMag; 454 bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag); 455 bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag); 456 } 457 } 458 459 // joint limits 460 { 461 ///solve angular part 462 btVector3 angVelA; 463 bodyA.internalGetAngularVelocity(angVelA); 464 btVector3 angVelB; 465 bodyB.internalGetAngularVelocity(angVelB); 466 467 // solve swing limit 468 if (m_solveSwingLimit) 469 { 470 btScalar amplitude = m_swingLimitRatio * m_swingCorrection*m_biasFactor/timeStep; 471 btScalar relSwingVel = (angVelB - angVelA).dot(m_swingAxis); 472 if (relSwingVel > 0) 473 amplitude += m_swingLimitRatio * relSwingVel * m_relaxationFactor; 474 btScalar impulseMag = amplitude * m_kSwing; 475 476 // Clamp the accumulated impulse 477 btScalar temp = m_accSwingLimitImpulse; 478 m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) ); 479 impulseMag = m_accSwingLimitImpulse - temp; 480 481 btVector3 impulse = m_swingAxis * impulseMag; 482 483 // don't let cone response affect twist 484 // (this can happen since body A's twist doesn't match body B's AND we use an elliptical cone limit) 485 { 486 btVector3 impulseTwistCouple = impulse.dot(m_twistAxisA) * m_twistAxisA; 487 btVector3 impulseNoTwistCouple = impulse - impulseTwistCouple; 488 impulse = impulseNoTwistCouple; 489 } 490 491 impulseMag = impulse.length(); 492 btVector3 noTwistSwingAxis = impulse / impulseMag; 493 494 bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag); 495 bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag); 496 } 497 498 499 // solve twist limit 500 if (m_solveTwistLimit) 501 { 502 btScalar amplitude = m_twistLimitRatio * m_twistCorrection*m_biasFactor/timeStep; 503 btScalar relTwistVel = (angVelB - angVelA).dot( m_twistAxis ); 504 if (relTwistVel > 0) // only damp when moving towards limit (m_twistAxis flipping is important) 505 amplitude += m_twistLimitRatio * relTwistVel * m_relaxationFactor; 506 btScalar impulseMag = amplitude * m_kTwist; 507 508 // Clamp the accumulated impulse 509 btScalar temp = m_accTwistLimitImpulse; 510 m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) ); 511 impulseMag = m_accTwistLimitImpulse - temp; 512 513 // btVector3 impulse = m_twistAxis * impulseMag; 514 515 bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag); 516 bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag); 517 } 518 } 519 } 520 #else 521 btAssert(0); 522 #endif //__SPU__ 523 } 524 525 526 527 528 void btConeTwistConstraint::updateRHS(btScalar timeStep) 529 { 530 (void)timeStep; 531 532 } 533 534 535 #ifndef __SPU__ 536 void btConeTwistConstraint::calcAngleInfo() 537 { 538 m_swingCorrection = btScalar(0.); 539 m_twistLimitSign = btScalar(0.); 540 m_solveTwistLimit = false; 541 m_solveSwingLimit = false; 542 543 btVector3 b1Axis1(0,0,0),b1Axis2(0,0,0),b1Axis3(0,0,0); 544 btVector3 b2Axis1(0,0,0),b2Axis2(0,0,0); 545 546 b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0); 547 b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0); 548 549 btScalar swing1=btScalar(0.),swing2 = btScalar(0.); 550 551 btScalar swx=btScalar(0.),swy = btScalar(0.); 552 btScalar thresh = btScalar(10.); 553 btScalar fact; 554 555 // Get Frame into world space 556 if (m_swingSpan1 >= btScalar(0.05f)) 557 { 558 b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1); 559 swx = b2Axis1.dot(b1Axis1); 560 swy = b2Axis1.dot(b1Axis2); 561 swing1 = btAtan2Fast(swy, swx); 562 fact = (swy*swy + swx*swx) * thresh * thresh; 563 fact = fact / (fact + btScalar(1.0)); 564 swing1 *= fact; 565 } 566 567 if (m_swingSpan2 >= btScalar(0.05f)) 568 { 569 b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2); 570 swx = b2Axis1.dot(b1Axis1); 571 swy = b2Axis1.dot(b1Axis3); 572 swing2 = btAtan2Fast(swy, swx); 573 fact = (swy*swy + swx*swx) * thresh * thresh; 574 fact = fact / (fact + btScalar(1.0)); 575 swing2 *= fact; 576 } 577 578 btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1); 579 btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2); 580 btScalar EllipseAngle = btFabs(swing1*swing1)* RMaxAngle1Sq + btFabs(swing2*swing2) * RMaxAngle2Sq; 581 582 if (EllipseAngle > 1.0f) 583 { 584 m_swingCorrection = EllipseAngle-1.0f; 585 m_solveSwingLimit = true; 586 // Calculate necessary axis & factors 587 m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3)); 588 m_swingAxis.normalize(); 589 btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f; 590 m_swingAxis *= swingAxisSign; 591 } 592 593 // Twist limits 594 if (m_twistSpan >= btScalar(0.)) 595 { 596 btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1); 597 btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1); 598 btVector3 TwistRef = quatRotate(rotationArc,b2Axis2); 599 btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) ); 600 m_twistAngle = twist; 601 602 // btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.); 603 btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? btScalar(1.0f) : btScalar(0.); 604 if (twist <= -m_twistSpan*lockedFreeFactor) 605 { 606 m_twistCorrection = -(twist + m_twistSpan); 607 m_solveTwistLimit = true; 608 m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; 609 m_twistAxis.normalize(); 610 m_twistAxis *= -1.0f; 611 } 612 else if (twist > m_twistSpan*lockedFreeFactor) 613 { 614 m_twistCorrection = (twist - m_twistSpan); 615 m_solveTwistLimit = true; 616 m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; 617 m_twistAxis.normalize(); 618 } 619 } 620 } 621 #endif //__SPU__ 622 623 static btVector3 vTwist(1,0,0); // twist axis in constraint's space 624 625 626 627 void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB) 628 { 629 m_swingCorrection = btScalar(0.); 630 m_twistLimitSign = btScalar(0.); 631 m_solveTwistLimit = false; 632 m_solveSwingLimit = false; 633 // compute rotation of A wrt B (in constraint space) 634 if (m_bMotorEnabled && (!m_useSolveConstraintObsolete)) 635 { // it is assumed that setMotorTarget() was alredy called 636 // and motor target m_qTarget is within constraint limits 637 // TODO : split rotation to pure swing and pure twist 638 // compute desired transforms in world 639 btTransform trPose(m_qTarget); 640 btTransform trA = transA * m_rbAFrame; 641 btTransform trB = transB * m_rbBFrame; 642 btTransform trDeltaAB = trB * trPose * trA.inverse(); 643 btQuaternion qDeltaAB = trDeltaAB.getRotation(); 644 btVector3 swingAxis = btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z()); 645 float swingAxisLen2 = swingAxis.length2(); 646 if(btFuzzyZero(swingAxisLen2)) 647 { 648 return; 649 } 650 m_swingAxis = swingAxis; 651 m_swingAxis.normalize(); 652 m_swingCorrection = qDeltaAB.getAngle(); 653 if(!btFuzzyZero(m_swingCorrection)) 654 { 655 m_solveSwingLimit = true; 656 } 657 return; 658 } 659 660 661 { 662 // compute rotation of A wrt B (in constraint space) 663 btQuaternion qA = transA.getRotation() * m_rbAFrame.getRotation(); 664 btQuaternion qB = transB.getRotation() * m_rbBFrame.getRotation(); 665 btQuaternion qAB = qB.inverse() * qA; 666 // split rotation into cone and twist 667 // (all this is done from B's perspective. Maybe I should be averaging axes...) 668 btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize(); 669 btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize(); 670 btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize(); 671 672 if (m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh) 673 { 674 btScalar swingAngle, swingLimit = 0; btVector3 swingAxis; 675 computeConeLimitInfo(qABCone, swingAngle, swingAxis, swingLimit); 676 677 if (swingAngle > swingLimit * m_limitSoftness) 678 { 679 m_solveSwingLimit = true; 680 681 // compute limit ratio: 0->1, where 682 // 0 == beginning of soft limit 683 // 1 == hard/real limit 684 m_swingLimitRatio = 1.f; 685 if (swingAngle < swingLimit && m_limitSoftness < 1.f - SIMD_EPSILON) 686 { 687 m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness)/ 688 (swingLimit - swingLimit * m_limitSoftness); 689 } 690 691 // swing correction tries to get back to soft limit 692 m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness); 693 694 // adjustment of swing axis (based on ellipse normal) 695 adjustSwingAxisToUseEllipseNormal(swingAxis); 696 697 // Calculate necessary axis & factors 698 m_swingAxis = quatRotate(qB, -swingAxis); 699 700 m_twistAxisA.setValue(0,0,0); 701 702 m_kSwing = btScalar(1.) / 703 (computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldA) + 704 computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldB)); 705 } 706 } 707 else 708 { 709 // you haven't set any limits; 710 // or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?) 711 // anyway, we have either hinge or fixed joint 712 btVector3 ivA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0); 713 btVector3 jvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1); 714 btVector3 kvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(2); 715 btVector3 ivB = transB.getBasis() * m_rbBFrame.getBasis().getColumn(0); 716 btVector3 target; 717 btScalar x = ivB.dot(ivA); 718 btScalar y = ivB.dot(jvA); 719 btScalar z = ivB.dot(kvA); 720 if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) 721 { // fixed. We'll need to add one more row to constraint 722 if((!btFuzzyZero(y)) || (!(btFuzzyZero(z)))) 723 { 724 m_solveSwingLimit = true; 725 m_swingAxis = -ivB.cross(ivA); 726 } 727 } 728 else 729 { 730 if(m_swingSpan1 < m_fixThresh) 731 { // hinge around Y axis 732 // if(!(btFuzzyZero(y))) 733 if((!(btFuzzyZero(x))) || (!(btFuzzyZero(z)))) 734 { 735 m_solveSwingLimit = true; 736 if(m_swingSpan2 >= m_fixThresh) 737 { 738 y = btScalar(0.f); 739 btScalar span2 = btAtan2(z, x); 740 if(span2 > m_swingSpan2) 741 { 742 x = btCos(m_swingSpan2); 743 z = btSin(m_swingSpan2); 744 } 745 else if(span2 < -m_swingSpan2) 746 { 747 x = btCos(m_swingSpan2); 748 z = -btSin(m_swingSpan2); 749 } 750 } 751 } 752 } 753 else 754 { // hinge around Z axis 755 // if(!btFuzzyZero(z)) 756 if((!(btFuzzyZero(x))) || (!(btFuzzyZero(y)))) 757 { 758 m_solveSwingLimit = true; 759 if(m_swingSpan1 >= m_fixThresh) 760 { 761 z = btScalar(0.f); 762 btScalar span1 = btAtan2(y, x); 763 if(span1 > m_swingSpan1) 764 { 765 x = btCos(m_swingSpan1); 766 y = btSin(m_swingSpan1); 767 } 768 else if(span1 < -m_swingSpan1) 769 { 770 x = btCos(m_swingSpan1); 771 y = -btSin(m_swingSpan1); 772 } 773 } 774 } 775 } 776 target[0] = x * ivA[0] + y * jvA[0] + z * kvA[0]; 777 target[1] = x * ivA[1] + y * jvA[1] + z * kvA[1]; 778 target[2] = x * ivA[2] + y * jvA[2] + z * kvA[2]; 779 target.normalize(); 780 m_swingAxis = -ivB.cross(target); 781 m_swingCorrection = m_swingAxis.length(); 782 783 if (!btFuzzyZero(m_swingCorrection)) 784 m_swingAxis.normalize(); 785 } 786 } 787 788 if (m_twistSpan >= btScalar(0.f)) 789 { 790 btVector3 twistAxis; 791 computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis); 792 793 if (m_twistAngle > m_twistSpan*m_limitSoftness) 794 { 795 m_solveTwistLimit = true; 796 797 m_twistLimitRatio = 1.f; 798 if (m_twistAngle < m_twistSpan && m_limitSoftness < 1.f - SIMD_EPSILON) 799 { 800 m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness)/ 801 (m_twistSpan - m_twistSpan * m_limitSoftness); 802 } 803 804 // twist correction tries to get back to soft limit 805 m_twistCorrection = m_twistAngle - (m_twistSpan * m_limitSoftness); 806 807 m_twistAxis = quatRotate(qB, -twistAxis); 808 809 m_kTwist = btScalar(1.) / 810 (computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldA) + 811 computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldB)); 812 } 813 814 if (m_solveSwingLimit) 815 m_twistAxisA = quatRotate(qA, -twistAxis); 816 } 817 else 818 { 819 m_twistAngle = btScalar(0.f); 820 } 821 } 822 } 823 824 825 826 // given a cone rotation in constraint space, (pre: twist must already be removed) 827 // this method computes its corresponding swing angle and axis. 828 // more interestingly, it computes the cone/swing limit (angle) for this cone "pose". 829 void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone, 830 btScalar& swingAngle, // out 831 btVector3& vSwingAxis, // out 832 btScalar& swingLimit) // out 833 { 834 swingAngle = qCone.getAngle(); 835 if (swingAngle > SIMD_EPSILON) 836 { 837 vSwingAxis = btVector3(qCone.x(), qCone.y(), qCone.z()); 838 vSwingAxis.normalize(); 839 #if 0 840 // non-zero twist?! this should never happen. 841 btAssert(fabs(vSwingAxis.x()) <= SIMD_EPSILON)); 842 #endif 843 844 // Compute limit for given swing. tricky: 845 // Given a swing axis, we're looking for the intersection with the bounding cone ellipse. 846 // (Since we're dealing with angles, this ellipse is embedded on the surface of a sphere.) 847 848 // For starters, compute the direction from center to surface of ellipse. 849 // This is just the perpendicular (ie. rotate 2D vector by PI/2) of the swing axis. 850 // (vSwingAxis is the cone rotation (in z,y); change vars and rotate to (x,y) coords.) 851 btScalar xEllipse = vSwingAxis.y(); 852 btScalar yEllipse = -vSwingAxis.z(); 853 854 // Now, we use the slope of the vector (using x/yEllipse) and find the length 855 // of the line that intersects the ellipse: 856 // x^2 y^2 857 // --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits) 858 // a^2 b^2 859 // Do the math and it should be clear. 860 861 swingLimit = m_swingSpan1; // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1 862 if (fabs(xEllipse) > SIMD_EPSILON) 863 { 864 btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse); 865 btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2); 866 norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1); 867 btScalar swingLimit2 = (1 + surfaceSlope2) / norm; 868 swingLimit = sqrt(swingLimit2); 869 } 870 871 // test! 872 /*swingLimit = m_swingSpan2; 873 if (fabs(vSwingAxis.z()) > SIMD_EPSILON) 874 { 875 btScalar mag_2 = m_swingSpan1*m_swingSpan1 + m_swingSpan2*m_swingSpan2; 876 btScalar sinphi = m_swingSpan2 / sqrt(mag_2); 877 btScalar phi = asin(sinphi); 878 btScalar theta = atan2(fabs(vSwingAxis.y()),fabs(vSwingAxis.z())); 879 btScalar alpha = 3.14159f - theta - phi; 880 btScalar sinalpha = sin(alpha); 881 swingLimit = m_swingSpan1 * sinphi/sinalpha; 882 }*/ 883 } 884 else if (swingAngle < 0) 885 { 886 // this should never happen! 887 #if 0 888 btAssert(0); 889 #endif 890 } 891 } 892 893 btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const 894 { 895 // compute x/y in ellipse using cone angle (0 -> 2*PI along surface of cone) 896 btScalar xEllipse = btCos(fAngleInRadians); 897 btScalar yEllipse = btSin(fAngleInRadians); 898 899 // Use the slope of the vector (using x/yEllipse) and find the length 900 // of the line that intersects the ellipse: 901 // x^2 y^2 902 // --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits) 903 // a^2 b^2 904 // Do the math and it should be clear. 905 906 float swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1) 907 if (fabs(xEllipse) > SIMD_EPSILON) 908 { 909 btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse); 910 btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2); 911 norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1); 912 btScalar swingLimit2 = (1 + surfaceSlope2) / norm; 913 swingLimit = sqrt(swingLimit2); 914 } 915 916 // convert into point in constraint space: 917 // note: twist is x-axis, swing 1 and 2 are along the z and y axes respectively 918 btVector3 vSwingAxis(0, xEllipse, -yEllipse); 919 btQuaternion qSwing(vSwingAxis, swingLimit); 920 btVector3 vPointInConstraintSpace(fLength,0,0); 921 return quatRotate(qSwing, vPointInConstraintSpace); 922 } 923 924 // given a twist rotation in constraint space, (pre: cone must already be removed) 925 // this method computes its corresponding angle and axis. 926 void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist, 927 btScalar& twistAngle, // out 928 btVector3& vTwistAxis) // out 929 { 930 btQuaternion qMinTwist = qTwist; 931 twistAngle = qTwist.getAngle(); 932 933 if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate. 934 { 935 qMinTwist = -(qTwist); 936 twistAngle = qMinTwist.getAngle(); 937 } 938 if (twistAngle < 0) 939 { 940 // this should never happen 941 #if 0 942 btAssert(0); 943 #endif 944 } 945 946 vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z()); 947 if (twistAngle > SIMD_EPSILON) 948 vTwistAxis.normalize(); 949 } 950 951 952 void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const 953 { 954 // the swing axis is computed as the "twist-free" cone rotation, 955 // but the cone limit is not circular, but elliptical (if swingspan1 != swingspan2). 956 // so, if we're outside the limits, the closest way back inside the cone isn't 957 // along the vector back to the center. better (and more stable) to use the ellipse normal. 958 959 // convert swing axis to direction from center to surface of ellipse 960 // (ie. rotate 2D vector by PI/2) 961 btScalar y = -vSwingAxis.z(); 962 btScalar z = vSwingAxis.y(); 963 964 // do the math... 965 if (fabs(z) > SIMD_EPSILON) // avoid division by 0. and we don't need an update if z == 0. 966 { 967 // compute gradient/normal of ellipse surface at current "point" 968 btScalar grad = y/z; 969 grad *= m_swingSpan2 / m_swingSpan1; 970 971 // adjust y/z to represent normal at point (instead of vector to point) 972 if (y > 0) 973 y = fabs(grad * z); 974 else 975 y = -fabs(grad * z); 976 977 // convert ellipse direction back to swing axis 978 vSwingAxis.setZ(-y); 979 vSwingAxis.setY( z); 980 vSwingAxis.normalize(); 981 } 982 } 983 984 985 986 void btConeTwistConstraint::setMotorTarget(const btQuaternion &q) 987 { 988 //btTransform trACur = m_rbA.getCenterOfMassTransform(); 989 //btTransform trBCur = m_rbB.getCenterOfMassTransform(); 990 // btTransform trABCur = trBCur.inverse() * trACur; 991 // btQuaternion qABCur = trABCur.getRotation(); 992 // btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame); 993 //btQuaternion qConstraintCur = trConstraintCur.getRotation(); 994 995 btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * q * m_rbAFrame.getRotation(); 996 setMotorTargetInConstraintSpace(qConstraint); 997 } 998 999 1000 void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &q) 1001 { 1002 m_qTarget = q; 1003 1004 // clamp motor target to within limits 1005 { 1006 btScalar softness = 1.f;//m_limitSoftness; 1007 1008 // split into twist and cone 1009 btVector3 vTwisted = quatRotate(m_qTarget, vTwist); 1010 btQuaternion qTargetCone = shortestArcQuat(vTwist, vTwisted); qTargetCone.normalize(); 1011 btQuaternion qTargetTwist = qTargetCone.inverse() * m_qTarget; qTargetTwist.normalize(); 1012 1013 // clamp cone 1014 if (m_swingSpan1 >= btScalar(0.05f) && m_swingSpan2 >= btScalar(0.05f)) 1015 { 1016 btScalar swingAngle, swingLimit; btVector3 swingAxis; 1017 computeConeLimitInfo(qTargetCone, swingAngle, swingAxis, swingLimit); 1018 1019 if (fabs(swingAngle) > SIMD_EPSILON) 1020 { 1021 if (swingAngle > swingLimit*softness) 1022 swingAngle = swingLimit*softness; 1023 else if (swingAngle < -swingLimit*softness) 1024 swingAngle = -swingLimit*softness; 1025 qTargetCone = btQuaternion(swingAxis, swingAngle); 1026 } 1027 } 1028 1029 // clamp twist 1030 if (m_twistSpan >= btScalar(0.05f)) 1031 { 1032 btScalar twistAngle; btVector3 twistAxis; 1033 computeTwistLimitInfo(qTargetTwist, twistAngle, twistAxis); 1034 1035 if (fabs(twistAngle) > SIMD_EPSILON) 1036 { 1037 // eddy todo: limitSoftness used here??? 1038 if (twistAngle > m_twistSpan*softness) 1039 twistAngle = m_twistSpan*softness; 1040 else if (twistAngle < -m_twistSpan*softness) 1041 twistAngle = -m_twistSpan*softness; 1042 qTargetTwist = btQuaternion(twistAxis, twistAngle); 1043 } 1044 } 1045 1046 m_qTarget = qTargetCone * qTargetTwist; 1047 } 1048 } 1049 1050 ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 1051 ///If no axis is provided, it uses the default axis for this constraint. 1052 void btConeTwistConstraint::setParam(int num, btScalar value, int axis) 1053 { 1054 switch(num) 1055 { 1056 case BT_CONSTRAINT_ERP : 1057 case BT_CONSTRAINT_STOP_ERP : 1058 if((axis >= 0) && (axis < 3)) 1059 { 1060 m_linERP = value; 1061 m_flags |= BT_CONETWIST_FLAGS_LIN_ERP; 1062 } 1063 else 1064 { 1065 m_biasFactor = value; 1066 } 1067 break; 1068 case BT_CONSTRAINT_CFM : 1069 case BT_CONSTRAINT_STOP_CFM : 1070 if((axis >= 0) && (axis < 3)) 1071 { 1072 m_linCFM = value; 1073 m_flags |= BT_CONETWIST_FLAGS_LIN_CFM; 1074 } 1075 else 1076 { 1077 m_angCFM = value; 1078 m_flags |= BT_CONETWIST_FLAGS_ANG_CFM; 1079 } 1080 break; 1081 default: 1082 btAssertConstrParams(0); 1083 break; 1084 } 1085 } 1086 1087 ///return the local value of parameter 1088 btScalar btConeTwistConstraint::getParam(int num, int axis) const 1089 { 1090 btScalar retVal = 0; 1091 switch(num) 1092 { 1093 case BT_CONSTRAINT_ERP : 1094 case BT_CONSTRAINT_STOP_ERP : 1095 if((axis >= 0) && (axis < 3)) 1096 { 1097 btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_ERP); 1098 retVal = m_linERP; 1099 } 1100 else if((axis >= 3) && (axis < 6)) 1101 { 1102 retVal = m_biasFactor; 1103 } 1104 else 1105 { 1106 btAssertConstrParams(0); 1107 } 1108 break; 1109 case BT_CONSTRAINT_CFM : 1110 case BT_CONSTRAINT_STOP_CFM : 1111 if((axis >= 0) && (axis < 3)) 1112 { 1113 btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_CFM); 1114 retVal = m_linCFM; 1115 } 1116 else if((axis >= 3) && (axis < 6)) 1117 { 1118 btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_ANG_CFM); 1119 retVal = m_angCFM; 1120 } 1121 else 1122 { 1123 btAssertConstrParams(0); 1124 } 1125 break; 1126 default : 1127 btAssertConstrParams(0); 1128 } 1129 return retVal; 1130 } 1131 1132 1133 void btConeTwistConstraint::setFrames(const btTransform & frameA, const btTransform & frameB) 1134 { 1135 m_rbAFrame = frameA; 1136 m_rbBFrame = frameB; 1137 buildJacobian(); 1138 //calculateTransforms(); 1139 } 1140 1141 1142 1143 1144
