Changeset 7983 for code/branches/kicklib/src/external/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
- Timestamp:
- Feb 27, 2011, 7:43:24 AM (14 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
code/branches/kicklib/src/external/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
r5781 r7983 22 22 #include "btSolverBody.h" 23 23 24 //----------------------------------------------------------------------------- 25 24 25 26 //#define HINGE_USE_OBSOLETE_SOLVER false 26 27 #define HINGE_USE_OBSOLETE_SOLVER false 27 28 28 //----------------------------------------------------------------------------- 29 30 31 btHingeConstraint::btHingeConstraint() 32 : btTypedConstraint (HINGE_CONSTRAINT_TYPE), 33 m_enableAngularMotor(false), 34 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), 35 m_useReferenceFrameA(false) 36 { 37 m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f); 38 } 39 40 //----------------------------------------------------------------------------- 29 #define HINGE_USE_FRAME_OFFSET true 30 31 #ifndef __SPU__ 32 33 34 35 41 36 42 37 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, 43 btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA)38 const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA) 44 39 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB), 45 40 m_angularOnly(false), 46 41 m_enableAngularMotor(false), 47 42 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), 48 m_useReferenceFrameA(useReferenceFrameA) 43 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), 44 m_useReferenceFrameA(useReferenceFrameA), 45 m_flags(0) 49 46 { 50 47 m_rbAFrame.getOrigin() = pivotInA; … … 80 77 81 78 //start with free 82 m_lowerLimit = btScalar(1 e30);83 m_upperLimit = btScalar(-1 e30);79 m_lowerLimit = btScalar(1.0f); 80 m_upperLimit = btScalar(-1.0f); 84 81 m_biasFactor = 0.3f; 85 82 m_relaxationFactor = 1.0f; … … 89 86 } 90 87 91 //----------------------------------------------------------------------------- 92 93 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA, btVector3& axisInA, bool useReferenceFrameA)88 89 90 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA) 94 91 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false), 95 92 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), 96 m_useReferenceFrameA(useReferenceFrameA) 93 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), 94 m_useReferenceFrameA(useReferenceFrameA), 95 m_flags(0) 97 96 { 98 97 … … 120 119 121 120 //start with free 122 m_lowerLimit = btScalar(1 e30);123 m_upperLimit = btScalar(-1 e30);121 m_lowerLimit = btScalar(1.0f); 122 m_upperLimit = btScalar(-1.0f); 124 123 m_biasFactor = 0.3f; 125 124 m_relaxationFactor = 1.0f; … … 129 128 } 130 129 131 //----------------------------------------------------------------------------- 130 132 131 133 132 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, … … 137 136 m_enableAngularMotor(false), 138 137 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), 139 m_useReferenceFrameA(useReferenceFrameA) 138 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), 139 m_useReferenceFrameA(useReferenceFrameA), 140 m_flags(0) 140 141 { 141 142 //start with free 142 m_lowerLimit = btScalar(1 e30);143 m_upperLimit = btScalar(-1 e30);143 m_lowerLimit = btScalar(1.0f); 144 m_upperLimit = btScalar(-1.0f); 144 145 m_biasFactor = 0.3f; 145 146 m_relaxationFactor = 1.0f; … … 149 150 } 150 151 151 //----------------------------------------------------------------------------- 152 152 153 153 154 btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA) … … 156 157 m_enableAngularMotor(false), 157 158 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), 158 m_useReferenceFrameA(useReferenceFrameA) 159 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), 160 m_useReferenceFrameA(useReferenceFrameA), 161 m_flags(0) 159 162 { 160 163 ///not providing rigidbody B means implicitly using worldspace for body B … … 163 166 164 167 //start with free 165 m_lowerLimit = btScalar(1 e30);166 m_upperLimit = btScalar(-1 e30);168 m_lowerLimit = btScalar(1.0f); 169 m_upperLimit = btScalar(-1.0f); 167 170 m_biasFactor = 0.3f; 168 171 m_relaxationFactor = 1.0f; … … 172 175 } 173 176 174 //----------------------------------------------------------------------------- 177 175 178 176 179 void btHingeConstraint::buildJacobian() … … 179 182 { 180 183 m_appliedImpulse = btScalar(0.); 184 m_accMotorImpulse = btScalar(0.); 181 185 182 186 if (!m_angularOnly) … … 222 226 btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local); 223 227 224 getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);225 228 btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local; 226 229 btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local; … … 249 252 250 253 // test angular limit 251 testLimit( );254 testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); 252 255 253 256 //Compute K = J*W*J' for hinge axis … … 259 262 } 260 263 261 //----------------------------------------------------------------------------- 264 265 #endif //__SPU__ 266 262 267 263 268 void btHingeConstraint::getInfo1(btConstraintInfo1* info) … … 272 277 info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular 273 278 info->nub = 1; 279 //always add the row, to avoid computation (data is not available yet) 274 280 //prepare constraint 275 testLimit( );281 testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); 276 282 if(getSolveLimit() || getEnableAngularMotor()) 277 283 { … … 279 285 info->nub--; 280 286 } 281 } 282 } // btHingeConstraint::getInfo1 () 283 284 //----------------------------------------------------------------------------- 287 288 } 289 } 290 291 void btHingeConstraint::getInfo1NonVirtual(btConstraintInfo1* info) 292 { 293 if (m_useSolveConstraintObsolete) 294 { 295 info->m_numConstraintRows = 0; 296 info->nub = 0; 297 } 298 else 299 { 300 //always add the 'limit' row, to avoid computation (data is not available yet) 301 info->m_numConstraintRows = 6; // Fixed 3 linear + 2 angular 302 info->nub = 0; 303 } 304 } 285 305 286 306 void btHingeConstraint::getInfo2 (btConstraintInfo2* info) 287 307 { 308 if(m_useOffsetForConstraintFrame) 309 { 310 getInfo2InternalUsingFrameOffset(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity()); 311 } 312 else 313 { 314 getInfo2Internal(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity()); 315 } 316 } 317 318 319 void btHingeConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB) 320 { 321 ///the regular (virtual) implementation getInfo2 already performs 'testLimit' during getInfo1, so we need to do it now 322 testLimit(transA,transB); 323 324 getInfo2Internal(info,transA,transB,angVelA,angVelB); 325 } 326 327 328 void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB) 329 { 330 288 331 btAssert(!m_useSolveConstraintObsolete); 289 int i, s = info->rowskip;332 int i, skip = info->rowskip; 290 333 // transforms in world space 291 btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame;292 btTransform trB = m_rbB.getCenterOfMassTransform()*m_rbBFrame;334 btTransform trA = transA*m_rbAFrame; 335 btTransform trB = transB*m_rbBFrame; 293 336 // pivot point 294 337 btVector3 pivotAInW = trA.getOrigin(); 295 338 btVector3 pivotBInW = trB.getOrigin(); 339 #if 0 340 if (0) 341 { 342 for (i=0;i<6;i++) 343 { 344 info->m_J1linearAxis[i*skip]=0; 345 info->m_J1linearAxis[i*skip+1]=0; 346 info->m_J1linearAxis[i*skip+2]=0; 347 348 info->m_J1angularAxis[i*skip]=0; 349 info->m_J1angularAxis[i*skip+1]=0; 350 info->m_J1angularAxis[i*skip+2]=0; 351 352 info->m_J2angularAxis[i*skip]=0; 353 info->m_J2angularAxis[i*skip+1]=0; 354 info->m_J2angularAxis[i*skip+2]=0; 355 356 info->m_constraintError[i*skip]=0.f; 357 } 358 } 359 #endif //#if 0 296 360 // linear (all fixed) 297 info->m_J1linearAxis[0] = 1; 298 info->m_J1linearAxis[s + 1] = 1; 299 info->m_J1linearAxis[2 * s + 2] = 1; 300 btVector3 a1 = pivotAInW - m_rbA.getCenterOfMassTransform().getOrigin(); 361 362 if (!m_angularOnly) 363 { 364 info->m_J1linearAxis[0] = 1; 365 info->m_J1linearAxis[skip + 1] = 1; 366 info->m_J1linearAxis[2 * skip + 2] = 1; 367 } 368 369 370 371 372 btVector3 a1 = pivotAInW - transA.getOrigin(); 301 373 { 302 374 btVector3* angular0 = (btVector3*)(info->m_J1angularAxis); 303 btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + s );304 btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * s );375 btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + skip); 376 btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * skip); 305 377 btVector3 a1neg = -a1; 306 378 a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); 307 379 } 308 btVector3 a2 = pivotBInW - m_rbB.getCenterOfMassTransform().getOrigin();380 btVector3 a2 = pivotBInW - transB.getOrigin(); 309 381 { 310 382 btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); 311 btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + s );312 btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * s );383 btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + skip); 384 btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * skip); 313 385 a2.getSkewSymmetricMatrix(angular0,angular1,angular2); 314 386 } 315 387 // linear RHS 316 388 btScalar k = info->fps * info->erp; 317 for(i = 0; i < 3; i++) 318 { 319 info->m_constraintError[i * s] = k * (pivotBInW[i] - pivotAInW[i]); 320 } 389 if (!m_angularOnly) 390 { 391 for(i = 0; i < 3; i++) 392 { 393 info->m_constraintError[i * skip] = k * (pivotBInW[i] - pivotAInW[i]); 394 } 395 } 321 396 // make rotations around X and Y equal 322 397 // the hinge axis should be the only unconstrained … … 403 478 } 404 479 info->m_constraintError[srow] = btScalar(0.0f); 480 btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp; 405 481 if(powered) 406 482 { 407 info->cfm[srow] = btScalar(0.0); 408 btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * info->erp); 483 if(m_flags & BT_HINGE_FLAGS_CFM_NORM) 484 { 485 info->cfm[srow] = m_normalCFM; 486 } 487 btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP); 409 488 info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign; 410 489 info->m_lowerLimit[srow] = - m_maxMotorImpulse; … … 413 492 if(limit) 414 493 { 415 k = info->fps * info->erp;494 k = info->fps * currERP; 416 495 info->m_constraintError[srow] += k * limit_err; 417 info->cfm[srow] = btScalar(0.0); 496 if(m_flags & BT_HINGE_FLAGS_CFM_STOP) 497 { 498 info->cfm[srow] = m_stopCFM; 499 } 418 500 if(lostop == histop) 419 501 { … … 436 518 if(bounce > btScalar(0.0)) 437 519 { 438 btScalar vel = m_rbA.getAngularVelocity().dot(ax1);439 vel -= m_rbB.getAngularVelocity().dot(ax1);520 btScalar vel = angVelA.dot(ax1); 521 vel -= angVelB.dot(ax1); 440 522 // only apply bounce if the velocity is incoming, and if the 441 523 // resulting c[] exceeds what we already have. … … 468 550 } 469 551 470 //----------------------------------------------------------------------------- 471 472 void btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) 473 { 474 475 ///for backwards compatibility during the transition to 'getInfo/getInfo2' 476 if (m_useSolveConstraintObsolete) 477 { 478 479 btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin(); 480 btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin(); 481 482 btScalar tau = btScalar(0.3); 483 484 //linear part 485 if (!m_angularOnly) 486 { 487 btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 488 btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition(); 489 490 btVector3 vel1,vel2; 491 bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1); 492 bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2); 493 btVector3 vel = vel1 - vel2; 494 495 for (int i=0;i<3;i++) 496 { 497 const btVector3& normal = m_jac[i].m_linearJointAxis; 498 btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal(); 499 500 btScalar rel_vel; 501 rel_vel = normal.dot(vel); 502 //positional error (zeroth order error) 503 btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal 504 btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv; 505 m_appliedImpulse += impulse; 506 btVector3 impulse_vector = normal * impulse; 507 btVector3 ftorqueAxis1 = rel_pos1.cross(normal); 508 btVector3 ftorqueAxis2 = rel_pos2.cross(normal); 509 bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse); 510 bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse); 511 } 512 } 513 514 515 { 516 ///solve angular part 517 518 // get axes in world space 519 btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); 520 btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(2); 521 522 btVector3 angVelA; 523 bodyA.getAngularVelocity(angVelA); 524 btVector3 angVelB; 525 bodyB.getAngularVelocity(angVelB); 526 527 btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA); 528 btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB); 529 530 btVector3 angAorthog = angVelA - angVelAroundHingeAxisA; 531 btVector3 angBorthog = angVelB - angVelAroundHingeAxisB; 532 btVector3 velrelOrthog = angAorthog-angBorthog; 533 { 534 535 536 //solve orthogonal angular velocity correction 537 btScalar relaxation = btScalar(1.); 538 btScalar len = velrelOrthog.length(); 539 if (len > btScalar(0.00001)) 540 { 541 btVector3 normal = velrelOrthog.normalized(); 542 btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) + 543 getRigidBodyB().computeAngularImpulseDenominator(normal); 544 // scale for mass and relaxation 545 //velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor; 546 547 bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*velrelOrthog,-(btScalar(1.)/denom)); 548 bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*velrelOrthog,(btScalar(1.)/denom)); 549 550 } 551 552 //solve angular positional correction 553 btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/timeStep); 554 btScalar len2 = angularError.length(); 555 if (len2>btScalar(0.00001)) 556 { 557 btVector3 normal2 = angularError.normalized(); 558 btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) + 559 getRigidBodyB().computeAngularImpulseDenominator(normal2); 560 //angularError *= (btScalar(1.)/denom2) * relaxation; 561 562 bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*angularError,(btScalar(1.)/denom2)); 563 bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*angularError,-(btScalar(1.)/denom2)); 564 565 } 566 567 568 569 570 571 // solve limit 572 if (m_solveLimit) 573 { 574 btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor ) * m_limitSign; 575 576 btScalar impulseMag = amplitude * m_kHinge; 577 578 // Clamp the accumulated impulse 579 btScalar temp = m_accLimitImpulse; 580 m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) ); 581 impulseMag = m_accLimitImpulse - temp; 582 583 584 585 bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,impulseMag * m_limitSign); 586 bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-(impulseMag * m_limitSign)); 587 588 } 589 } 590 591 //apply motor 592 if (m_enableAngularMotor) 593 { 594 //todo: add limits too 595 btVector3 angularLimit(0,0,0); 596 597 btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB; 598 btScalar projRelVel = velrel.dot(axisA); 599 600 btScalar desiredMotorVel = m_motorTargetVelocity; 601 btScalar motor_relvel = desiredMotorVel - projRelVel; 602 603 btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;; 604 //todo: should clip against accumulated impulse 605 btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse; 606 clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse; 607 btVector3 motorImp = clippedMotorImpulse * axisA; 608 609 bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,clippedMotorImpulse); 610 bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-clippedMotorImpulse); 611 612 } 613 } 614 } 615 616 } 617 618 //----------------------------------------------------------------------------- 552 553 554 555 619 556 620 557 void btHingeConstraint::updateRHS(btScalar timeStep) … … 624 561 } 625 562 626 //-----------------------------------------------------------------------------627 563 628 564 btScalar btHingeConstraint::getHingeAngle() 629 565 { 630 const btVector3 refAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0); 631 const btVector3 refAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1); 632 const btVector3 swingAxis = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(1); 633 btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); 566 return getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); 567 } 568 569 btScalar btHingeConstraint::getHingeAngle(const btTransform& transA,const btTransform& transB) 570 { 571 const btVector3 refAxis0 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0); 572 const btVector3 refAxis1 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1); 573 const btVector3 swingAxis = transB.getBasis() * m_rbBFrame.getBasis().getColumn(1); 574 // btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); 575 btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); 634 576 return m_referenceSign * angle; 635 577 } 636 578 637 //----------------------------------------------------------------------------- 638 579 580 #if 0 639 581 void btHingeConstraint::testLimit() 640 582 { … … 660 602 } 661 603 return; 662 } // btHingeConstraint::testLimit() 663 664 //----------------------------------------------------------------------------- 665 //----------------------------------------------------------------------------- 666 //----------------------------------------------------------------------------- 604 } 605 #else 606 607 608 void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& transB) 609 { 610 // Compute limit information 611 m_hingeAngle = getHingeAngle(transA,transB); 612 m_correction = btScalar(0.); 613 m_limitSign = btScalar(0.); 614 m_solveLimit = false; 615 if (m_lowerLimit <= m_upperLimit) 616 { 617 m_hingeAngle = btAdjustAngleToLimits(m_hingeAngle, m_lowerLimit, m_upperLimit); 618 if (m_hingeAngle <= m_lowerLimit) 619 { 620 m_correction = (m_lowerLimit - m_hingeAngle); 621 m_limitSign = 1.0f; 622 m_solveLimit = true; 623 } 624 else if (m_hingeAngle >= m_upperLimit) 625 { 626 m_correction = m_upperLimit - m_hingeAngle; 627 m_limitSign = -1.0f; 628 m_solveLimit = true; 629 } 630 } 631 return; 632 } 633 #endif 634 635 static btVector3 vHinge(0, 0, btScalar(1)); 636 637 void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt) 638 { 639 // convert target from body to constraint space 640 btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * qAinB * m_rbAFrame.getRotation(); 641 qConstraint.normalize(); 642 643 // extract "pure" hinge component 644 btVector3 vNoHinge = quatRotate(qConstraint, vHinge); vNoHinge.normalize(); 645 btQuaternion qNoHinge = shortestArcQuat(vHinge, vNoHinge); 646 btQuaternion qHinge = qNoHinge.inverse() * qConstraint; 647 qHinge.normalize(); 648 649 // compute angular target, clamped to limits 650 btScalar targetAngle = qHinge.getAngle(); 651 if (targetAngle > SIMD_PI) // long way around. flip quat and recalculate. 652 { 653 qHinge = operator-(qHinge); 654 targetAngle = qHinge.getAngle(); 655 } 656 if (qHinge.getZ() < 0) 657 targetAngle = -targetAngle; 658 659 setMotorTarget(targetAngle, dt); 660 } 661 662 void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt) 663 { 664 if (m_lowerLimit < m_upperLimit) 665 { 666 if (targetAngle < m_lowerLimit) 667 targetAngle = m_lowerLimit; 668 else if (targetAngle > m_upperLimit) 669 targetAngle = m_upperLimit; 670 } 671 672 // compute angular velocity 673 btScalar curAngle = getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform()); 674 btScalar dAngle = targetAngle - curAngle; 675 m_motorTargetVelocity = dAngle / dt; 676 } 677 678 679 680 void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB) 681 { 682 btAssert(!m_useSolveConstraintObsolete); 683 int i, s = info->rowskip; 684 // transforms in world space 685 btTransform trA = transA*m_rbAFrame; 686 btTransform trB = transB*m_rbBFrame; 687 // pivot point 688 btVector3 pivotAInW = trA.getOrigin(); 689 btVector3 pivotBInW = trB.getOrigin(); 690 #if 1 691 // difference between frames in WCS 692 btVector3 ofs = trB.getOrigin() - trA.getOrigin(); 693 // now get weight factors depending on masses 694 btScalar miA = getRigidBodyA().getInvMass(); 695 btScalar miB = getRigidBodyB().getInvMass(); 696 bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON); 697 btScalar miS = miA + miB; 698 btScalar factA, factB; 699 if(miS > btScalar(0.f)) 700 { 701 factA = miB / miS; 702 } 703 else 704 { 705 factA = btScalar(0.5f); 706 } 707 factB = btScalar(1.0f) - factA; 708 // get the desired direction of hinge axis 709 // as weighted sum of Z-orthos of frameA and frameB in WCS 710 btVector3 ax1A = trA.getBasis().getColumn(2); 711 btVector3 ax1B = trB.getBasis().getColumn(2); 712 btVector3 ax1 = ax1A * factA + ax1B * factB; 713 ax1.normalize(); 714 // fill first 3 rows 715 // we want: velA + wA x relA == velB + wB x relB 716 btTransform bodyA_trans = transA; 717 btTransform bodyB_trans = transB; 718 int s0 = 0; 719 int s1 = s; 720 int s2 = s * 2; 721 int nrow = 2; // last filled row 722 btVector3 tmpA, tmpB, relA, relB, p, q; 723 // get vector from bodyB to frameB in WCS 724 relB = trB.getOrigin() - bodyB_trans.getOrigin(); 725 // get its projection to hinge axis 726 btVector3 projB = ax1 * relB.dot(ax1); 727 // get vector directed from bodyB to hinge axis (and orthogonal to it) 728 btVector3 orthoB = relB - projB; 729 // same for bodyA 730 relA = trA.getOrigin() - bodyA_trans.getOrigin(); 731 btVector3 projA = ax1 * relA.dot(ax1); 732 btVector3 orthoA = relA - projA; 733 btVector3 totalDist = projA - projB; 734 // get offset vectors relA and relB 735 relA = orthoA + totalDist * factA; 736 relB = orthoB - totalDist * factB; 737 // now choose average ortho to hinge axis 738 p = orthoB * factA + orthoA * factB; 739 btScalar len2 = p.length2(); 740 if(len2 > SIMD_EPSILON) 741 { 742 p /= btSqrt(len2); 743 } 744 else 745 { 746 p = trA.getBasis().getColumn(1); 747 } 748 // make one more ortho 749 q = ax1.cross(p); 750 // fill three rows 751 tmpA = relA.cross(p); 752 tmpB = relB.cross(p); 753 for (i=0; i<3; i++) info->m_J1angularAxis[s0+i] = tmpA[i]; 754 for (i=0; i<3; i++) info->m_J2angularAxis[s0+i] = -tmpB[i]; 755 tmpA = relA.cross(q); 756 tmpB = relB.cross(q); 757 if(hasStaticBody && getSolveLimit()) 758 { // to make constraint between static and dynamic objects more rigid 759 // remove wA (or wB) from equation if angular limit is hit 760 tmpB *= factB; 761 tmpA *= factA; 762 } 763 for (i=0; i<3; i++) info->m_J1angularAxis[s1+i] = tmpA[i]; 764 for (i=0; i<3; i++) info->m_J2angularAxis[s1+i] = -tmpB[i]; 765 tmpA = relA.cross(ax1); 766 tmpB = relB.cross(ax1); 767 if(hasStaticBody) 768 { // to make constraint between static and dynamic objects more rigid 769 // remove wA (or wB) from equation 770 tmpB *= factB; 771 tmpA *= factA; 772 } 773 for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i]; 774 for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i]; 775 776 btScalar k = info->fps * info->erp; 777 778 if (!m_angularOnly) 779 { 780 for (i=0; i<3; i++) info->m_J1linearAxis[s0+i] = p[i]; 781 for (i=0; i<3; i++) info->m_J1linearAxis[s1+i] = q[i]; 782 for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = ax1[i]; 783 784 // compute three elements of right hand side 785 786 btScalar rhs = k * p.dot(ofs); 787 info->m_constraintError[s0] = rhs; 788 rhs = k * q.dot(ofs); 789 info->m_constraintError[s1] = rhs; 790 rhs = k * ax1.dot(ofs); 791 info->m_constraintError[s2] = rhs; 792 } 793 // the hinge axis should be the only unconstrained 794 // rotational axis, the angular velocity of the two bodies perpendicular to 795 // the hinge axis should be equal. thus the constraint equations are 796 // p*w1 - p*w2 = 0 797 // q*w1 - q*w2 = 0 798 // where p and q are unit vectors normal to the hinge axis, and w1 and w2 799 // are the angular velocity vectors of the two bodies. 800 int s3 = 3 * s; 801 int s4 = 4 * s; 802 info->m_J1angularAxis[s3 + 0] = p[0]; 803 info->m_J1angularAxis[s3 + 1] = p[1]; 804 info->m_J1angularAxis[s3 + 2] = p[2]; 805 info->m_J1angularAxis[s4 + 0] = q[0]; 806 info->m_J1angularAxis[s4 + 1] = q[1]; 807 info->m_J1angularAxis[s4 + 2] = q[2]; 808 809 info->m_J2angularAxis[s3 + 0] = -p[0]; 810 info->m_J2angularAxis[s3 + 1] = -p[1]; 811 info->m_J2angularAxis[s3 + 2] = -p[2]; 812 info->m_J2angularAxis[s4 + 0] = -q[0]; 813 info->m_J2angularAxis[s4 + 1] = -q[1]; 814 info->m_J2angularAxis[s4 + 2] = -q[2]; 815 // compute the right hand side of the constraint equation. set relative 816 // body velocities along p and q to bring the hinge back into alignment. 817 // if ax1A,ax1B are the unit length hinge axes as computed from bodyA and 818 // bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2). 819 // if "theta" is the angle between ax1 and ax2, we need an angular velocity 820 // along u to cover angle erp*theta in one step : 821 // |angular_velocity| = angle/time = erp*theta / stepsize 822 // = (erp*fps) * theta 823 // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| 824 // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) 825 // ...as ax1 and ax2 are unit length. if theta is smallish, 826 // theta ~= sin(theta), so 827 // angular_velocity = (erp*fps) * (ax1 x ax2) 828 // ax1 x ax2 is in the plane space of ax1, so we project the angular 829 // velocity to p and q to find the right hand side. 830 k = info->fps * info->erp; 831 btVector3 u = ax1A.cross(ax1B); 832 info->m_constraintError[s3] = k * u.dot(p); 833 info->m_constraintError[s4] = k * u.dot(q); 834 #endif 835 // check angular limits 836 nrow = 4; // last filled row 837 int srow; 838 btScalar limit_err = btScalar(0.0); 839 int limit = 0; 840 if(getSolveLimit()) 841 { 842 limit_err = m_correction * m_referenceSign; 843 limit = (limit_err > btScalar(0.0)) ? 1 : 2; 844 } 845 // if the hinge has joint limits or motor, add in the extra row 846 int powered = 0; 847 if(getEnableAngularMotor()) 848 { 849 powered = 1; 850 } 851 if(limit || powered) 852 { 853 nrow++; 854 srow = nrow * info->rowskip; 855 info->m_J1angularAxis[srow+0] = ax1[0]; 856 info->m_J1angularAxis[srow+1] = ax1[1]; 857 info->m_J1angularAxis[srow+2] = ax1[2]; 858 859 info->m_J2angularAxis[srow+0] = -ax1[0]; 860 info->m_J2angularAxis[srow+1] = -ax1[1]; 861 info->m_J2angularAxis[srow+2] = -ax1[2]; 862 863 btScalar lostop = getLowerLimit(); 864 btScalar histop = getUpperLimit(); 865 if(limit && (lostop == histop)) 866 { // the joint motor is ineffective 867 powered = 0; 868 } 869 info->m_constraintError[srow] = btScalar(0.0f); 870 btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp; 871 if(powered) 872 { 873 if(m_flags & BT_HINGE_FLAGS_CFM_NORM) 874 { 875 info->cfm[srow] = m_normalCFM; 876 } 877 btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP); 878 info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign; 879 info->m_lowerLimit[srow] = - m_maxMotorImpulse; 880 info->m_upperLimit[srow] = m_maxMotorImpulse; 881 } 882 if(limit) 883 { 884 k = info->fps * currERP; 885 info->m_constraintError[srow] += k * limit_err; 886 if(m_flags & BT_HINGE_FLAGS_CFM_STOP) 887 { 888 info->cfm[srow] = m_stopCFM; 889 } 890 if(lostop == histop) 891 { 892 // limited low and high simultaneously 893 info->m_lowerLimit[srow] = -SIMD_INFINITY; 894 info->m_upperLimit[srow] = SIMD_INFINITY; 895 } 896 else if(limit == 1) 897 { // low limit 898 info->m_lowerLimit[srow] = 0; 899 info->m_upperLimit[srow] = SIMD_INFINITY; 900 } 901 else 902 { // high limit 903 info->m_lowerLimit[srow] = -SIMD_INFINITY; 904 info->m_upperLimit[srow] = 0; 905 } 906 // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that) 907 btScalar bounce = m_relaxationFactor; 908 if(bounce > btScalar(0.0)) 909 { 910 btScalar vel = angVelA.dot(ax1); 911 vel -= angVelB.dot(ax1); 912 // only apply bounce if the velocity is incoming, and if the 913 // resulting c[] exceeds what we already have. 914 if(limit == 1) 915 { // low limit 916 if(vel < 0) 917 { 918 btScalar newc = -bounce * vel; 919 if(newc > info->m_constraintError[srow]) 920 { 921 info->m_constraintError[srow] = newc; 922 } 923 } 924 } 925 else 926 { // high limit - all those computations are reversed 927 if(vel > 0) 928 { 929 btScalar newc = -bounce * vel; 930 if(newc < info->m_constraintError[srow]) 931 { 932 info->m_constraintError[srow] = newc; 933 } 934 } 935 } 936 } 937 info->m_constraintError[srow] *= m_biasFactor; 938 } // if(limit) 939 } // if angular limit or powered 940 } 941 942 943 ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 944 ///If no axis is provided, it uses the default axis for this constraint. 945 void btHingeConstraint::setParam(int num, btScalar value, int axis) 946 { 947 if((axis == -1) || (axis == 5)) 948 { 949 switch(num) 950 { 951 case BT_CONSTRAINT_STOP_ERP : 952 m_stopERP = value; 953 m_flags |= BT_HINGE_FLAGS_ERP_STOP; 954 break; 955 case BT_CONSTRAINT_STOP_CFM : 956 m_stopCFM = value; 957 m_flags |= BT_HINGE_FLAGS_CFM_STOP; 958 break; 959 case BT_CONSTRAINT_CFM : 960 m_normalCFM = value; 961 m_flags |= BT_HINGE_FLAGS_CFM_NORM; 962 break; 963 default : 964 btAssertConstrParams(0); 965 } 966 } 967 else 968 { 969 btAssertConstrParams(0); 970 } 971 } 972 973 ///return the local value of parameter 974 btScalar btHingeConstraint::getParam(int num, int axis) const 975 { 976 btScalar retVal = 0; 977 if((axis == -1) || (axis == 5)) 978 { 979 switch(num) 980 { 981 case BT_CONSTRAINT_STOP_ERP : 982 btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_STOP); 983 retVal = m_stopERP; 984 break; 985 case BT_CONSTRAINT_STOP_CFM : 986 btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_STOP); 987 retVal = m_stopCFM; 988 break; 989 case BT_CONSTRAINT_CFM : 990 btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_NORM); 991 retVal = m_normalCFM; 992 break; 993 default : 994 btAssertConstrParams(0); 995 } 996 } 997 else 998 { 999 btAssertConstrParams(0); 1000 } 1001 return retVal; 1002 } 1003 1004
Note: See TracChangeset
for help on using the changeset viewer.