- Timestamp:
- Mar 31, 2009, 8:05:51 PM (15 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
r2662 r2882 23 23 #include <new> 24 24 25 //----------------------------------------------------------------------------- 26 27 #define CONETWIST_USE_OBSOLETE_SOLVER false 28 #define CONETWIST_DEF_FIX_THRESH btScalar(.05f) 29 30 //----------------------------------------------------------------------------- 31 25 32 btConeTwistConstraint::btConeTwistConstraint() 26 :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE) 33 :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE), 34 m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER) 27 35 { 28 36 } … … 32 40 const btTransform& rbAFrame,const btTransform& rbBFrame) 33 41 :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame), 34 m_angularOnly(false) 35 { 36 m_swingSpan1 = btScalar(1e30); 37 m_swingSpan2 = btScalar(1e30); 38 m_twistSpan = btScalar(1e30); 39 m_biasFactor = 0.3f; 40 m_relaxationFactor = 1.0f; 41 42 m_angularOnly(false), 43 m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER) 44 { 45 init(); 46 } 47 48 btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame) 49 :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame), 50 m_angularOnly(false), 51 m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER) 52 { 53 m_rbBFrame = m_rbAFrame; 54 init(); 55 } 56 57 58 void btConeTwistConstraint::init() 59 { 60 m_angularOnly = false; 42 61 m_solveTwistLimit = false; 43 62 m_solveSwingLimit = false; 44 45 } 46 47 btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame) 48 :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame), 49 m_angularOnly(false) 50 { 51 m_rbBFrame = m_rbAFrame; 63 m_bMotorEnabled = false; 64 m_maxMotorImpulse = btScalar(-1); 65 66 setLimit(btScalar(1e30), btScalar(1e30), btScalar(1e30)); 67 m_damping = btScalar(0.01); 68 m_fixThresh = CONETWIST_DEF_FIX_THRESH; 69 } 70 71 72 //----------------------------------------------------------------------------- 73 74 void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info) 75 { 76 if (m_useSolveConstraintObsolete) 77 { 78 info->m_numConstraintRows = 0; 79 info->nub = 0; 80 } 81 else 82 { 83 info->m_numConstraintRows = 3; 84 info->nub = 3; 85 calcAngleInfo2(); 86 if(m_solveSwingLimit) 87 { 88 info->m_numConstraintRows++; 89 info->nub--; 90 if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) 91 { 92 info->m_numConstraintRows++; 93 info->nub--; 94 } 95 } 96 if(m_solveTwistLimit) 97 { 98 info->m_numConstraintRows++; 99 info->nub--; 100 } 101 } 102 } // btConeTwistConstraint::getInfo1() 52 103 53 m_swingSpan1 = btScalar(1e30); 54 m_swingSpan2 = btScalar(1e30); 55 m_twistSpan = btScalar(1e30); 56 m_biasFactor = 0.3f; 57 m_relaxationFactor = 1.0f; 58 59 m_solveTwistLimit = false; 60 m_solveSwingLimit = false; 104 //----------------------------------------------------------------------------- 105 106 void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info) 107 { 108 btAssert(!m_useSolveConstraintObsolete); 109 //retrieve matrices 110 btTransform body0_trans; 111 body0_trans = m_rbA.getCenterOfMassTransform(); 112 btTransform body1_trans; 113 body1_trans = m_rbB.getCenterOfMassTransform(); 114 // set jacobian 115 info->m_J1linearAxis[0] = 1; 116 info->m_J1linearAxis[info->rowskip+1] = 1; 117 info->m_J1linearAxis[2*info->rowskip+2] = 1; 118 btVector3 a1 = body0_trans.getBasis() * m_rbAFrame.getOrigin(); 119 { 120 btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); 121 btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip); 122 btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip); 123 btVector3 a1neg = -a1; 124 a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); 125 } 126 btVector3 a2 = body1_trans.getBasis() * m_rbBFrame.getOrigin(); 127 { 128 btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); 129 btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip); 130 btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip); 131 a2.getSkewSymmetricMatrix(angular0,angular1,angular2); 132 } 133 // set right hand side 134 btScalar k = info->fps * info->erp; 135 int j; 136 for (j=0; j<3; j++) 137 { 138 info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]); 139 info->m_lowerLimit[j*info->rowskip] = -SIMD_INFINITY; 140 info->m_upperLimit[j*info->rowskip] = SIMD_INFINITY; 141 } 142 int row = 3; 143 int srow = row * info->rowskip; 144 btVector3 ax1; 145 // angular limits 146 if(m_solveSwingLimit) 147 { 148 btScalar *J1 = info->m_J1angularAxis; 149 btScalar *J2 = info->m_J2angularAxis; 150 if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) 151 { 152 btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame; 153 btVector3 p = trA.getBasis().getColumn(1); 154 btVector3 q = trA.getBasis().getColumn(2); 155 int srow1 = srow + info->rowskip; 156 J1[srow+0] = p[0]; 157 J1[srow+1] = p[1]; 158 J1[srow+2] = p[2]; 159 J1[srow1+0] = q[0]; 160 J1[srow1+1] = q[1]; 161 J1[srow1+2] = q[2]; 162 J2[srow+0] = -p[0]; 163 J2[srow+1] = -p[1]; 164 J2[srow+2] = -p[2]; 165 J2[srow1+0] = -q[0]; 166 J2[srow1+1] = -q[1]; 167 J2[srow1+2] = -q[2]; 168 btScalar fact = info->fps * m_relaxationFactor; 169 info->m_constraintError[srow] = fact * m_swingAxis.dot(p); 170 info->m_constraintError[srow1] = fact * m_swingAxis.dot(q); 171 info->m_lowerLimit[srow] = -SIMD_INFINITY; 172 info->m_upperLimit[srow] = SIMD_INFINITY; 173 info->m_lowerLimit[srow1] = -SIMD_INFINITY; 174 info->m_upperLimit[srow1] = SIMD_INFINITY; 175 srow = srow1 + info->rowskip; 176 } 177 else 178 { 179 ax1 = m_swingAxis * m_relaxationFactor * m_relaxationFactor; 180 J1[srow+0] = ax1[0]; 181 J1[srow+1] = ax1[1]; 182 J1[srow+2] = ax1[2]; 183 J2[srow+0] = -ax1[0]; 184 J2[srow+1] = -ax1[1]; 185 J2[srow+2] = -ax1[2]; 186 btScalar k = info->fps * m_biasFactor; 187 188 info->m_constraintError[srow] = k * m_swingCorrection; 189 info->cfm[srow] = 0.0f; 190 // m_swingCorrection is always positive or 0 191 info->m_lowerLimit[srow] = 0; 192 info->m_upperLimit[srow] = SIMD_INFINITY; 193 srow += info->rowskip; 194 } 195 } 196 if(m_solveTwistLimit) 197 { 198 ax1 = m_twistAxis * m_relaxationFactor * m_relaxationFactor; 199 btScalar *J1 = info->m_J1angularAxis; 200 btScalar *J2 = info->m_J2angularAxis; 201 J1[srow+0] = ax1[0]; 202 J1[srow+1] = ax1[1]; 203 J1[srow+2] = ax1[2]; 204 J2[srow+0] = -ax1[0]; 205 J2[srow+1] = -ax1[1]; 206 J2[srow+2] = -ax1[2]; 207 btScalar k = info->fps * m_biasFactor; 208 info->m_constraintError[srow] = k * m_twistCorrection; 209 info->cfm[srow] = 0.0f; 210 if(m_twistSpan > 0.0f) 211 { 212 213 if(m_twistCorrection > 0.0f) 214 { 215 info->m_lowerLimit[srow] = 0; 216 info->m_upperLimit[srow] = SIMD_INFINITY; 217 } 218 else 219 { 220 info->m_lowerLimit[srow] = -SIMD_INFINITY; 221 info->m_upperLimit[srow] = 0; 222 } 223 } 224 else 225 { 226 info->m_lowerLimit[srow] = -SIMD_INFINITY; 227 info->m_upperLimit[srow] = SIMD_INFINITY; 228 } 229 srow += info->rowskip; 230 } 231 } 61 232 62 } 233 //----------------------------------------------------------------------------- 63 234 64 235 void btConeTwistConstraint::buildJacobian() 65 236 { 66 m_appliedImpulse = btScalar(0.); 67 68 //set bias, sign, clear accumulator 69 m_swingCorrection = btScalar(0.); 70 m_twistLimitSign = btScalar(0.); 71 m_solveTwistLimit = false; 72 m_solveSwingLimit = false; 73 m_accTwistLimitImpulse = btScalar(0.); 74 m_accSwingLimitImpulse = btScalar(0.); 75 76 if (!m_angularOnly) 77 { 78 btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); 79 btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); 80 btVector3 relPos = pivotBInW - pivotAInW; 81 82 btVector3 normal[3]; 83 if (relPos.length2() > SIMD_EPSILON) 84 { 85 normal[0] = relPos.normalized(); 86 } 87 else 88 { 89 normal[0].setValue(btScalar(1.0),0,0); 90 } 91 92 btPlaneSpace1(normal[0], normal[1], normal[2]); 93 94 for (int i=0;i<3;i++) 95 { 96 new (&m_jac[i]) btJacobianEntry( 237 if (m_useSolveConstraintObsolete) 238 { 239 m_appliedImpulse = btScalar(0.); 240 m_accTwistLimitImpulse = btScalar(0.); 241 m_accSwingLimitImpulse = btScalar(0.); 242 243 if (!m_angularOnly) 244 { 245 btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); 246 btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); 247 btVector3 relPos = pivotBInW - pivotAInW; 248 249 btVector3 normal[3]; 250 if (relPos.length2() > SIMD_EPSILON) 251 { 252 normal[0] = relPos.normalized(); 253 } 254 else 255 { 256 normal[0].setValue(btScalar(1.0),0,0); 257 } 258 259 btPlaneSpace1(normal[0], normal[1], normal[2]); 260 261 for (int i=0;i<3;i++) 262 { 263 new (&m_jac[i]) btJacobianEntry( 97 264 m_rbA.getCenterOfMassTransform().getBasis().transpose(), 98 265 m_rbB.getCenterOfMassTransform().getBasis().transpose(), … … 104 271 m_rbB.getInvInertiaDiagLocal(), 105 272 m_rbB.getInvMass()); 106 } 107 } 273 } 274 } 275 276 calcAngleInfo2(); 277 } 278 } 279 280 //----------------------------------------------------------------------------- 281 282 void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) 283 { 284 if (m_useSolveConstraintObsolete) 285 { 286 btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); 287 btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); 288 289 btScalar tau = btScalar(0.3); 290 291 //linear part 292 if (!m_angularOnly) 293 { 294 btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 295 btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); 296 297 btVector3 vel1; 298 bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1); 299 btVector3 vel2; 300 bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2); 301 btVector3 vel = vel1 - vel2; 302 303 for (int i=0;i<3;i++) 304 { 305 const btVector3& normal = m_jac[i].m_linearJointAxis; 306 btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); 307 308 btScalar rel_vel; 309 rel_vel = normal.dot(vel); 310 //positional error (zeroth order error) 311 btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal 312 btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv; 313 m_appliedImpulse += impulse; 314 315 btVector3 ftorqueAxis1 = rel_pos1.cross(normal); 316 btVector3 ftorqueAxis2 = rel_pos2.cross(normal); 317 bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse); 318 bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse); 319 320 } 321 } 322 323 // apply motor 324 if (m_bMotorEnabled) 325 { 326 // compute current and predicted transforms 327 btTransform trACur = m_rbA.getCenterOfMassTransform(); 328 btTransform trBCur = m_rbB.getCenterOfMassTransform(); 329 btVector3 omegaA; bodyA.getAngularVelocity(omegaA); 330 btVector3 omegaB; bodyB.getAngularVelocity(omegaB); 331 btTransform trAPred; trAPred.setIdentity(); 332 btVector3 zerovec(0,0,0); 333 btTransformUtil::integrateTransform( 334 trACur, zerovec, omegaA, timeStep, trAPred); 335 btTransform trBPred; trBPred.setIdentity(); 336 btTransformUtil::integrateTransform( 337 trBCur, zerovec, omegaB, timeStep, trBPred); 338 339 // compute desired transforms in world 340 btTransform trPose(m_qTarget); 341 btTransform trABDes = m_rbBFrame * trPose * m_rbAFrame.inverse(); 342 btTransform trADes = trBPred * trABDes; 343 btTransform trBDes = trAPred * trABDes.inverse(); 344 345 // compute desired omegas in world 346 btVector3 omegaADes, omegaBDes; 347 348 btTransformUtil::calculateVelocity(trACur, trADes, timeStep, zerovec, omegaADes); 349 btTransformUtil::calculateVelocity(trBCur, trBDes, timeStep, zerovec, omegaBDes); 350 351 // compute delta omegas 352 btVector3 dOmegaA = omegaADes - omegaA; 353 btVector3 dOmegaB = omegaBDes - omegaB; 354 355 // compute weighted avg axis of dOmega (weighting based on inertias) 356 btVector3 axisA, axisB; 357 btScalar kAxisAInv = 0, kAxisBInv = 0; 358 359 if (dOmegaA.length2() > SIMD_EPSILON) 360 { 361 axisA = dOmegaA.normalized(); 362 kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(axisA); 363 } 364 365 if (dOmegaB.length2() > SIMD_EPSILON) 366 { 367 axisB = dOmegaB.normalized(); 368 kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(axisB); 369 } 370 371 btVector3 avgAxis = kAxisAInv * axisA + kAxisBInv * axisB; 372 373 static bool bDoTorque = true; 374 if (bDoTorque && avgAxis.length2() > SIMD_EPSILON) 375 { 376 avgAxis.normalize(); 377 kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(avgAxis); 378 kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(avgAxis); 379 btScalar kInvCombined = kAxisAInv + kAxisBInv; 380 381 btVector3 impulse = (kAxisAInv * dOmegaA - kAxisBInv * dOmegaB) / 382 (kInvCombined * kInvCombined); 383 384 if (m_maxMotorImpulse >= 0) 385 { 386 btScalar fMaxImpulse = m_maxMotorImpulse; 387 if (m_bNormalizedMotorStrength) 388 fMaxImpulse = fMaxImpulse/kAxisAInv; 389 390 btVector3 newUnclampedAccImpulse = m_accMotorImpulse + impulse; 391 btScalar newUnclampedMag = newUnclampedAccImpulse.length(); 392 if (newUnclampedMag > fMaxImpulse) 393 { 394 newUnclampedAccImpulse.normalize(); 395 newUnclampedAccImpulse *= fMaxImpulse; 396 impulse = newUnclampedAccImpulse - m_accMotorImpulse; 397 } 398 m_accMotorImpulse += impulse; 399 } 400 401 btScalar impulseMag = impulse.length(); 402 btVector3 impulseAxis = impulse / impulseMag; 403 404 bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag); 405 bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag); 406 407 } 408 } 409 else // no motor: do a little damping 410 { 411 const btVector3& angVelA = getRigidBodyA().getAngularVelocity(); 412 const btVector3& angVelB = getRigidBodyB().getAngularVelocity(); 413 btVector3 relVel = angVelB - angVelA; 414 if (relVel.length2() > SIMD_EPSILON) 415 { 416 btVector3 relVelAxis = relVel.normalized(); 417 btScalar m_kDamping = btScalar(1.) / 418 (getRigidBodyA().computeAngularImpulseDenominator(relVelAxis) + 419 getRigidBodyB().computeAngularImpulseDenominator(relVelAxis)); 420 btVector3 impulse = m_damping * m_kDamping * relVel; 421 422 btScalar impulseMag = impulse.length(); 423 btVector3 impulseAxis = impulse / impulseMag; 424 bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag); 425 bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag); 426 } 427 } 428 429 // joint limits 430 { 431 ///solve angular part 432 btVector3 angVelA; 433 bodyA.getAngularVelocity(angVelA); 434 btVector3 angVelB; 435 bodyB.getAngularVelocity(angVelB); 436 437 // solve swing limit 438 if (m_solveSwingLimit) 439 { 440 btScalar amplitude = m_swingLimitRatio * m_swingCorrection*m_biasFactor/timeStep; 441 btScalar relSwingVel = (angVelB - angVelA).dot(m_swingAxis); 442 if (relSwingVel > 0) 443 amplitude += m_swingLimitRatio * relSwingVel * m_relaxationFactor; 444 btScalar impulseMag = amplitude * m_kSwing; 445 446 // Clamp the accumulated impulse 447 btScalar temp = m_accSwingLimitImpulse; 448 m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) ); 449 impulseMag = m_accSwingLimitImpulse - temp; 450 451 btVector3 impulse = m_swingAxis * impulseMag; 452 453 // don't let cone response affect twist 454 // (this can happen since body A's twist doesn't match body B's AND we use an elliptical cone limit) 455 { 456 btVector3 impulseTwistCouple = impulse.dot(m_twistAxisA) * m_twistAxisA; 457 btVector3 impulseNoTwistCouple = impulse - impulseTwistCouple; 458 impulse = impulseNoTwistCouple; 459 } 460 461 impulseMag = impulse.length(); 462 btVector3 noTwistSwingAxis = impulse / impulseMag; 463 464 bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag); 465 bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag); 466 } 467 468 469 // solve twist limit 470 if (m_solveTwistLimit) 471 { 472 btScalar amplitude = m_twistLimitRatio * m_twistCorrection*m_biasFactor/timeStep; 473 btScalar relTwistVel = (angVelB - angVelA).dot( m_twistAxis ); 474 if (relTwistVel > 0) // only damp when moving towards limit (m_twistAxis flipping is important) 475 amplitude += m_twistLimitRatio * relTwistVel * m_relaxationFactor; 476 btScalar impulseMag = amplitude * m_kTwist; 477 478 // Clamp the accumulated impulse 479 btScalar temp = m_accTwistLimitImpulse; 480 m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) ); 481 impulseMag = m_accTwistLimitImpulse - temp; 482 483 btVector3 impulse = m_twistAxis * impulseMag; 484 485 bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag); 486 bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag); 487 } 488 } 489 } 490 491 } 492 493 //----------------------------------------------------------------------------- 494 495 void btConeTwistConstraint::updateRHS(btScalar timeStep) 496 { 497 (void)timeStep; 498 499 } 500 501 //----------------------------------------------------------------------------- 502 503 void btConeTwistConstraint::calcAngleInfo() 504 { 505 m_swingCorrection = btScalar(0.); 506 m_twistLimitSign = btScalar(0.); 507 m_solveTwistLimit = false; 508 m_solveSwingLimit = false; 108 509 109 510 btVector3 b1Axis1,b1Axis2,b1Axis3; … … 123 524 { 124 525 b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1); 125 // swing1 = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) );126 526 swx = b2Axis1.dot(b1Axis1); 127 527 swy = b2Axis1.dot(b1Axis2); … … 130 530 fact = fact / (fact + btScalar(1.0)); 131 531 swing1 *= fact; 132 133 532 } 134 533 … … 136 535 { 137 536 b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2); 138 // swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) );139 537 swx = b2Axis1.dot(b1Axis1); 140 538 swy = b2Axis1.dot(b1Axis3); … … 153 551 m_swingCorrection = EllipseAngle-1.0f; 154 552 m_solveSwingLimit = true; 155 156 553 // Calculate necessary axis & factors 157 554 m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3)); 158 555 m_swingAxis.normalize(); 159 160 556 btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f; 161 557 m_swingAxis *= swingAxisSign; 162 163 m_kSwing = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) +164 getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis));165 166 558 } 167 559 … … 173 565 btVector3 TwistRef = quatRotate(rotationArc,b2Axis2); 174 566 btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) ); 175 176 btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.); 567 m_twistAngle = twist; 568 569 // btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.); 570 btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? btScalar(1.0f) : btScalar(0.); 177 571 if (twist <= -m_twistSpan*lockedFreeFactor) 178 572 { 179 573 m_twistCorrection = -(twist + m_twistSpan); 180 574 m_solveTwistLimit = true; 181 182 575 m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; 183 576 m_twistAxis.normalize(); 184 577 m_twistAxis *= -1.0f; 185 186 m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) + 187 getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis)); 188 189 } else 190 if (twist > m_twistSpan*lockedFreeFactor) 191 { 192 m_twistCorrection = (twist - m_twistSpan); 578 } 579 else if (twist > m_twistSpan*lockedFreeFactor) 580 { 581 m_twistCorrection = (twist - m_twistSpan); 582 m_solveTwistLimit = true; 583 m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; 584 m_twistAxis.normalize(); 585 } 586 } 587 } // btConeTwistConstraint::calcAngleInfo() 588 589 590 static btVector3 vTwist(1,0,0); // twist axis in constraint's space 591 592 //----------------------------------------------------------------------------- 593 594 void btConeTwistConstraint::calcAngleInfo2() 595 { 596 m_swingCorrection = btScalar(0.); 597 m_twistLimitSign = btScalar(0.); 598 m_solveTwistLimit = false; 599 m_solveSwingLimit = false; 600 601 { 602 // compute rotation of A wrt B (in constraint space) 603 btQuaternion qA = getRigidBodyA().getCenterOfMassTransform().getRotation() * m_rbAFrame.getRotation(); 604 btQuaternion qB = getRigidBodyB().getCenterOfMassTransform().getRotation() * m_rbBFrame.getRotation(); 605 btQuaternion qAB = qB.inverse() * qA; 606 607 // split rotation into cone and twist 608 // (all this is done from B's perspective. Maybe I should be averaging axes...) 609 btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize(); 610 btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize(); 611 btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize(); 612 613 if (m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh) 614 { 615 btScalar swingAngle, swingLimit = 0; btVector3 swingAxis; 616 computeConeLimitInfo(qABCone, swingAngle, swingAxis, swingLimit); 617 618 if (swingAngle > swingLimit * m_limitSoftness) 619 { 620 m_solveSwingLimit = true; 621 622 // compute limit ratio: 0->1, where 623 // 0 == beginning of soft limit 624 // 1 == hard/real limit 625 m_swingLimitRatio = 1.f; 626 if (swingAngle < swingLimit && m_limitSoftness < 1.f - SIMD_EPSILON) 627 { 628 m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness)/ 629 (swingLimit - swingLimit * m_limitSoftness); 630 } 631 632 // swing correction tries to get back to soft limit 633 m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness); 634 635 // adjustment of swing axis (based on ellipse normal) 636 adjustSwingAxisToUseEllipseNormal(swingAxis); 637 638 // Calculate necessary axis & factors 639 m_swingAxis = quatRotate(qB, -swingAxis); 640 641 m_twistAxisA.setValue(0,0,0); 642 643 m_kSwing = btScalar(1.) / 644 (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) + 645 getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis)); 646 } 647 } 648 else 649 { 650 // you haven't set any limits; 651 // or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?) 652 // anyway, we have either hinge or fixed joint 653 btVector3 ivA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0); 654 btVector3 jvA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1); 655 btVector3 kvA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); 656 btVector3 ivB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(0); 657 btVector3 target; 658 btScalar x = ivB.dot(ivA); 659 btScalar y = ivB.dot(jvA); 660 btScalar z = ivB.dot(kvA); 661 if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh)) 662 { // fixed. We'll need to add one more row to constraint 663 if((!btFuzzyZero(y)) || (!(btFuzzyZero(z)))) 664 { 665 m_solveSwingLimit = true; 666 m_swingAxis = -ivB.cross(ivA); 667 } 668 } 669 else 670 { 671 if(m_swingSpan1 < m_fixThresh) 672 { // hinge around Y axis 673 if(!(btFuzzyZero(y))) 674 { 675 m_solveSwingLimit = true; 676 if(m_swingSpan2 >= m_fixThresh) 677 { 678 y = btScalar(0.f); 679 btScalar span2 = btAtan2(z, x); 680 if(span2 > m_swingSpan2) 681 { 682 x = btCos(m_swingSpan2); 683 z = btSin(m_swingSpan2); 684 } 685 else if(span2 < -m_swingSpan2) 686 { 687 x = btCos(m_swingSpan2); 688 z = -btSin(m_swingSpan2); 689 } 690 } 691 } 692 } 693 else 694 { // hinge around Z axis 695 if(!btFuzzyZero(z)) 696 { 697 m_solveSwingLimit = true; 698 if(m_swingSpan1 >= m_fixThresh) 699 { 700 z = btScalar(0.f); 701 btScalar span1 = btAtan2(y, x); 702 if(span1 > m_swingSpan1) 703 { 704 x = btCos(m_swingSpan1); 705 y = btSin(m_swingSpan1); 706 } 707 else if(span1 < -m_swingSpan1) 708 { 709 x = btCos(m_swingSpan1); 710 y = -btSin(m_swingSpan1); 711 } 712 } 713 } 714 } 715 target[0] = x * ivA[0] + y * jvA[0] + z * kvA[0]; 716 target[1] = x * ivA[1] + y * jvA[1] + z * kvA[1]; 717 target[2] = x * ivA[2] + y * jvA[2] + z * kvA[2]; 718 target.normalize(); 719 m_swingAxis = -ivB.cross(target); 720 m_swingCorrection = m_swingAxis.length(); 721 m_swingAxis.normalize(); 722 } 723 } 724 725 if (m_twistSpan >= btScalar(0.f)) 726 { 727 btVector3 twistAxis; 728 computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis); 729 730 if (m_twistAngle > m_twistSpan*m_limitSoftness) 731 { 193 732 m_solveTwistLimit = true; 194 733 195 m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f; 196 m_twistAxis.normalize(); 197 198 m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) + 199 getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis)); 200 201 } 202 } 203 } 204 205 void btConeTwistConstraint::solveConstraint(btScalar timeStep) 206 { 207 208 btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); 209 btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); 210 211 btScalar tau = btScalar(0.3); 212 213 //linear part 214 if (!m_angularOnly) 215 { 216 btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 217 btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); 218 219 btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1); 220 btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2); 221 btVector3 vel = vel1 - vel2; 222 223 for (int i=0;i<3;i++) 224 { 225 const btVector3& normal = m_jac[i].m_linearJointAxis; 226 btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); 227 228 btScalar rel_vel; 229 rel_vel = normal.dot(vel); 230 //positional error (zeroth order error) 231 btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal 232 btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv; 233 m_appliedImpulse += impulse; 234 btVector3 impulse_vector = normal * impulse; 235 m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition()); 236 m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition()); 237 } 238 } 239 240 { 241 ///solve angular part 242 const btVector3& angVelA = getRigidBodyA().getAngularVelocity(); 243 const btVector3& angVelB = getRigidBodyB().getAngularVelocity(); 244 245 // solve swing limit 246 if (m_solveSwingLimit) 247 { 248 btScalar amplitude = ((angVelB - angVelA).dot( m_swingAxis )*m_relaxationFactor*m_relaxationFactor + m_swingCorrection*(btScalar(1.)/timeStep)*m_biasFactor); 249 btScalar impulseMag = amplitude * m_kSwing; 250 251 // Clamp the accumulated impulse 252 btScalar temp = m_accSwingLimitImpulse; 253 m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) ); 254 impulseMag = m_accSwingLimitImpulse - temp; 255 256 btVector3 impulse = m_swingAxis * impulseMag; 257 258 m_rbA.applyTorqueImpulse(impulse); 259 m_rbB.applyTorqueImpulse(-impulse); 260 261 } 262 263 // solve twist limit 264 if (m_solveTwistLimit) 265 { 266 btScalar amplitude = ((angVelB - angVelA).dot( m_twistAxis )*m_relaxationFactor*m_relaxationFactor + m_twistCorrection*(btScalar(1.)/timeStep)*m_biasFactor ); 267 btScalar impulseMag = amplitude * m_kTwist; 268 269 // Clamp the accumulated impulse 270 btScalar temp = m_accTwistLimitImpulse; 271 m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) ); 272 impulseMag = m_accTwistLimitImpulse - temp; 273 274 btVector3 impulse = m_twistAxis * impulseMag; 275 276 m_rbA.applyTorqueImpulse(impulse); 277 m_rbB.applyTorqueImpulse(-impulse); 278 279 } 280 281 } 282 283 } 284 285 void btConeTwistConstraint::updateRHS(btScalar timeStep) 286 { 287 (void)timeStep; 288 289 } 734 m_twistLimitRatio = 1.f; 735 if (m_twistAngle < m_twistSpan && m_limitSoftness < 1.f - SIMD_EPSILON) 736 { 737 m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness)/ 738 (m_twistSpan - m_twistSpan * m_limitSoftness); 739 } 740 741 // twist correction tries to get back to soft limit 742 m_twistCorrection = m_twistAngle - (m_twistSpan * m_limitSoftness); 743 744 m_twistAxis = quatRotate(qB, -twistAxis); 745 746 m_kTwist = btScalar(1.) / 747 (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) + 748 getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis)); 749 } 750 751 if (m_solveSwingLimit) 752 m_twistAxisA = quatRotate(qA, -twistAxis); 753 } 754 else 755 { 756 m_twistAngle = btScalar(0.f); 757 } 758 } 759 } // btConeTwistConstraint::calcAngleInfo2() 760 761 762 763 // given a cone rotation in constraint space, (pre: twist must already be removed) 764 // this method computes its corresponding swing angle and axis. 765 // more interestingly, it computes the cone/swing limit (angle) for this cone "pose". 766 void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone, 767 btScalar& swingAngle, // out 768 btVector3& vSwingAxis, // out 769 btScalar& swingLimit) // out 770 { 771 swingAngle = qCone.getAngle(); 772 if (swingAngle > SIMD_EPSILON) 773 { 774 vSwingAxis = btVector3(qCone.x(), qCone.y(), qCone.z()); 775 vSwingAxis.normalize(); 776 if (fabs(vSwingAxis.x()) > SIMD_EPSILON) 777 { 778 // non-zero twist?! this should never happen. 779 int wtf = 0; wtf = wtf; 780 } 781 782 // Compute limit for given swing. tricky: 783 // Given a swing axis, we're looking for the intersection with the bounding cone ellipse. 784 // (Since we're dealing with angles, this ellipse is embedded on the surface of a sphere.) 785 786 // For starters, compute the direction from center to surface of ellipse. 787 // This is just the perpendicular (ie. rotate 2D vector by PI/2) of the swing axis. 788 // (vSwingAxis is the cone rotation (in z,y); change vars and rotate to (x,y) coords.) 789 btScalar xEllipse = vSwingAxis.y(); 790 btScalar yEllipse = -vSwingAxis.z(); 791 792 // Now, we use the slope of the vector (using x/yEllipse) and find the length 793 // of the line that intersects the ellipse: 794 // x^2 y^2 795 // --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits) 796 // a^2 b^2 797 // Do the math and it should be clear. 798 799 swingLimit = m_swingSpan1; // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1 800 if (fabs(xEllipse) > SIMD_EPSILON) 801 { 802 btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse); 803 btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2); 804 norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1); 805 btScalar swingLimit2 = (1 + surfaceSlope2) / norm; 806 swingLimit = sqrt(swingLimit2); 807 } 808 809 // test! 810 /*swingLimit = m_swingSpan2; 811 if (fabs(vSwingAxis.z()) > SIMD_EPSILON) 812 { 813 btScalar mag_2 = m_swingSpan1*m_swingSpan1 + m_swingSpan2*m_swingSpan2; 814 btScalar sinphi = m_swingSpan2 / sqrt(mag_2); 815 btScalar phi = asin(sinphi); 816 btScalar theta = atan2(fabs(vSwingAxis.y()),fabs(vSwingAxis.z())); 817 btScalar alpha = 3.14159f - theta - phi; 818 btScalar sinalpha = sin(alpha); 819 swingLimit = m_swingSpan1 * sinphi/sinalpha; 820 }*/ 821 } 822 else if (swingAngle < 0) 823 { 824 // this should never happen! 825 int wtf = 0; wtf = wtf; 826 } 827 } 828 829 btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const 830 { 831 // compute x/y in ellipse using cone angle (0 -> 2*PI along surface of cone) 832 btScalar xEllipse = btCos(fAngleInRadians); 833 btScalar yEllipse = btSin(fAngleInRadians); 834 835 // Use the slope of the vector (using x/yEllipse) and find the length 836 // of the line that intersects the ellipse: 837 // x^2 y^2 838 // --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits) 839 // a^2 b^2 840 // Do the math and it should be clear. 841 842 float swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1) 843 if (fabs(xEllipse) > SIMD_EPSILON) 844 { 845 btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse); 846 btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2); 847 norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1); 848 btScalar swingLimit2 = (1 + surfaceSlope2) / norm; 849 swingLimit = sqrt(swingLimit2); 850 } 851 852 // convert into point in constraint space: 853 // note: twist is x-axis, swing 1 and 2 are along the z and y axes respectively 854 btVector3 vSwingAxis(0, xEllipse, -yEllipse); 855 btQuaternion qSwing(vSwingAxis, swingLimit); 856 btVector3 vPointInConstraintSpace(fLength,0,0); 857 return quatRotate(qSwing, vPointInConstraintSpace); 858 } 859 860 // given a twist rotation in constraint space, (pre: cone must already be removed) 861 // this method computes its corresponding angle and axis. 862 void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist, 863 btScalar& twistAngle, // out 864 btVector3& vTwistAxis) // out 865 { 866 btQuaternion qMinTwist = qTwist; 867 twistAngle = qTwist.getAngle(); 868 869 if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate. 870 { 871 qMinTwist = operator-(qTwist); 872 twistAngle = qMinTwist.getAngle(); 873 } 874 if (twistAngle < 0) 875 { 876 // this should never happen 877 int wtf = 0; wtf = wtf; 878 } 879 880 vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z()); 881 if (twistAngle > SIMD_EPSILON) 882 vTwistAxis.normalize(); 883 } 884 885 886 void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const 887 { 888 // the swing axis is computed as the "twist-free" cone rotation, 889 // but the cone limit is not circular, but elliptical (if swingspan1 != swingspan2). 890 // so, if we're outside the limits, the closest way back inside the cone isn't 891 // along the vector back to the center. better (and more stable) to use the ellipse normal. 892 893 // convert swing axis to direction from center to surface of ellipse 894 // (ie. rotate 2D vector by PI/2) 895 btScalar y = -vSwingAxis.z(); 896 btScalar z = vSwingAxis.y(); 897 898 // do the math... 899 if (fabs(z) > SIMD_EPSILON) // avoid division by 0. and we don't need an update if z == 0. 900 { 901 // compute gradient/normal of ellipse surface at current "point" 902 btScalar grad = y/z; 903 grad *= m_swingSpan2 / m_swingSpan1; 904 905 // adjust y/z to represent normal at point (instead of vector to point) 906 if (y > 0) 907 y = fabs(grad * z); 908 else 909 y = -fabs(grad * z); 910 911 // convert ellipse direction back to swing axis 912 vSwingAxis.setZ(-y); 913 vSwingAxis.setY( z); 914 vSwingAxis.normalize(); 915 } 916 } 917 918 919 920 void btConeTwistConstraint::setMotorTarget(const btQuaternion &q) 921 { 922 btTransform trACur = m_rbA.getCenterOfMassTransform(); 923 btTransform trBCur = m_rbB.getCenterOfMassTransform(); 924 btTransform trABCur = trBCur.inverse() * trACur; 925 btQuaternion qABCur = trABCur.getRotation(); 926 btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame); 927 btQuaternion qConstraintCur = trConstraintCur.getRotation(); 928 929 btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * q * m_rbAFrame.getRotation(); 930 setMotorTargetInConstraintSpace(qConstraint); 931 } 932 933 934 void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &q) 935 { 936 m_qTarget = q; 937 938 // clamp motor target to within limits 939 { 940 btScalar softness = 1.f;//m_limitSoftness; 941 942 // split into twist and cone 943 btVector3 vTwisted = quatRotate(m_qTarget, vTwist); 944 btQuaternion qTargetCone = shortestArcQuat(vTwist, vTwisted); qTargetCone.normalize(); 945 btQuaternion qTargetTwist = qTargetCone.inverse() * m_qTarget; qTargetTwist.normalize(); 946 947 // clamp cone 948 if (m_swingSpan1 >= btScalar(0.05f) && m_swingSpan2 >= btScalar(0.05f)) 949 { 950 btScalar swingAngle, swingLimit; btVector3 swingAxis; 951 computeConeLimitInfo(qTargetCone, swingAngle, swingAxis, swingLimit); 952 953 if (fabs(swingAngle) > SIMD_EPSILON) 954 { 955 if (swingAngle > swingLimit*softness) 956 swingAngle = swingLimit*softness; 957 else if (swingAngle < -swingLimit*softness) 958 swingAngle = -swingLimit*softness; 959 qTargetCone = btQuaternion(swingAxis, swingAngle); 960 } 961 } 962 963 // clamp twist 964 if (m_twistSpan >= btScalar(0.05f)) 965 { 966 btScalar twistAngle; btVector3 twistAxis; 967 computeTwistLimitInfo(qTargetTwist, twistAngle, twistAxis); 968 969 if (fabs(twistAngle) > SIMD_EPSILON) 970 { 971 // eddy todo: limitSoftness used here??? 972 if (twistAngle > m_twistSpan*softness) 973 twistAngle = m_twistSpan*softness; 974 else if (twistAngle < -m_twistSpan*softness) 975 twistAngle = -m_twistSpan*softness; 976 qTargetTwist = btQuaternion(twistAxis, twistAngle); 977 } 978 } 979 980 m_qTarget = qTargetCone * qTargetTwist; 981 } 982 } 983 984 985 //----------------------------------------------------------------------------- 986 //----------------------------------------------------------------------------- 987 //----------------------------------------------------------------------------- 988 989
Note: See TracChangeset
for help on using the changeset viewer.
