1 /* 2 Bullet Continuous Collision Detection and Physics Library 3 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ 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 16 17 #include "btHingeConstraint.h" 18 #include "BulletDynamics/Dynamics/btRigidBody.h" 19 #include "LinearMath/btTransformUtil.h" 20 #include "LinearMath/btMinMax.h" 21 #include <new> 22 #include "btSolverBody.h" 23 24 25 26 //#define HINGE_USE_OBSOLETE_SOLVER false 27 #define HINGE_USE_OBSOLETE_SOLVER false 28 29 #define HINGE_USE_FRAME_OFFSET true 30 31 #ifndef __SPU__ 32 33 34 35 36 37 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, 38 const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA) 39 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB), 40 #ifdef _BT_USE_CENTER_LIMIT_ 41 m_limit(), 42 #endif 43 m_angularOnly(false), 44 m_enableAngularMotor(false), 45 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), 46 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), 47 m_useReferenceFrameA(useReferenceFrameA), 48 m_flags(0), 49 m_normalCFM(0), 50 m_normalERP(0), 51 m_stopCFM(0), 52 m_stopERP(0) 53 { 54 m_rbAFrame.getOrigin() = pivotInA; 55 56 // since no frame is given, assume this to be zero angle and just pick rb transform axis 57 btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0); 58 59 btVector3 rbAxisA2; 60 btScalar projection = axisInA.dot(rbAxisA1); 61 if (projection >= 1.0f - SIMD_EPSILON) { 62 rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2); 63 rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); 64 } else if (projection <= -1.0f + SIMD_EPSILON) { 65 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2); 66 rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1); 67 } else { 68 rbAxisA2 = axisInA.cross(rbAxisA1); 69 rbAxisA1 = rbAxisA2.cross(axisInA); 70 } 71 72 m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), 73 rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), 74 rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); 75 76 btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); 77 btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); 78 btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); 79 80 m_rbBFrame.getOrigin() = pivotInB; 81 m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), 82 rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), 83 rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); 84 85 #ifndef _BT_USE_CENTER_LIMIT_ 86 //start with free 87 m_lowerLimit = btScalar(1.0f); 88 m_upperLimit = btScalar(-1.0f); 89 m_biasFactor = 0.3f; 90 m_relaxationFactor = 1.0f; 91 m_limitSoftness = 0.9f; 92 m_solveLimit = false; 93 #endif 94 m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); 95 } 96 97 98 99 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA) 100 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), 101 #ifdef _BT_USE_CENTER_LIMIT_ 102 m_limit(), 103 #endif 104 m_angularOnly(false), m_enableAngularMotor(false), 105 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), 106 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), 107 m_useReferenceFrameA(useReferenceFrameA), 108 m_flags(0), 109 m_normalCFM(0), 110 m_normalERP(0), 111 m_stopCFM(0), 112 m_stopERP(0) 113 { 114 115 // since no frame is given, assume this to be zero angle and just pick rb transform axis 116 // fixed axis in worldspace 117 btVector3 rbAxisA1, rbAxisA2; 118 btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2); 119 120 m_rbAFrame.getOrigin() = pivotInA; 121 m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(), 122 rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(), 123 rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() ); 124 125 btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA; 126 127 btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB); 128 btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1); 129 btVector3 rbAxisB2 = axisInB.cross(rbAxisB1); 130 131 132 m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA); 133 m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(), 134 rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(), 135 rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() ); 136 137 #ifndef _BT_USE_CENTER_LIMIT_ 138 //start with free 139 m_lowerLimit = btScalar(1.0f); 140 m_upperLimit = btScalar(-1.0f); 141 m_biasFactor = 0.3f; 142 m_relaxationFactor = 1.0f; 143 m_limitSoftness = 0.9f; 144 m_solveLimit = false; 145 #endif 146 m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); 147 } 148 149 150 151 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, 152 const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA) 153 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame), 154 #ifdef _BT_USE_CENTER_LIMIT_ 155 m_limit(), 156 #endif 157 m_angularOnly(false), 158 m_enableAngularMotor(false), 159 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), 160 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), 161 m_useReferenceFrameA(useReferenceFrameA), 162 m_flags(0), 163 m_normalCFM(0), 164 m_normalERP(0), 165 m_stopCFM(0), 166 m_stopERP(0) 167 { 168 #ifndef _BT_USE_CENTER_LIMIT_ 169 //start with free 170 m_lowerLimit = btScalar(1.0f); 171 m_upperLimit = btScalar(-1.0f); 172 m_biasFactor = 0.3f; 173 m_relaxationFactor = 1.0f; 174 m_limitSoftness = 0.9f; 175 m_solveLimit = false; 176 #endif 177 m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); 178 } 179 180 181 182 btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA) 183 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame), 184 #ifdef _BT_USE_CENTER_LIMIT_ 185 m_limit(), 186 #endif 187 m_angularOnly(false), 188 m_enableAngularMotor(false), 189 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), 190 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), 191 m_useReferenceFrameA(useReferenceFrameA), 192 m_flags(0), 193 m_normalCFM(0), 194 m_normalERP(0), 195 m_stopCFM(0), 196 m_stopERP(0) 197 { 198 ///not providing rigidbody B means implicitly using worldspace for body B 199 200 m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin()); 201 #ifndef _BT_USE_CENTER_LIMIT_ 202 //start with free 203 m_lowerLimit = btScalar(1.0f); 204 m_upperLimit = btScalar(-1.0f); 205 m_biasFactor = 0.3f; 206 m_relaxationFactor = 1.0f; 207 m_limitSoftness = 0.9f; 208 m_solveLimit = false; 209 #endif 210 m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); 211 } 212 213 214 215 void btHingeConstraint::buildJacobian() 216 { 217 if (m_useSolveConstraintObsolete) 218 { 219 m_appliedImpulse = btScalar(0.); 220 m_accMotorImpulse = btScalar(0.); 221 222 if (!m_angularOnly) 223 { 224 btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); 225 btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); 226 btVector3 relPos = pivotBInW - pivotAInW; 227 228 btVector3 normal[3]; 229 if (relPos.length2() > SIMD_EPSILON) 230 { 231 normal[0] = relPos.normalized(); 232 } 233 else 234 { 235 normal[0].setValue(btScalar(1.0),0,0); 236 } 237 238 btPlaneSpace1(normal[0], normal[1], normal[2]); 239 240 for (int i=0;i<3;i++) 241 { 242 new (&m_jac[i]) btJacobianEntry( 243 m_rbA.getCenterOfMassTransform().getBasis().transpose(), 244 m_rbB.getCenterOfMassTransform().getBasis().transpose(), 245 pivotAInW - m_rbA.getCenterOfMassPosition(), 246 pivotBInW - m_rbB.getCenterOfMassPosition(), 247 normal[i], 248 m_rbA.getInvInertiaDiagLocal(), 249 m_rbA.getInvMass(), 250 m_rbB.getInvInertiaDiagLocal(), 251 m_rbB.getInvMass()); 252 } 253 } 254 255 //calculate two perpendicular jointAxis, orthogonal to hingeAxis 256 //these two jointAxis require equal angular velocities for both bodies 257 258 //this is unused for now, it's a todo 259 btVector3 jointAxis0local; 260 btVector3 jointAxis1local; 261 262 btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local); 263 264 btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local; 265 btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local; 266 btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); 267 268 new (&m_jacAng[0]) btJacobianEntry(jointAxis0, 269 m_rbA.getCenterOfMassTransform().getBasis().transpose(), 270 m_rbB.getCenterOfMassTransform().getBasis().transpose(), 271 m_rbA.getInvInertiaDiagLocal(), 272 m_rbB.getInvInertiaDiagLocal()); 273 274 new (&m_jacAng[1]) btJacobianEntry(jointAxis1, 275 m_rbA.getCenterOfMassTransform().getBasis().transpose(), 276 m_rbB.getCenterOfMassTransform().getBasis().transpose(), 277 m_rbA.getInvInertiaDiagLocal(), 278 m_rbB.getInvInertiaDiagLocal()); 279 280 new (&m_jacAng[2]) btJacobianEntry(hingeAxisWorld, 281 m_rbA.getCenterOfMassTransform().getBasis().transpose(), 282 m_rbB.getCenterOfMassTransform().getBasis().transpose(), 283 m_rbA.getInvInertiaDiagLocal(), 284 m_rbB.getInvInertiaDiagLocal()); 285 286 // clear accumulator 287 m_accLimitImpulse = btScalar(0.); 288 289 // test angular limit 290 testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); 291 292 //Compute K = J*W*J' for hinge axis 293 btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); 294 m_kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) + 295 getRigidBodyB().computeAngularImpulseDenominator(axisA)); 296 297 } 298 } 299 300 301 #endif //__SPU__ 302 303 304 static inline btScalar btNormalizeAnglePositive(btScalar angle) 305 { 306 return btFmod(btFmod(angle, btScalar(2.0*SIMD_PI)) + btScalar(2.0*SIMD_PI), btScalar(2.0*SIMD_PI)); 307 } 308 309 310 311 static btScalar btShortestAngularDistance(btScalar accAngle, btScalar curAngle) 312 { 313 btScalar result = btNormalizeAngle(btNormalizeAnglePositive(btNormalizeAnglePositive(curAngle) - 314 btNormalizeAnglePositive(accAngle))); 315 return result; 316 } 317 318 static btScalar btShortestAngleUpdate(btScalar accAngle, btScalar curAngle) 319 { 320 btScalar tol(0.3); 321 btScalar result = btShortestAngularDistance(accAngle, curAngle); 322 323 if (btFabs(result) > tol) 324 return curAngle; 325 else 326 return accAngle + result; 327 328 return curAngle; 329 } 330 331 332 btScalar btHingeAccumulatedAngleConstraint::getAccumulatedHingeAngle() 333 { 334 btScalar hingeAngle = getHingeAngle(); 335 m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle,hingeAngle); 336 return m_accumulatedAngle; 337 } 338 void btHingeAccumulatedAngleConstraint::setAccumulatedHingeAngle(btScalar accAngle) 339 { 340 m_accumulatedAngle = accAngle; 341 } 342 343 void btHingeAccumulatedAngleConstraint::getInfo1(btConstraintInfo1* info) 344 { 345 //update m_accumulatedAngle 346 btScalar curHingeAngle = getHingeAngle(); 347 m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle,curHingeAngle); 348 349 btHingeConstraint::getInfo1(info); 350 351 } 352 353 354 void btHingeConstraint::getInfo1(btConstraintInfo1* info) 355 { 356 357 358 if (m_useSolveConstraintObsolete) 359 { 360 info->m_numConstraintRows = 0; 361 info->nub = 0; 362 } 363 else 364 { 365 info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular 366 info->nub = 1; 367 //always add the row, to avoid computation (data is not available yet) 368 //prepare constraint 369 testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); 370 if(getSolveLimit() || getEnableAngularMotor()) 371 { 372 info->m_numConstraintRows++; // limit 3rd anguar as well 373 info->nub--; 374 } 375 376 } 377 } 378 379 void btHingeConstraint::getInfo1NonVirtual(btConstraintInfo1* info) 380 { 381 if (m_useSolveConstraintObsolete) 382 { 383 info->m_numConstraintRows = 0; 384 info->nub = 0; 385 } 386 else 387 { 388 //always add the 'limit' row, to avoid computation (data is not available yet) 389 info->m_numConstraintRows = 6; // Fixed 3 linear + 2 angular 390 info->nub = 0; 391 } 392 } 393 394 void btHingeConstraint::getInfo2 (btConstraintInfo2* info) 395 { 396 if(m_useOffsetForConstraintFrame) 397 { 398 getInfo2InternalUsingFrameOffset(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity()); 399 } 400 else 401 { 402 getInfo2Internal(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity()); 403 } 404 } 405 406 407 void btHingeConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB) 408 { 409 ///the regular (virtual) implementation getInfo2 already performs 'testLimit' during getInfo1, so we need to do it now 410 testLimit(transA,transB); 411 412 getInfo2Internal(info,transA,transB,angVelA,angVelB); 413 } 414 415 416 void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB) 417 { 418 419 btAssert(!m_useSolveConstraintObsolete); 420 int i, skip = info->rowskip; 421 // transforms in world space 422 btTransform trA = transA*m_rbAFrame; 423 btTransform trB = transB*m_rbBFrame; 424 // pivot point 425 btVector3 pivotAInW = trA.getOrigin(); 426 btVector3 pivotBInW = trB.getOrigin(); 427 #if 0 428 if (0) 429 { 430 for (i=0;i<6;i++) 431 { 432 info->m_J1linearAxis[i*skip]=0; 433 info->m_J1linearAxis[i*skip+1]=0; 434 info->m_J1linearAxis[i*skip+2]=0; 435 436 info->m_J1angularAxis[i*skip]=0; 437 info->m_J1angularAxis[i*skip+1]=0; 438 info->m_J1angularAxis[i*skip+2]=0; 439 440 info->m_J2linearAxis[i*skip]=0; 441 info->m_J2linearAxis[i*skip+1]=0; 442 info->m_J2linearAxis[i*skip+2]=0; 443 444 info->m_J2angularAxis[i*skip]=0; 445 info->m_J2angularAxis[i*skip+1]=0; 446 info->m_J2angularAxis[i*skip+2]=0; 447 448 info->m_constraintError[i*skip]=0.f; 449 } 450 } 451 #endif //#if 0 452 // linear (all fixed) 453 454 if (!m_angularOnly) 455 { 456 info->m_J1linearAxis[0] = 1; 457 info->m_J1linearAxis[skip + 1] = 1; 458 info->m_J1linearAxis[2 * skip + 2] = 1; 459 460 info->m_J2linearAxis[0] = -1; 461 info->m_J2linearAxis[skip + 1] = -1; 462 info->m_J2linearAxis[2 * skip + 2] = -1; 463 } 464 465 466 467 468 btVector3 a1 = pivotAInW - transA.getOrigin(); 469 { 470 btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); 471 btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + skip); 472 btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * skip); 473 btVector3 a1neg = -a1; 474 a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); 475 } 476 btVector3 a2 = pivotBInW - transB.getOrigin(); 477 { 478 btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); 479 btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + skip); 480 btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * skip); 481 a2.getSkewSymmetricMatrix(angular0,angular1,angular2); 482 } 483 // linear RHS 484 btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM) ? m_normalERP : info->erp; 485 486 btScalar k = info->fps * normalErp; 487 if (!m_angularOnly) 488 { 489 for(i = 0; i < 3; i++) 490 { 491 info->m_constraintError[i * skip] = k * (pivotBInW[i] - pivotAInW[i]); 492 } 493 } 494 // make rotations around X and Y equal 495 // the hinge axis should be the only unconstrained 496 // rotational axis, the angular velocity of the two bodies perpendicular to 497 // the hinge axis should be equal. thus the constraint equations are 498 // p*w1 - p*w2 = 0 499 // q*w1 - q*w2 = 0 500 // where p and q are unit vectors normal to the hinge axis, and w1 and w2 501 // are the angular velocity vectors of the two bodies. 502 // get hinge axis (Z) 503 btVector3 ax1 = trA.getBasis().getColumn(2); 504 // get 2 orthos to hinge axis (X, Y) 505 btVector3 p = trA.getBasis().getColumn(0); 506 btVector3 q = trA.getBasis().getColumn(1); 507 // set the two hinge angular rows 508 int s3 = 3 * info->rowskip; 509 int s4 = 4 * info->rowskip; 510 511 info->m_J1angularAxis[s3 + 0] = p[0]; 512 info->m_J1angularAxis[s3 + 1] = p[1]; 513 info->m_J1angularAxis[s3 + 2] = p[2]; 514 info->m_J1angularAxis[s4 + 0] = q[0]; 515 info->m_J1angularAxis[s4 + 1] = q[1]; 516 info->m_J1angularAxis[s4 + 2] = q[2]; 517 518 info->m_J2angularAxis[s3 + 0] = -p[0]; 519 info->m_J2angularAxis[s3 + 1] = -p[1]; 520 info->m_J2angularAxis[s3 + 2] = -p[2]; 521 info->m_J2angularAxis[s4 + 0] = -q[0]; 522 info->m_J2angularAxis[s4 + 1] = -q[1]; 523 info->m_J2angularAxis[s4 + 2] = -q[2]; 524 // compute the right hand side of the constraint equation. set relative 525 // body velocities along p and q to bring the hinge back into alignment. 526 // if ax1,ax2 are the unit length hinge axes as computed from body1 and 527 // body2, we need to rotate both bodies along the axis u = (ax1 x ax2). 528 // if `theta' is the angle between ax1 and ax2, we need an angular velocity 529 // along u to cover angle erp*theta in one step : 530 // |angular_velocity| = angle/time = erp*theta / stepsize 531 // = (erp*fps) * theta 532 // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| 533 // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) 534 // ...as ax1 and ax2 are unit length. if theta is smallish, 535 // theta ~= sin(theta), so 536 // angular_velocity = (erp*fps) * (ax1 x ax2) 537 // ax1 x ax2 is in the plane space of ax1, so we project the angular 538 // velocity to p and q to find the right hand side. 539 btVector3 ax2 = trB.getBasis().getColumn(2); 540 btVector3 u = ax1.cross(ax2); 541 info->m_constraintError[s3] = k * u.dot(p); 542 info->m_constraintError[s4] = k * u.dot(q); 543 // check angular limits 544 int nrow = 4; // last filled row 545 int srow; 546 btScalar limit_err = btScalar(0.0); 547 int limit = 0; 548 if(getSolveLimit()) 549 { 550 #ifdef _BT_USE_CENTER_LIMIT_ 551 limit_err = m_limit.getCorrection() * m_referenceSign; 552 #else 553 limit_err = m_correction * m_referenceSign; 554 #endif 555 limit = (limit_err > btScalar(0.0)) ? 1 : 2; 556 557 } 558 // if the hinge has joint limits or motor, add in the extra row 559 int powered = 0; 560 if(getEnableAngularMotor()) 561 { 562 powered = 1; 563 } 564 if(limit || powered) 565 { 566 nrow++; 567 srow = nrow * info->rowskip; 568 info->m_J1angularAxis[srow+0] = ax1[0]; 569 info->m_J1angularAxis[srow+1] = ax1[1]; 570 info->m_J1angularAxis[srow+2] = ax1[2]; 571 572 info->m_J2angularAxis[srow+0] = -ax1[0]; 573 info->m_J2angularAxis[srow+1] = -ax1[1]; 574 info->m_J2angularAxis[srow+2] = -ax1[2]; 575 576 btScalar lostop = getLowerLimit(); 577 btScalar histop = getUpperLimit(); 578 if(limit && (lostop == histop)) 579 { // the joint motor is ineffective 580 powered = 0; 581 } 582 info->m_constraintError[srow] = btScalar(0.0f); 583 btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp; 584 if(powered) 585 { 586 if(m_flags & BT_HINGE_FLAGS_CFM_NORM) 587 { 588 info->cfm[srow] = m_normalCFM; 589 } 590 btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP); 591 info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign; 592 info->m_lowerLimit[srow] = - m_maxMotorImpulse; 593 info->m_upperLimit[srow] = m_maxMotorImpulse; 594 } 595 if(limit) 596 { 597 k = info->fps * currERP; 598 info->m_constraintError[srow] += k * limit_err; 599 if(m_flags & BT_HINGE_FLAGS_CFM_STOP) 600 { 601 info->cfm[srow] = m_stopCFM; 602 } 603 if(lostop == histop) 604 { 605 // limited low and high simultaneously 606 info->m_lowerLimit[srow] = -SIMD_INFINITY; 607 info->m_upperLimit[srow] = SIMD_INFINITY; 608 } 609 else if(limit == 1) 610 { // low limit 611 info->m_lowerLimit[srow] = 0; 612 info->m_upperLimit[srow] = SIMD_INFINITY; 613 } 614 else 615 { // high limit 616 info->m_lowerLimit[srow] = -SIMD_INFINITY; 617 info->m_upperLimit[srow] = 0; 618 } 619 // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) 620 #ifdef _BT_USE_CENTER_LIMIT_ 621 btScalar bounce = m_limit.getRelaxationFactor(); 622 #else 623 btScalar bounce = m_relaxationFactor; 624 #endif 625 if(bounce > btScalar(0.0)) 626 { 627 btScalar vel = angVelA.dot(ax1); 628 vel -= angVelB.dot(ax1); 629 // only apply bounce if the velocity is incoming, and if the 630 // resulting c[] exceeds what we already have. 631 if(limit == 1) 632 { // low limit 633 if(vel < 0) 634 { 635 btScalar newc = -bounce * vel; 636 if(newc > info->m_constraintError[srow]) 637 { 638 info->m_constraintError[srow] = newc; 639 } 640 } 641 } 642 else 643 { // high limit - all those computations are reversed 644 if(vel > 0) 645 { 646 btScalar newc = -bounce * vel; 647 if(newc < info->m_constraintError[srow]) 648 { 649 info->m_constraintError[srow] = newc; 650 } 651 } 652 } 653 } 654 #ifdef _BT_USE_CENTER_LIMIT_ 655 info->m_constraintError[srow] *= m_limit.getBiasFactor(); 656 #else 657 info->m_constraintError[srow] *= m_biasFactor; 658 #endif 659 } // if(limit) 660 } // if angular limit or powered 661 } 662 663 664 void btHingeConstraint::setFrames(const btTransform & frameA, const btTransform & frameB) 665 { 666 m_rbAFrame = frameA; 667 m_rbBFrame = frameB; 668 buildJacobian(); 669 } 670 671 672 void btHingeConstraint::updateRHS(btScalar timeStep) 673 { 674 (void)timeStep; 675 676 } 677 678 679 680 681 btScalar btHingeConstraint::getHingeAngle() 682 { 683 return getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); 684 } 685 686 btScalar btHingeConstraint::getHingeAngle(const btTransform& transA,const btTransform& transB) 687 { 688 const btVector3 refAxis0 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0); 689 const btVector3 refAxis1 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1); 690 const btVector3 swingAxis = transB.getBasis() * m_rbBFrame.getBasis().getColumn(1); 691 // btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); 692 btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); 693 return m_referenceSign * angle; 694 } 695 696 697 698 void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& transB) 699 { 700 // Compute limit information 701 m_hingeAngle = getHingeAngle(transA,transB); 702 #ifdef _BT_USE_CENTER_LIMIT_ 703 m_limit.test(m_hingeAngle); 704 #else 705 m_correction = btScalar(0.); 706 m_limitSign = btScalar(0.); 707 m_solveLimit = false; 708 if (m_lowerLimit <= m_upperLimit) 709 { 710 m_hingeAngle = btAdjustAngleToLimits(m_hingeAngle, m_lowerLimit, m_upperLimit); 711 if (m_hingeAngle <= m_lowerLimit) 712 { 713 m_correction = (m_lowerLimit - m_hingeAngle); 714 m_limitSign = 1.0f; 715 m_solveLimit = true; 716 } 717 else if (m_hingeAngle >= m_upperLimit) 718 { 719 m_correction = m_upperLimit - m_hingeAngle; 720 m_limitSign = -1.0f; 721 m_solveLimit = true; 722 } 723 } 724 #endif 725 return; 726 } 727 728 729 static btVector3 vHinge(0, 0, btScalar(1)); 730 731 void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt) 732 { 733 // convert target from body to constraint space 734 btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * qAinB * m_rbAFrame.getRotation(); 735 qConstraint.normalize(); 736 737 // extract "pure" hinge component 738 btVector3 vNoHinge = quatRotate(qConstraint, vHinge); vNoHinge.normalize(); 739 btQuaternion qNoHinge = shortestArcQuat(vHinge, vNoHinge); 740 btQuaternion qHinge = qNoHinge.inverse() * qConstraint; 741 qHinge.normalize(); 742 743 // compute angular target, clamped to limits 744 btScalar targetAngle = qHinge.getAngle(); 745 if (targetAngle > SIMD_PI) // long way around. flip quat and recalculate. 746 { 747 qHinge = -(qHinge); 748 targetAngle = qHinge.getAngle(); 749 } 750 if (qHinge.getZ() < 0) 751 targetAngle = -targetAngle; 752 753 setMotorTarget(targetAngle, dt); 754 } 755 756 void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt) 757 { 758 #ifdef _BT_USE_CENTER_LIMIT_ 759 m_limit.fit(targetAngle); 760 #else 761 if (m_lowerLimit < m_upperLimit) 762 { 763 if (targetAngle < m_lowerLimit) 764 targetAngle = m_lowerLimit; 765 else if (targetAngle > m_upperLimit) 766 targetAngle = m_upperLimit; 767 } 768 #endif 769 // compute angular velocity 770 btScalar curAngle = getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); 771 btScalar dAngle = targetAngle - curAngle; 772 m_motorTargetVelocity = dAngle / dt; 773 } 774 775 776 777 void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB) 778 { 779 btAssert(!m_useSolveConstraintObsolete); 780 int i, s = info->rowskip; 781 // transforms in world space 782 btTransform trA = transA*m_rbAFrame; 783 btTransform trB = transB*m_rbBFrame; 784 // pivot point 785 // btVector3 pivotAInW = trA.getOrigin(); 786 // btVector3 pivotBInW = trB.getOrigin(); 787 #if 1 788 // difference between frames in WCS 789 btVector3 ofs = trB.getOrigin() - trA.getOrigin(); 790 // now get weight factors depending on masses 791 btScalar miA = getRigidBodyA().getInvMass(); 792 btScalar miB = getRigidBodyB().getInvMass(); 793 bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON); 794 btScalar miS = miA + miB; 795 btScalar factA, factB; 796 if(miS > btScalar(0.f)) 797 { 798 factA = miB / miS; 799 } 800 else 801 { 802 factA = btScalar(0.5f); 803 } 804 factB = btScalar(1.0f) - factA; 805 // get the desired direction of hinge axis 806 // as weighted sum of Z-orthos of frameA and frameB in WCS 807 btVector3 ax1A = trA.getBasis().getColumn(2); 808 btVector3 ax1B = trB.getBasis().getColumn(2); 809 btVector3 ax1 = ax1A * factA + ax1B * factB; 810 ax1.normalize(); 811 // fill first 3 rows 812 // we want: velA + wA x relA == velB + wB x relB 813 btTransform bodyA_trans = transA; 814 btTransform bodyB_trans = transB; 815 int s0 = 0; 816 int s1 = s; 817 int s2 = s * 2; 818 int nrow = 2; // last filled row 819 btVector3 tmpA, tmpB, relA, relB, p, q; 820 // get vector from bodyB to frameB in WCS 821 relB = trB.getOrigin() - bodyB_trans.getOrigin(); 822 // get its projection to hinge axis 823 btVector3 projB = ax1 * relB.dot(ax1); 824 // get vector directed from bodyB to hinge axis (and orthogonal to it) 825 btVector3 orthoB = relB - projB; 826 // same for bodyA 827 relA = trA.getOrigin() - bodyA_trans.getOrigin(); 828 btVector3 projA = ax1 * relA.dot(ax1); 829 btVector3 orthoA = relA - projA; 830 btVector3 totalDist = projA - projB; 831 // get offset vectors relA and relB 832 relA = orthoA + totalDist * factA; 833 relB = orthoB - totalDist * factB; 834 // now choose average ortho to hinge axis 835 p = orthoB * factA + orthoA * factB; 836 btScalar len2 = p.length2(); 837 if(len2 > SIMD_EPSILON) 838 { 839 p /= btSqrt(len2); 840 } 841 else 842 { 843 p = trA.getBasis().getColumn(1); 844 } 845 // make one more ortho 846 q = ax1.cross(p); 847 // fill three rows 848 tmpA = relA.cross(p); 849 tmpB = relB.cross(p); 850 for (i=0; i<3; i++) info->m_J1angularAxis[s0+i] = tmpA[i]; 851 for (i=0; i<3; i++) info->m_J2angularAxis[s0+i] = -tmpB[i]; 852 tmpA = relA.cross(q); 853 tmpB = relB.cross(q); 854 if(hasStaticBody && getSolveLimit()) 855 { // to make constraint between static and dynamic objects more rigid 856 // remove wA (or wB) from equation if angular limit is hit 857 tmpB *= factB; 858 tmpA *= factA; 859 } 860 for (i=0; i<3; i++) info->m_J1angularAxis[s1+i] = tmpA[i]; 861 for (i=0; i<3; i++) info->m_J2angularAxis[s1+i] = -tmpB[i]; 862 tmpA = relA.cross(ax1); 863 tmpB = relB.cross(ax1); 864 if(hasStaticBody) 865 { // to make constraint between static and dynamic objects more rigid 866 // remove wA (or wB) from equation 867 tmpB *= factB; 868 tmpA *= factA; 869 } 870 for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i]; 871 for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i]; 872 873 btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM)? m_normalERP : info->erp; 874 btScalar k = info->fps * normalErp; 875 876 if (!m_angularOnly) 877 { 878 for (i=0; i<3; i++) info->m_J1linearAxis[s0+i] = p[i]; 879 for (i=0; i<3; i++) info->m_J1linearAxis[s1+i] = q[i]; 880 for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = ax1[i]; 881 882 for (i=0; i<3; i++) info->m_J2linearAxis[s0+i] = -p[i]; 883 for (i=0; i<3; i++) info->m_J2linearAxis[s1+i] = -q[i]; 884 for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -ax1[i]; 885 886 // compute three elements of right hand side 887 888 btScalar rhs = k * p.dot(ofs); 889 info->m_constraintError[s0] = rhs; 890 rhs = k * q.dot(ofs); 891 info->m_constraintError[s1] = rhs; 892 rhs = k * ax1.dot(ofs); 893 info->m_constraintError[s2] = rhs; 894 } 895 // the hinge axis should be the only unconstrained 896 // rotational axis, the angular velocity of the two bodies perpendicular to 897 // the hinge axis should be equal. thus the constraint equations are 898 // p*w1 - p*w2 = 0 899 // q*w1 - q*w2 = 0 900 // where p and q are unit vectors normal to the hinge axis, and w1 and w2 901 // are the angular velocity vectors of the two bodies. 902 int s3 = 3 * s; 903 int s4 = 4 * s; 904 info->m_J1angularAxis[s3 + 0] = p[0]; 905 info->m_J1angularAxis[s3 + 1] = p[1]; 906 info->m_J1angularAxis[s3 + 2] = p[2]; 907 info->m_J1angularAxis[s4 + 0] = q[0]; 908 info->m_J1angularAxis[s4 + 1] = q[1]; 909 info->m_J1angularAxis[s4 + 2] = q[2]; 910 911 info->m_J2angularAxis[s3 + 0] = -p[0]; 912 info->m_J2angularAxis[s3 + 1] = -p[1]; 913 info->m_J2angularAxis[s3 + 2] = -p[2]; 914 info->m_J2angularAxis[s4 + 0] = -q[0]; 915 info->m_J2angularAxis[s4 + 1] = -q[1]; 916 info->m_J2angularAxis[s4 + 2] = -q[2]; 917 // compute the right hand side of the constraint equation. set relative 918 // body velocities along p and q to bring the hinge back into alignment. 919 // if ax1A,ax1B are the unit length hinge axes as computed from bodyA and 920 // bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2). 921 // if "theta" is the angle between ax1 and ax2, we need an angular velocity 922 // along u to cover angle erp*theta in one step : 923 // |angular_velocity| = angle/time = erp*theta / stepsize 924 // = (erp*fps) * theta 925 // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| 926 // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) 927 // ...as ax1 and ax2 are unit length. if theta is smallish, 928 // theta ~= sin(theta), so 929 // angular_velocity = (erp*fps) * (ax1 x ax2) 930 // ax1 x ax2 is in the plane space of ax1, so we project the angular 931 // velocity to p and q to find the right hand side. 932 k = info->fps * normalErp;//?? 933 934 btVector3 u = ax1A.cross(ax1B); 935 info->m_constraintError[s3] = k * u.dot(p); 936 info->m_constraintError[s4] = k * u.dot(q); 937 #endif 938 // check angular limits 939 nrow = 4; // last filled row 940 int srow; 941 btScalar limit_err = btScalar(0.0); 942 int limit = 0; 943 if(getSolveLimit()) 944 { 945 #ifdef _BT_USE_CENTER_LIMIT_ 946 limit_err = m_limit.getCorrection() * m_referenceSign; 947 #else 948 limit_err = m_correction * m_referenceSign; 949 #endif 950 limit = (limit_err > btScalar(0.0)) ? 1 : 2; 951 952 } 953 // if the hinge has joint limits or motor, add in the extra row 954 int powered = 0; 955 if(getEnableAngularMotor()) 956 { 957 powered = 1; 958 } 959 if(limit || powered) 960 { 961 nrow++; 962 srow = nrow * info->rowskip; 963 info->m_J1angularAxis[srow+0] = ax1[0]; 964 info->m_J1angularAxis[srow+1] = ax1[1]; 965 info->m_J1angularAxis[srow+2] = ax1[2]; 966 967 info->m_J2angularAxis[srow+0] = -ax1[0]; 968 info->m_J2angularAxis[srow+1] = -ax1[1]; 969 info->m_J2angularAxis[srow+2] = -ax1[2]; 970 971 btScalar lostop = getLowerLimit(); 972 btScalar histop = getUpperLimit(); 973 if(limit && (lostop == histop)) 974 { // the joint motor is ineffective 975 powered = 0; 976 } 977 info->m_constraintError[srow] = btScalar(0.0f); 978 btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp; 979 if(powered) 980 { 981 if(m_flags & BT_HINGE_FLAGS_CFM_NORM) 982 { 983 info->cfm[srow] = m_normalCFM; 984 } 985 btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP); 986 info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign; 987 info->m_lowerLimit[srow] = - m_maxMotorImpulse; 988 info->m_upperLimit[srow] = m_maxMotorImpulse; 989 } 990 if(limit) 991 { 992 k = info->fps * currERP; 993 info->m_constraintError[srow] += k * limit_err; 994 if(m_flags & BT_HINGE_FLAGS_CFM_STOP) 995 { 996 info->cfm[srow] = m_stopCFM; 997 } 998 if(lostop == histop) 999 { 1000 // limited low and high simultaneously 1001 info->m_lowerLimit[srow] = -SIMD_INFINITY; 1002 info->m_upperLimit[srow] = SIMD_INFINITY; 1003 } 1004 else if(limit == 1) 1005 { // low limit 1006 info->m_lowerLimit[srow] = 0; 1007 info->m_upperLimit[srow] = SIMD_INFINITY; 1008 } 1009 else 1010 { // high limit 1011 info->m_lowerLimit[srow] = -SIMD_INFINITY; 1012 info->m_upperLimit[srow] = 0; 1013 } 1014 // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) 1015 #ifdef _BT_USE_CENTER_LIMIT_ 1016 btScalar bounce = m_limit.getRelaxationFactor(); 1017 #else 1018 btScalar bounce = m_relaxationFactor; 1019 #endif 1020 if(bounce > btScalar(0.0)) 1021 { 1022 btScalar vel = angVelA.dot(ax1); 1023 vel -= angVelB.dot(ax1); 1024 // only apply bounce if the velocity is incoming, and if the 1025 // resulting c[] exceeds what we already have. 1026 if(limit == 1) 1027 { // low limit 1028 if(vel < 0) 1029 { 1030 btScalar newc = -bounce * vel; 1031 if(newc > info->m_constraintError[srow]) 1032 { 1033 info->m_constraintError[srow] = newc; 1034 } 1035 } 1036 } 1037 else 1038 { // high limit - all those computations are reversed 1039 if(vel > 0) 1040 { 1041 btScalar newc = -bounce * vel; 1042 if(newc < info->m_constraintError[srow]) 1043 { 1044 info->m_constraintError[srow] = newc; 1045 } 1046 } 1047 } 1048 } 1049 #ifdef _BT_USE_CENTER_LIMIT_ 1050 info->m_constraintError[srow] *= m_limit.getBiasFactor(); 1051 #else 1052 info->m_constraintError[srow] *= m_biasFactor; 1053 #endif 1054 } // if(limit) 1055 } // if angular limit or powered 1056 } 1057 1058 1059 ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 1060 ///If no axis is provided, it uses the default axis for this constraint. 1061 void btHingeConstraint::setParam(int num, btScalar value, int axis) 1062 { 1063 if((axis == -1) || (axis == 5)) 1064 { 1065 switch(num) 1066 { 1067 case BT_CONSTRAINT_STOP_ERP : 1068 m_stopERP = value; 1069 m_flags |= BT_HINGE_FLAGS_ERP_STOP; 1070 break; 1071 case BT_CONSTRAINT_STOP_CFM : 1072 m_stopCFM = value; 1073 m_flags |= BT_HINGE_FLAGS_CFM_STOP; 1074 break; 1075 case BT_CONSTRAINT_CFM : 1076 m_normalCFM = value; 1077 m_flags |= BT_HINGE_FLAGS_CFM_NORM; 1078 break; 1079 case BT_CONSTRAINT_ERP: 1080 m_normalERP = value; 1081 m_flags |= BT_HINGE_FLAGS_ERP_NORM; 1082 break; 1083 default : 1084 btAssertConstrParams(0); 1085 } 1086 } 1087 else 1088 { 1089 btAssertConstrParams(0); 1090 } 1091 } 1092 1093 ///return the local value of parameter 1094 btScalar btHingeConstraint::getParam(int num, int axis) const 1095 { 1096 btScalar retVal = 0; 1097 if((axis == -1) || (axis == 5)) 1098 { 1099 switch(num) 1100 { 1101 case BT_CONSTRAINT_STOP_ERP : 1102 btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_STOP); 1103 retVal = m_stopERP; 1104 break; 1105 case BT_CONSTRAINT_STOP_CFM : 1106 btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_STOP); 1107 retVal = m_stopCFM; 1108 break; 1109 case BT_CONSTRAINT_CFM : 1110 btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_NORM); 1111 retVal = m_normalCFM; 1112 break; 1113 case BT_CONSTRAINT_ERP: 1114 btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_NORM); 1115 retVal = m_normalERP; 1116 break; 1117 default : 1118 btAssertConstrParams(0); 1119 } 1120 } 1121 else 1122 { 1123 btAssertConstrParams(0); 1124 } 1125 return retVal; 1126 } 1127 1128 1129
