00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "btHingeConstraint.h"
00018 #include "BulletDynamics/Dynamics/btRigidBody.h"
00019 #include "LinearMath/btTransformUtil.h"
00020 #include "LinearMath/btMinMax.h"
00021 #include <new>
00022 #include "btSolverBody.h"
00023
00024
00025
00026
00027 #define HINGE_USE_OBSOLETE_SOLVER false
00028
00029 #define HINGE_USE_FRAME_OFFSET true
00030
00031 #ifndef __SPU__
00032
00033
00034
00035
00036
00037 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
00038 const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA)
00039 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
00040 #ifdef _BT_USE_CENTER_LIMIT_
00041 m_limit(),
00042 #endif
00043 m_angularOnly(false),
00044 m_enableAngularMotor(false),
00045 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
00046 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
00047 m_useReferenceFrameA(useReferenceFrameA),
00048 m_flags(0)
00049 {
00050 m_rbAFrame.getOrigin() = pivotInA;
00051
00052
00053 btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);
00054
00055 btVector3 rbAxisA2;
00056 btScalar projection = axisInA.dot(rbAxisA1);
00057 if (projection >= 1.0f - SIMD_EPSILON) {
00058 rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2);
00059 rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
00060 } else if (projection <= -1.0f + SIMD_EPSILON) {
00061 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2);
00062 rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
00063 } else {
00064 rbAxisA2 = axisInA.cross(rbAxisA1);
00065 rbAxisA1 = rbAxisA2.cross(axisInA);
00066 }
00067
00068 m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
00069 rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
00070 rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
00071
00072 btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
00073 btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1);
00074 btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
00075
00076 m_rbBFrame.getOrigin() = pivotInB;
00077 m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
00078 rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
00079 rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
00080
00081 #ifndef _BT_USE_CENTER_LIMIT_
00082
00083 m_lowerLimit = btScalar(1.0f);
00084 m_upperLimit = btScalar(-1.0f);
00085 m_biasFactor = 0.3f;
00086 m_relaxationFactor = 1.0f;
00087 m_limitSoftness = 0.9f;
00088 m_solveLimit = false;
00089 #endif
00090 m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
00091 }
00092
00093
00094
00095 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA)
00096 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),
00097 #ifdef _BT_USE_CENTER_LIMIT_
00098 m_limit(),
00099 #endif
00100 m_angularOnly(false), m_enableAngularMotor(false),
00101 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
00102 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
00103 m_useReferenceFrameA(useReferenceFrameA),
00104 m_flags(0)
00105 {
00106
00107
00108
00109 btVector3 rbAxisA1, rbAxisA2;
00110 btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2);
00111
00112 m_rbAFrame.getOrigin() = pivotInA;
00113 m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
00114 rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
00115 rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
00116
00117 btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA;
00118
00119 btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
00120 btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1);
00121 btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
00122
00123
00124 m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA);
00125 m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
00126 rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
00127 rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
00128
00129 #ifndef _BT_USE_CENTER_LIMIT_
00130
00131 m_lowerLimit = btScalar(1.0f);
00132 m_upperLimit = btScalar(-1.0f);
00133 m_biasFactor = 0.3f;
00134 m_relaxationFactor = 1.0f;
00135 m_limitSoftness = 0.9f;
00136 m_solveLimit = false;
00137 #endif
00138 m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
00139 }
00140
00141
00142
00143 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB,
00144 const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA)
00145 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
00146 #ifdef _BT_USE_CENTER_LIMIT_
00147 m_limit(),
00148 #endif
00149 m_angularOnly(false),
00150 m_enableAngularMotor(false),
00151 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
00152 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
00153 m_useReferenceFrameA(useReferenceFrameA),
00154 m_flags(0)
00155 {
00156 #ifndef _BT_USE_CENTER_LIMIT_
00157
00158 m_lowerLimit = btScalar(1.0f);
00159 m_upperLimit = btScalar(-1.0f);
00160 m_biasFactor = 0.3f;
00161 m_relaxationFactor = 1.0f;
00162 m_limitSoftness = 0.9f;
00163 m_solveLimit = false;
00164 #endif
00165 m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
00166 }
00167
00168
00169
00170 btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA)
00171 :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame),
00172 #ifdef _BT_USE_CENTER_LIMIT_
00173 m_limit(),
00174 #endif
00175 m_angularOnly(false),
00176 m_enableAngularMotor(false),
00177 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
00178 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
00179 m_useReferenceFrameA(useReferenceFrameA),
00180 m_flags(0)
00181 {
00183
00184 m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin());
00185 #ifndef _BT_USE_CENTER_LIMIT_
00186
00187 m_lowerLimit = btScalar(1.0f);
00188 m_upperLimit = btScalar(-1.0f);
00189 m_biasFactor = 0.3f;
00190 m_relaxationFactor = 1.0f;
00191 m_limitSoftness = 0.9f;
00192 m_solveLimit = false;
00193 #endif
00194 m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
00195 }
00196
00197
00198
00199 void btHingeConstraint::buildJacobian()
00200 {
00201 if (m_useSolveConstraintObsolete)
00202 {
00203 m_appliedImpulse = btScalar(0.);
00204 m_accMotorImpulse = btScalar(0.);
00205
00206 if (!m_angularOnly)
00207 {
00208 btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
00209 btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
00210 btVector3 relPos = pivotBInW - pivotAInW;
00211
00212 btVector3 normal[3];
00213 if (relPos.length2() > SIMD_EPSILON)
00214 {
00215 normal[0] = relPos.normalized();
00216 }
00217 else
00218 {
00219 normal[0].setValue(btScalar(1.0),0,0);
00220 }
00221
00222 btPlaneSpace1(normal[0], normal[1], normal[2]);
00223
00224 for (int i=0;i<3;i++)
00225 {
00226 new (&m_jac[i]) btJacobianEntry(
00227 m_rbA.getCenterOfMassTransform().getBasis().transpose(),
00228 m_rbB.getCenterOfMassTransform().getBasis().transpose(),
00229 pivotAInW - m_rbA.getCenterOfMassPosition(),
00230 pivotBInW - m_rbB.getCenterOfMassPosition(),
00231 normal[i],
00232 m_rbA.getInvInertiaDiagLocal(),
00233 m_rbA.getInvMass(),
00234 m_rbB.getInvInertiaDiagLocal(),
00235 m_rbB.getInvMass());
00236 }
00237 }
00238
00239
00240
00241
00242
00243 btVector3 jointAxis0local;
00244 btVector3 jointAxis1local;
00245
00246 btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
00247
00248 btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
00249 btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
00250 btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
00251
00252 new (&m_jacAng[0]) btJacobianEntry(jointAxis0,
00253 m_rbA.getCenterOfMassTransform().getBasis().transpose(),
00254 m_rbB.getCenterOfMassTransform().getBasis().transpose(),
00255 m_rbA.getInvInertiaDiagLocal(),
00256 m_rbB.getInvInertiaDiagLocal());
00257
00258 new (&m_jacAng[1]) btJacobianEntry(jointAxis1,
00259 m_rbA.getCenterOfMassTransform().getBasis().transpose(),
00260 m_rbB.getCenterOfMassTransform().getBasis().transpose(),
00261 m_rbA.getInvInertiaDiagLocal(),
00262 m_rbB.getInvInertiaDiagLocal());
00263
00264 new (&m_jacAng[2]) btJacobianEntry(hingeAxisWorld,
00265 m_rbA.getCenterOfMassTransform().getBasis().transpose(),
00266 m_rbB.getCenterOfMassTransform().getBasis().transpose(),
00267 m_rbA.getInvInertiaDiagLocal(),
00268 m_rbB.getInvInertiaDiagLocal());
00269
00270
00271 m_accLimitImpulse = btScalar(0.);
00272
00273
00274 testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
00275
00276
00277 btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
00278 m_kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
00279 getRigidBodyB().computeAngularImpulseDenominator(axisA));
00280
00281 }
00282 }
00283
00284
00285 #endif //__SPU__
00286
00287
00288 void btHingeConstraint::getInfo1(btConstraintInfo1* info)
00289 {
00290 if (m_useSolveConstraintObsolete)
00291 {
00292 info->m_numConstraintRows = 0;
00293 info->nub = 0;
00294 }
00295 else
00296 {
00297 info->m_numConstraintRows = 5;
00298 info->nub = 1;
00299
00300
00301 testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
00302 if(getSolveLimit() || getEnableAngularMotor())
00303 {
00304 info->m_numConstraintRows++;
00305 info->nub--;
00306 }
00307
00308 }
00309 }
00310
00311 void btHingeConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
00312 {
00313 if (m_useSolveConstraintObsolete)
00314 {
00315 info->m_numConstraintRows = 0;
00316 info->nub = 0;
00317 }
00318 else
00319 {
00320
00321 info->m_numConstraintRows = 6;
00322 info->nub = 0;
00323 }
00324 }
00325
00326 void btHingeConstraint::getInfo2 (btConstraintInfo2* info)
00327 {
00328 if(m_useOffsetForConstraintFrame)
00329 {
00330 getInfo2InternalUsingFrameOffset(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity());
00331 }
00332 else
00333 {
00334 getInfo2Internal(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity());
00335 }
00336 }
00337
00338
00339 void btHingeConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
00340 {
00342 testLimit(transA,transB);
00343
00344 getInfo2Internal(info,transA,transB,angVelA,angVelB);
00345 }
00346
00347
00348 void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
00349 {
00350
00351 btAssert(!m_useSolveConstraintObsolete);
00352 int i, skip = info->rowskip;
00353
00354 btTransform trA = transA*m_rbAFrame;
00355 btTransform trB = transB*m_rbBFrame;
00356
00357 btVector3 pivotAInW = trA.getOrigin();
00358 btVector3 pivotBInW = trB.getOrigin();
00359 #if 0
00360 if (0)
00361 {
00362 for (i=0;i<6;i++)
00363 {
00364 info->m_J1linearAxis[i*skip]=0;
00365 info->m_J1linearAxis[i*skip+1]=0;
00366 info->m_J1linearAxis[i*skip+2]=0;
00367
00368 info->m_J1angularAxis[i*skip]=0;
00369 info->m_J1angularAxis[i*skip+1]=0;
00370 info->m_J1angularAxis[i*skip+2]=0;
00371
00372 info->m_J2angularAxis[i*skip]=0;
00373 info->m_J2angularAxis[i*skip+1]=0;
00374 info->m_J2angularAxis[i*skip+2]=0;
00375
00376 info->m_constraintError[i*skip]=0.f;
00377 }
00378 }
00379 #endif //#if 0
00380
00381
00382 if (!m_angularOnly)
00383 {
00384 info->m_J1linearAxis[0] = 1;
00385 info->m_J1linearAxis[skip + 1] = 1;
00386 info->m_J1linearAxis[2 * skip + 2] = 1;
00387 }
00388
00389
00390
00391
00392 btVector3 a1 = pivotAInW - transA.getOrigin();
00393 {
00394 btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
00395 btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + skip);
00396 btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * skip);
00397 btVector3 a1neg = -a1;
00398 a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
00399 }
00400 btVector3 a2 = pivotBInW - transB.getOrigin();
00401 {
00402 btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
00403 btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + skip);
00404 btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * skip);
00405 a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
00406 }
00407
00408 btScalar k = info->fps * info->erp;
00409 if (!m_angularOnly)
00410 {
00411 for(i = 0; i < 3; i++)
00412 {
00413 info->m_constraintError[i * skip] = k * (pivotBInW[i] - pivotAInW[i]);
00414 }
00415 }
00416
00417
00418
00419
00420
00421
00422
00423
00424
00425 btVector3 ax1 = trA.getBasis().getColumn(2);
00426
00427 btVector3 p = trA.getBasis().getColumn(0);
00428 btVector3 q = trA.getBasis().getColumn(1);
00429
00430 int s3 = 3 * info->rowskip;
00431 int s4 = 4 * info->rowskip;
00432
00433 info->m_J1angularAxis[s3 + 0] = p[0];
00434 info->m_J1angularAxis[s3 + 1] = p[1];
00435 info->m_J1angularAxis[s3 + 2] = p[2];
00436 info->m_J1angularAxis[s4 + 0] = q[0];
00437 info->m_J1angularAxis[s4 + 1] = q[1];
00438 info->m_J1angularAxis[s4 + 2] = q[2];
00439
00440 info->m_J2angularAxis[s3 + 0] = -p[0];
00441 info->m_J2angularAxis[s3 + 1] = -p[1];
00442 info->m_J2angularAxis[s3 + 2] = -p[2];
00443 info->m_J2angularAxis[s4 + 0] = -q[0];
00444 info->m_J2angularAxis[s4 + 1] = -q[1];
00445 info->m_J2angularAxis[s4 + 2] = -q[2];
00446
00447
00448
00449
00450
00451
00452
00453
00454
00455
00456
00457
00458
00459
00460
00461 btVector3 ax2 = trB.getBasis().getColumn(2);
00462 btVector3 u = ax1.cross(ax2);
00463 info->m_constraintError[s3] = k * u.dot(p);
00464 info->m_constraintError[s4] = k * u.dot(q);
00465
00466 int nrow = 4;
00467 int srow;
00468 btScalar limit_err = btScalar(0.0);
00469 int limit = 0;
00470 if(getSolveLimit())
00471 {
00472 #ifdef _BT_USE_CENTER_LIMIT_
00473 limit_err = m_limit.getCorrection() * m_referenceSign;
00474 #else
00475 limit_err = m_correction * m_referenceSign;
00476 #endif
00477 limit = (limit_err > btScalar(0.0)) ? 1 : 2;
00478
00479 }
00480
00481 int powered = 0;
00482 if(getEnableAngularMotor())
00483 {
00484 powered = 1;
00485 }
00486 if(limit || powered)
00487 {
00488 nrow++;
00489 srow = nrow * info->rowskip;
00490 info->m_J1angularAxis[srow+0] = ax1[0];
00491 info->m_J1angularAxis[srow+1] = ax1[1];
00492 info->m_J1angularAxis[srow+2] = ax1[2];
00493
00494 info->m_J2angularAxis[srow+0] = -ax1[0];
00495 info->m_J2angularAxis[srow+1] = -ax1[1];
00496 info->m_J2angularAxis[srow+2] = -ax1[2];
00497
00498 btScalar lostop = getLowerLimit();
00499 btScalar histop = getUpperLimit();
00500 if(limit && (lostop == histop))
00501 {
00502 powered = 0;
00503 }
00504 info->m_constraintError[srow] = btScalar(0.0f);
00505 btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp;
00506 if(powered)
00507 {
00508 if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
00509 {
00510 info->cfm[srow] = m_normalCFM;
00511 }
00512 btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
00513 info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
00514 info->m_lowerLimit[srow] = - m_maxMotorImpulse;
00515 info->m_upperLimit[srow] = m_maxMotorImpulse;
00516 }
00517 if(limit)
00518 {
00519 k = info->fps * currERP;
00520 info->m_constraintError[srow] += k * limit_err;
00521 if(m_flags & BT_HINGE_FLAGS_CFM_STOP)
00522 {
00523 info->cfm[srow] = m_stopCFM;
00524 }
00525 if(lostop == histop)
00526 {
00527
00528 info->m_lowerLimit[srow] = -SIMD_INFINITY;
00529 info->m_upperLimit[srow] = SIMD_INFINITY;
00530 }
00531 else if(limit == 1)
00532 {
00533 info->m_lowerLimit[srow] = 0;
00534 info->m_upperLimit[srow] = SIMD_INFINITY;
00535 }
00536 else
00537 {
00538 info->m_lowerLimit[srow] = -SIMD_INFINITY;
00539 info->m_upperLimit[srow] = 0;
00540 }
00541
00542 #ifdef _BT_USE_CENTER_LIMIT_
00543 btScalar bounce = m_limit.getRelaxationFactor();
00544 #else
00545 btScalar bounce = m_relaxationFactor;
00546 #endif
00547 if(bounce > btScalar(0.0))
00548 {
00549 btScalar vel = angVelA.dot(ax1);
00550 vel -= angVelB.dot(ax1);
00551
00552
00553 if(limit == 1)
00554 {
00555 if(vel < 0)
00556 {
00557 btScalar newc = -bounce * vel;
00558 if(newc > info->m_constraintError[srow])
00559 {
00560 info->m_constraintError[srow] = newc;
00561 }
00562 }
00563 }
00564 else
00565 {
00566 if(vel > 0)
00567 {
00568 btScalar newc = -bounce * vel;
00569 if(newc < info->m_constraintError[srow])
00570 {
00571 info->m_constraintError[srow] = newc;
00572 }
00573 }
00574 }
00575 }
00576 #ifdef _BT_USE_CENTER_LIMIT_
00577 info->m_constraintError[srow] *= m_limit.getBiasFactor();
00578 #else
00579 info->m_constraintError[srow] *= m_biasFactor;
00580 #endif
00581 }
00582 }
00583 }
00584
00585
00586 void btHingeConstraint::setFrames(const btTransform & frameA, const btTransform & frameB)
00587 {
00588 m_rbAFrame = frameA;
00589 m_rbBFrame = frameB;
00590 buildJacobian();
00591 }
00592
00593
00594 void btHingeConstraint::updateRHS(btScalar timeStep)
00595 {
00596 (void)timeStep;
00597
00598 }
00599
00600
00601 btScalar btHingeConstraint::getHingeAngle()
00602 {
00603 return getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
00604 }
00605
00606 btScalar btHingeConstraint::getHingeAngle(const btTransform& transA,const btTransform& transB)
00607 {
00608 const btVector3 refAxis0 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
00609 const btVector3 refAxis1 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
00610 const btVector3 swingAxis = transB.getBasis() * m_rbBFrame.getBasis().getColumn(1);
00611
00612 btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
00613 return m_referenceSign * angle;
00614 }
00615
00616
00617
00618 void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& transB)
00619 {
00620
00621 m_hingeAngle = getHingeAngle(transA,transB);
00622 #ifdef _BT_USE_CENTER_LIMIT_
00623 m_limit.test(m_hingeAngle);
00624 #else
00625 m_correction = btScalar(0.);
00626 m_limitSign = btScalar(0.);
00627 m_solveLimit = false;
00628 if (m_lowerLimit <= m_upperLimit)
00629 {
00630 m_hingeAngle = btAdjustAngleToLimits(m_hingeAngle, m_lowerLimit, m_upperLimit);
00631 if (m_hingeAngle <= m_lowerLimit)
00632 {
00633 m_correction = (m_lowerLimit - m_hingeAngle);
00634 m_limitSign = 1.0f;
00635 m_solveLimit = true;
00636 }
00637 else if (m_hingeAngle >= m_upperLimit)
00638 {
00639 m_correction = m_upperLimit - m_hingeAngle;
00640 m_limitSign = -1.0f;
00641 m_solveLimit = true;
00642 }
00643 }
00644 #endif
00645 return;
00646 }
00647
00648
00649 static btVector3 vHinge(0, 0, btScalar(1));
00650
00651 void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt)
00652 {
00653
00654 btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * qAinB * m_rbAFrame.getRotation();
00655 qConstraint.normalize();
00656
00657
00658 btVector3 vNoHinge = quatRotate(qConstraint, vHinge); vNoHinge.normalize();
00659 btQuaternion qNoHinge = shortestArcQuat(vHinge, vNoHinge);
00660 btQuaternion qHinge = qNoHinge.inverse() * qConstraint;
00661 qHinge.normalize();
00662
00663
00664 btScalar targetAngle = qHinge.getAngle();
00665 if (targetAngle > SIMD_PI)
00666 {
00667 qHinge = -(qHinge);
00668 targetAngle = qHinge.getAngle();
00669 }
00670 if (qHinge.getZ() < 0)
00671 targetAngle = -targetAngle;
00672
00673 setMotorTarget(targetAngle, dt);
00674 }
00675
00676 void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt)
00677 {
00678 #ifdef _BT_USE_CENTER_LIMIT_
00679 m_limit.fit(targetAngle);
00680 #else
00681 if (m_lowerLimit < m_upperLimit)
00682 {
00683 if (targetAngle < m_lowerLimit)
00684 targetAngle = m_lowerLimit;
00685 else if (targetAngle > m_upperLimit)
00686 targetAngle = m_upperLimit;
00687 }
00688 #endif
00689
00690 btScalar curAngle = getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
00691 btScalar dAngle = targetAngle - curAngle;
00692 m_motorTargetVelocity = dAngle / dt;
00693 }
00694
00695
00696
00697 void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
00698 {
00699 btAssert(!m_useSolveConstraintObsolete);
00700 int i, s = info->rowskip;
00701
00702 btTransform trA = transA*m_rbAFrame;
00703 btTransform trB = transB*m_rbBFrame;
00704
00705
00706
00707 #if 1
00708
00709 btVector3 ofs = trB.getOrigin() - trA.getOrigin();
00710
00711 btScalar miA = getRigidBodyA().getInvMass();
00712 btScalar miB = getRigidBodyB().getInvMass();
00713 bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
00714 btScalar miS = miA + miB;
00715 btScalar factA, factB;
00716 if(miS > btScalar(0.f))
00717 {
00718 factA = miB / miS;
00719 }
00720 else
00721 {
00722 factA = btScalar(0.5f);
00723 }
00724 factB = btScalar(1.0f) - factA;
00725
00726
00727 btVector3 ax1A = trA.getBasis().getColumn(2);
00728 btVector3 ax1B = trB.getBasis().getColumn(2);
00729 btVector3 ax1 = ax1A * factA + ax1B * factB;
00730 ax1.normalize();
00731
00732
00733 btTransform bodyA_trans = transA;
00734 btTransform bodyB_trans = transB;
00735 int s0 = 0;
00736 int s1 = s;
00737 int s2 = s * 2;
00738 int nrow = 2;
00739 btVector3 tmpA, tmpB, relA, relB, p, q;
00740
00741 relB = trB.getOrigin() - bodyB_trans.getOrigin();
00742
00743 btVector3 projB = ax1 * relB.dot(ax1);
00744
00745 btVector3 orthoB = relB - projB;
00746
00747 relA = trA.getOrigin() - bodyA_trans.getOrigin();
00748 btVector3 projA = ax1 * relA.dot(ax1);
00749 btVector3 orthoA = relA - projA;
00750 btVector3 totalDist = projA - projB;
00751
00752 relA = orthoA + totalDist * factA;
00753 relB = orthoB - totalDist * factB;
00754
00755 p = orthoB * factA + orthoA * factB;
00756 btScalar len2 = p.length2();
00757 if(len2 > SIMD_EPSILON)
00758 {
00759 p /= btSqrt(len2);
00760 }
00761 else
00762 {
00763 p = trA.getBasis().getColumn(1);
00764 }
00765
00766 q = ax1.cross(p);
00767
00768 tmpA = relA.cross(p);
00769 tmpB = relB.cross(p);
00770 for (i=0; i<3; i++) info->m_J1angularAxis[s0+i] = tmpA[i];
00771 for (i=0; i<3; i++) info->m_J2angularAxis[s0+i] = -tmpB[i];
00772 tmpA = relA.cross(q);
00773 tmpB = relB.cross(q);
00774 if(hasStaticBody && getSolveLimit())
00775 {
00776
00777 tmpB *= factB;
00778 tmpA *= factA;
00779 }
00780 for (i=0; i<3; i++) info->m_J1angularAxis[s1+i] = tmpA[i];
00781 for (i=0; i<3; i++) info->m_J2angularAxis[s1+i] = -tmpB[i];
00782 tmpA = relA.cross(ax1);
00783 tmpB = relB.cross(ax1);
00784 if(hasStaticBody)
00785 {
00786
00787 tmpB *= factB;
00788 tmpA *= factA;
00789 }
00790 for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i];
00791 for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i];
00792
00793 btScalar k = info->fps * info->erp;
00794
00795 if (!m_angularOnly)
00796 {
00797 for (i=0; i<3; i++) info->m_J1linearAxis[s0+i] = p[i];
00798 for (i=0; i<3; i++) info->m_J1linearAxis[s1+i] = q[i];
00799 for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = ax1[i];
00800
00801
00802
00803 btScalar rhs = k * p.dot(ofs);
00804 info->m_constraintError[s0] = rhs;
00805 rhs = k * q.dot(ofs);
00806 info->m_constraintError[s1] = rhs;
00807 rhs = k * ax1.dot(ofs);
00808 info->m_constraintError[s2] = rhs;
00809 }
00810
00811
00812
00813
00814
00815
00816
00817 int s3 = 3 * s;
00818 int s4 = 4 * s;
00819 info->m_J1angularAxis[s3 + 0] = p[0];
00820 info->m_J1angularAxis[s3 + 1] = p[1];
00821 info->m_J1angularAxis[s3 + 2] = p[2];
00822 info->m_J1angularAxis[s4 + 0] = q[0];
00823 info->m_J1angularAxis[s4 + 1] = q[1];
00824 info->m_J1angularAxis[s4 + 2] = q[2];
00825
00826 info->m_J2angularAxis[s3 + 0] = -p[0];
00827 info->m_J2angularAxis[s3 + 1] = -p[1];
00828 info->m_J2angularAxis[s3 + 2] = -p[2];
00829 info->m_J2angularAxis[s4 + 0] = -q[0];
00830 info->m_J2angularAxis[s4 + 1] = -q[1];
00831 info->m_J2angularAxis[s4 + 2] = -q[2];
00832
00833
00834
00835
00836
00837
00838
00839
00840
00841
00842
00843
00844
00845
00846
00847 k = info->fps * info->erp;
00848 btVector3 u = ax1A.cross(ax1B);
00849 info->m_constraintError[s3] = k * u.dot(p);
00850 info->m_constraintError[s4] = k * u.dot(q);
00851 #endif
00852
00853 nrow = 4;
00854 int srow;
00855 btScalar limit_err = btScalar(0.0);
00856 int limit = 0;
00857 if(getSolveLimit())
00858 {
00859 #ifdef _BT_USE_CENTER_LIMIT_
00860 limit_err = m_limit.getCorrection() * m_referenceSign;
00861 #else
00862 limit_err = m_correction * m_referenceSign;
00863 #endif
00864 limit = (limit_err > btScalar(0.0)) ? 1 : 2;
00865
00866 }
00867
00868 int powered = 0;
00869 if(getEnableAngularMotor())
00870 {
00871 powered = 1;
00872 }
00873 if(limit || powered)
00874 {
00875 nrow++;
00876 srow = nrow * info->rowskip;
00877 info->m_J1angularAxis[srow+0] = ax1[0];
00878 info->m_J1angularAxis[srow+1] = ax1[1];
00879 info->m_J1angularAxis[srow+2] = ax1[2];
00880
00881 info->m_J2angularAxis[srow+0] = -ax1[0];
00882 info->m_J2angularAxis[srow+1] = -ax1[1];
00883 info->m_J2angularAxis[srow+2] = -ax1[2];
00884
00885 btScalar lostop = getLowerLimit();
00886 btScalar histop = getUpperLimit();
00887 if(limit && (lostop == histop))
00888 {
00889 powered = 0;
00890 }
00891 info->m_constraintError[srow] = btScalar(0.0f);
00892 btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp;
00893 if(powered)
00894 {
00895 if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
00896 {
00897 info->cfm[srow] = m_normalCFM;
00898 }
00899 btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
00900 info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
00901 info->m_lowerLimit[srow] = - m_maxMotorImpulse;
00902 info->m_upperLimit[srow] = m_maxMotorImpulse;
00903 }
00904 if(limit)
00905 {
00906 k = info->fps * currERP;
00907 info->m_constraintError[srow] += k * limit_err;
00908 if(m_flags & BT_HINGE_FLAGS_CFM_STOP)
00909 {
00910 info->cfm[srow] = m_stopCFM;
00911 }
00912 if(lostop == histop)
00913 {
00914
00915 info->m_lowerLimit[srow] = -SIMD_INFINITY;
00916 info->m_upperLimit[srow] = SIMD_INFINITY;
00917 }
00918 else if(limit == 1)
00919 {
00920 info->m_lowerLimit[srow] = 0;
00921 info->m_upperLimit[srow] = SIMD_INFINITY;
00922 }
00923 else
00924 {
00925 info->m_lowerLimit[srow] = -SIMD_INFINITY;
00926 info->m_upperLimit[srow] = 0;
00927 }
00928
00929 #ifdef _BT_USE_CENTER_LIMIT_
00930 btScalar bounce = m_limit.getRelaxationFactor();
00931 #else
00932 btScalar bounce = m_relaxationFactor;
00933 #endif
00934 if(bounce > btScalar(0.0))
00935 {
00936 btScalar vel = angVelA.dot(ax1);
00937 vel -= angVelB.dot(ax1);
00938
00939
00940 if(limit == 1)
00941 {
00942 if(vel < 0)
00943 {
00944 btScalar newc = -bounce * vel;
00945 if(newc > info->m_constraintError[srow])
00946 {
00947 info->m_constraintError[srow] = newc;
00948 }
00949 }
00950 }
00951 else
00952 {
00953 if(vel > 0)
00954 {
00955 btScalar newc = -bounce * vel;
00956 if(newc < info->m_constraintError[srow])
00957 {
00958 info->m_constraintError[srow] = newc;
00959 }
00960 }
00961 }
00962 }
00963 #ifdef _BT_USE_CENTER_LIMIT_
00964 info->m_constraintError[srow] *= m_limit.getBiasFactor();
00965 #else
00966 info->m_constraintError[srow] *= m_biasFactor;
00967 #endif
00968 }
00969 }
00970 }
00971
00972
00975 void btHingeConstraint::setParam(int num, btScalar value, int axis)
00976 {
00977 if((axis == -1) || (axis == 5))
00978 {
00979 switch(num)
00980 {
00981 case BT_CONSTRAINT_STOP_ERP :
00982 m_stopERP = value;
00983 m_flags |= BT_HINGE_FLAGS_ERP_STOP;
00984 break;
00985 case BT_CONSTRAINT_STOP_CFM :
00986 m_stopCFM = value;
00987 m_flags |= BT_HINGE_FLAGS_CFM_STOP;
00988 break;
00989 case BT_CONSTRAINT_CFM :
00990 m_normalCFM = value;
00991 m_flags |= BT_HINGE_FLAGS_CFM_NORM;
00992 break;
00993 default :
00994 btAssertConstrParams(0);
00995 }
00996 }
00997 else
00998 {
00999 btAssertConstrParams(0);
01000 }
01001 }
01002
01004 btScalar btHingeConstraint::getParam(int num, int axis) const
01005 {
01006 btScalar retVal = 0;
01007 if((axis == -1) || (axis == 5))
01008 {
01009 switch(num)
01010 {
01011 case BT_CONSTRAINT_STOP_ERP :
01012 btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_STOP);
01013 retVal = m_stopERP;
01014 break;
01015 case BT_CONSTRAINT_STOP_CFM :
01016 btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_STOP);
01017 retVal = m_stopCFM;
01018 break;
01019 case BT_CONSTRAINT_CFM :
01020 btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_NORM);
01021 retVal = m_normalCFM;
01022 break;
01023 default :
01024 btAssertConstrParams(0);
01025 }
01026 }
01027 else
01028 {
01029 btAssertConstrParams(0);
01030 }
01031 return retVal;
01032 }
01033
01034