00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022 #include "btGeneric6DofConstraint.h"
00023 #include "BulletDynamics/Dynamics/btRigidBody.h"
00024 #include "LinearMath/btTransformUtil.h"
00025 #include "LinearMath/btTransformUtil.h"
00026 #include <new>
00027
00028
00029
00030 #define D6_USE_OBSOLETE_METHOD false
00031 #define D6_USE_FRAME_OFFSET true
00032
00033
00034
00035
00036
00037
00038 btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
00039 : btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB)
00040 , m_frameInA(frameInA)
00041 , m_frameInB(frameInB),
00042 m_useLinearReferenceFrameA(useLinearReferenceFrameA),
00043 m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
00044 m_flags(0),
00045 m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
00046 {
00047 calculateTransforms();
00048 }
00049
00050
00051
00052 btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB)
00053 : btTypedConstraint(D6_CONSTRAINT_TYPE, getFixedBody(), rbB),
00054 m_frameInB(frameInB),
00055 m_useLinearReferenceFrameA(useLinearReferenceFrameB),
00056 m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
00057 m_flags(0),
00058 m_useSolveConstraintObsolete(false)
00059 {
00061 m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB;
00062 calculateTransforms();
00063 }
00064
00065
00066
00067
00068 #define GENERIC_D6_DISABLE_WARMSTARTING 1
00069
00070
00071
00072 btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
00073 btScalar btGetMatrixElem(const btMatrix3x3& mat, int index)
00074 {
00075 int i = index%3;
00076 int j = index/3;
00077 return mat[i][j];
00078 }
00079
00080
00081
00083 bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);
00084 bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
00085 {
00086
00087
00088
00089
00090
00091 btScalar fi = btGetMatrixElem(mat,2);
00092 if (fi < btScalar(1.0f))
00093 {
00094 if (fi > btScalar(-1.0f))
00095 {
00096 xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
00097 xyz[1] = btAsin(btGetMatrixElem(mat,2));
00098 xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
00099 return true;
00100 }
00101 else
00102 {
00103
00104 xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
00105 xyz[1] = -SIMD_HALF_PI;
00106 xyz[2] = btScalar(0.0);
00107 return false;
00108 }
00109 }
00110 else
00111 {
00112
00113 xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
00114 xyz[1] = SIMD_HALF_PI;
00115 xyz[2] = 0.0;
00116 }
00117 return false;
00118 }
00119
00121
00122 int btRotationalLimitMotor::testLimitValue(btScalar test_value)
00123 {
00124 if(m_loLimit>m_hiLimit)
00125 {
00126 m_currentLimit = 0;
00127 return 0;
00128 }
00129 if (test_value < m_loLimit)
00130 {
00131 m_currentLimit = 1;
00132 m_currentLimitError = test_value - m_loLimit;
00133 if(m_currentLimitError>SIMD_PI)
00134 m_currentLimitError-=SIMD_2_PI;
00135 else if(m_currentLimitError<-SIMD_PI)
00136 m_currentLimitError+=SIMD_2_PI;
00137 return 1;
00138 }
00139 else if (test_value> m_hiLimit)
00140 {
00141 m_currentLimit = 2;
00142 m_currentLimitError = test_value - m_hiLimit;
00143 if(m_currentLimitError>SIMD_PI)
00144 m_currentLimitError-=SIMD_2_PI;
00145 else if(m_currentLimitError<-SIMD_PI)
00146 m_currentLimitError+=SIMD_2_PI;
00147 return 2;
00148 };
00149
00150 m_currentLimit = 0;
00151 return 0;
00152
00153 }
00154
00155
00156
00157 btScalar btRotationalLimitMotor::solveAngularLimits(
00158 btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
00159 btRigidBody * body0, btRigidBody * body1 )
00160 {
00161 if (needApplyTorques()==false) return 0.0f;
00162
00163 btScalar target_velocity = m_targetVelocity;
00164 btScalar maxMotorForce = m_maxMotorForce;
00165
00166
00167 if (m_currentLimit!=0)
00168 {
00169 target_velocity = -m_stopERP*m_currentLimitError/(timeStep);
00170 maxMotorForce = m_maxLimitForce;
00171 }
00172
00173 maxMotorForce *= timeStep;
00174
00175
00176
00177 btVector3 angVelA = body0->getAngularVelocity();
00178 btVector3 angVelB = body1->getAngularVelocity();
00179
00180 btVector3 vel_diff;
00181 vel_diff = angVelA-angVelB;
00182
00183
00184
00185 btScalar rel_vel = axis.dot(vel_diff);
00186
00187
00188 btScalar motor_relvel = m_limitSoftness*(target_velocity - m_damping*rel_vel);
00189
00190
00191 if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON )
00192 {
00193 return 0.0f;
00194 }
00195
00196
00197
00198 btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv;
00199
00200
00201 btScalar clippedMotorImpulse;
00202
00204 if (unclippedMotorImpulse>0.0f)
00205 {
00206 clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
00207 }
00208 else
00209 {
00210 clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
00211 }
00212
00213
00214
00215 btScalar lo = btScalar(-BT_LARGE_FLOAT);
00216 btScalar hi = btScalar(BT_LARGE_FLOAT);
00217
00218 btScalar oldaccumImpulse = m_accumulatedImpulse;
00219 btScalar sum = oldaccumImpulse + clippedMotorImpulse;
00220 m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
00221
00222 clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
00223
00224 btVector3 motorImp = clippedMotorImpulse * axis;
00225
00226 body0->applyTorqueImpulse(motorImp);
00227 body1->applyTorqueImpulse(-motorImp);
00228
00229 return clippedMotorImpulse;
00230
00231
00232 }
00233
00235
00236
00237
00238
00240
00241
00242 int btTranslationalLimitMotor::testLimitValue(int limitIndex, btScalar test_value)
00243 {
00244 btScalar loLimit = m_lowerLimit[limitIndex];
00245 btScalar hiLimit = m_upperLimit[limitIndex];
00246 if(loLimit > hiLimit)
00247 {
00248 m_currentLimit[limitIndex] = 0;
00249 m_currentLimitError[limitIndex] = btScalar(0.f);
00250 return 0;
00251 }
00252
00253 if (test_value < loLimit)
00254 {
00255 m_currentLimit[limitIndex] = 2;
00256 m_currentLimitError[limitIndex] = test_value - loLimit;
00257 return 2;
00258 }
00259 else if (test_value> hiLimit)
00260 {
00261 m_currentLimit[limitIndex] = 1;
00262 m_currentLimitError[limitIndex] = test_value - hiLimit;
00263 return 1;
00264 };
00265
00266 m_currentLimit[limitIndex] = 0;
00267 m_currentLimitError[limitIndex] = btScalar(0.f);
00268 return 0;
00269 }
00270
00271
00272
00273 btScalar btTranslationalLimitMotor::solveLinearAxis(
00274 btScalar timeStep,
00275 btScalar jacDiagABInv,
00276 btRigidBody& body1,const btVector3 &pointInA,
00277 btRigidBody& body2,const btVector3 &pointInB,
00278 int limit_index,
00279 const btVector3 & axis_normal_on_a,
00280 const btVector3 & anchorPos)
00281 {
00282
00284
00285
00286 btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition();
00287 btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition();
00288
00289 btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
00290 btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
00291 btVector3 vel = vel1 - vel2;
00292
00293 btScalar rel_vel = axis_normal_on_a.dot(vel);
00294
00295
00296
00298
00299
00300 btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a);
00301 btScalar lo = btScalar(-BT_LARGE_FLOAT);
00302 btScalar hi = btScalar(BT_LARGE_FLOAT);
00303
00304 btScalar minLimit = m_lowerLimit[limit_index];
00305 btScalar maxLimit = m_upperLimit[limit_index];
00306
00307
00308 if (minLimit < maxLimit)
00309 {
00310 {
00311 if (depth > maxLimit)
00312 {
00313 depth -= maxLimit;
00314 lo = btScalar(0.);
00315
00316 }
00317 else
00318 {
00319 if (depth < minLimit)
00320 {
00321 depth -= minLimit;
00322 hi = btScalar(0.);
00323 }
00324 else
00325 {
00326 return 0.0f;
00327 }
00328 }
00329 }
00330 }
00331
00332 btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv;
00333
00334
00335
00336
00337 btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index];
00338 btScalar sum = oldNormalImpulse + normalImpulse;
00339 m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
00340 normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
00341
00342 btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
00343 body1.applyImpulse( impulse_vector, rel_pos1);
00344 body2.applyImpulse(-impulse_vector, rel_pos2);
00345
00346
00347
00348 return normalImpulse;
00349 }
00350
00352
00353 void btGeneric6DofConstraint::calculateAngleInfo()
00354 {
00355 btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis();
00356 matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff);
00357
00358
00359
00360
00361
00362
00363
00364
00365
00366
00367
00368
00369
00370
00371 btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
00372 btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
00373
00374 m_calculatedAxis[1] = axis2.cross(axis0);
00375 m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
00376 m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
00377
00378 m_calculatedAxis[0].normalize();
00379 m_calculatedAxis[1].normalize();
00380 m_calculatedAxis[2].normalize();
00381
00382 }
00383
00384 void btGeneric6DofConstraint::calculateTransforms()
00385 {
00386 calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
00387 }
00388
00389 void btGeneric6DofConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB)
00390 {
00391 m_calculatedTransformA = transA * m_frameInA;
00392 m_calculatedTransformB = transB * m_frameInB;
00393 calculateLinearInfo();
00394 calculateAngleInfo();
00395 if(m_useOffsetForConstraintFrame)
00396 {
00397 btScalar miA = getRigidBodyA().getInvMass();
00398 btScalar miB = getRigidBodyB().getInvMass();
00399 m_hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
00400 btScalar miS = miA + miB;
00401 if(miS > btScalar(0.f))
00402 {
00403 m_factA = miB / miS;
00404 }
00405 else
00406 {
00407 m_factA = btScalar(0.5f);
00408 }
00409 m_factB = btScalar(1.0f) - m_factA;
00410 }
00411 }
00412
00413
00414
00415 void btGeneric6DofConstraint::buildLinearJacobian(
00416 btJacobianEntry & jacLinear,const btVector3 & normalWorld,
00417 const btVector3 & pivotAInW,const btVector3 & pivotBInW)
00418 {
00419 new (&jacLinear) btJacobianEntry(
00420 m_rbA.getCenterOfMassTransform().getBasis().transpose(),
00421 m_rbB.getCenterOfMassTransform().getBasis().transpose(),
00422 pivotAInW - m_rbA.getCenterOfMassPosition(),
00423 pivotBInW - m_rbB.getCenterOfMassPosition(),
00424 normalWorld,
00425 m_rbA.getInvInertiaDiagLocal(),
00426 m_rbA.getInvMass(),
00427 m_rbB.getInvInertiaDiagLocal(),
00428 m_rbB.getInvMass());
00429 }
00430
00431
00432
00433 void btGeneric6DofConstraint::buildAngularJacobian(
00434 btJacobianEntry & jacAngular,const btVector3 & jointAxisW)
00435 {
00436 new (&jacAngular) btJacobianEntry(jointAxisW,
00437 m_rbA.getCenterOfMassTransform().getBasis().transpose(),
00438 m_rbB.getCenterOfMassTransform().getBasis().transpose(),
00439 m_rbA.getInvInertiaDiagLocal(),
00440 m_rbB.getInvInertiaDiagLocal());
00441
00442 }
00443
00444
00445
00446 bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
00447 {
00448 btScalar angle = m_calculatedAxisAngleDiff[axis_index];
00449 angle = btAdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit);
00450 m_angularLimits[axis_index].m_currentPosition = angle;
00451
00452 m_angularLimits[axis_index].testLimitValue(angle);
00453 return m_angularLimits[axis_index].needApplyTorques();
00454 }
00455
00456
00457
00458 void btGeneric6DofConstraint::buildJacobian()
00459 {
00460 #ifndef __SPU__
00461 if (m_useSolveConstraintObsolete)
00462 {
00463
00464
00465 m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
00466 int i;
00467 for(i = 0; i < 3; i++)
00468 {
00469 m_angularLimits[i].m_accumulatedImpulse = btScalar(0.);
00470 }
00471
00472 calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
00473
00474
00475
00476 calcAnchorPos();
00477 btVector3 pivotAInW = m_AnchorPos;
00478 btVector3 pivotBInW = m_AnchorPos;
00479
00480
00481
00482
00483
00484 btVector3 normalWorld;
00485
00486 for (i=0;i<3;i++)
00487 {
00488 if (m_linearLimits.isLimited(i))
00489 {
00490 if (m_useLinearReferenceFrameA)
00491 normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
00492 else
00493 normalWorld = m_calculatedTransformB.getBasis().getColumn(i);
00494
00495 buildLinearJacobian(
00496 m_jacLinear[i],normalWorld ,
00497 pivotAInW,pivotBInW);
00498
00499 }
00500 }
00501
00502
00503 for (i=0;i<3;i++)
00504 {
00505
00506 if (testAngularLimitMotor(i))
00507 {
00508 normalWorld = this->getAxis(i);
00509
00510 buildAngularJacobian(m_jacAng[i],normalWorld);
00511 }
00512 }
00513
00514 }
00515 #endif //__SPU__
00516
00517 }
00518
00519
00520 void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info)
00521 {
00522 if (m_useSolveConstraintObsolete)
00523 {
00524 info->m_numConstraintRows = 0;
00525 info->nub = 0;
00526 } else
00527 {
00528
00529 calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
00530 info->m_numConstraintRows = 0;
00531 info->nub = 6;
00532 int i;
00533
00534 for(i = 0; i < 3; i++)
00535 {
00536 if(m_linearLimits.needApplyForce(i))
00537 {
00538 info->m_numConstraintRows++;
00539 info->nub--;
00540 }
00541 }
00542
00543 for (i=0;i<3 ;i++ )
00544 {
00545 if(testAngularLimitMotor(i))
00546 {
00547 info->m_numConstraintRows++;
00548 info->nub--;
00549 }
00550 }
00551 }
00552 }
00553
00554 void btGeneric6DofConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
00555 {
00556 if (m_useSolveConstraintObsolete)
00557 {
00558 info->m_numConstraintRows = 0;
00559 info->nub = 0;
00560 } else
00561 {
00562
00563 info->m_numConstraintRows = 6;
00564 info->nub = 0;
00565 }
00566 }
00567
00568
00569 void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info)
00570 {
00571 btAssert(!m_useSolveConstraintObsolete);
00572
00573 const btTransform& transA = m_rbA.getCenterOfMassTransform();
00574 const btTransform& transB = m_rbB.getCenterOfMassTransform();
00575 const btVector3& linVelA = m_rbA.getLinearVelocity();
00576 const btVector3& linVelB = m_rbB.getLinearVelocity();
00577 const btVector3& angVelA = m_rbA.getAngularVelocity();
00578 const btVector3& angVelB = m_rbB.getAngularVelocity();
00579
00580 if(m_useOffsetForConstraintFrame)
00581 {
00582 int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
00583 setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
00584 }
00585 else
00586 {
00587 int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
00588 setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
00589 }
00590
00591 }
00592
00593
00594 void btGeneric6DofConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
00595 {
00596
00597 btAssert(!m_useSolveConstraintObsolete);
00598
00599 calculateTransforms(transA,transB);
00600
00601 int i;
00602 for (i=0;i<3 ;i++ )
00603 {
00604 testAngularLimitMotor(i);
00605 }
00606
00607 if(m_useOffsetForConstraintFrame)
00608 {
00609 int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
00610 setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
00611 }
00612 else
00613 {
00614 int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
00615 setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
00616 }
00617 }
00618
00619
00620
00621 int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
00622 {
00623
00624
00625 btRotationalLimitMotor limot;
00626 for (int i=0;i<3 ;i++ )
00627 {
00628 if(m_linearLimits.needApplyForce(i))
00629 {
00630 limot.m_bounce = btScalar(0.f);
00631 limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
00632 limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i];
00633 limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i];
00634 limot.m_damping = m_linearLimits.m_damping;
00635 limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
00636 limot.m_hiLimit = m_linearLimits.m_upperLimit[i];
00637 limot.m_limitSoftness = m_linearLimits.m_limitSoftness;
00638 limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
00639 limot.m_maxLimitForce = btScalar(0.f);
00640 limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
00641 limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
00642 btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
00643 int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT);
00644 limot.m_normalCFM = (flags & BT_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0];
00645 limot.m_stopCFM = (flags & BT_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
00646 limot.m_stopERP = (flags & BT_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp;
00647 if(m_useOffsetForConstraintFrame)
00648 {
00649 int indx1 = (i + 1) % 3;
00650 int indx2 = (i + 2) % 3;
00651 int rotAllowed = 1;
00652 if(m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit)
00653 {
00654 rotAllowed = 0;
00655 }
00656 row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed);
00657 }
00658 else
00659 {
00660 row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0);
00661 }
00662 }
00663 }
00664 return row;
00665 }
00666
00667
00668
00669 int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
00670 {
00671 btGeneric6DofConstraint * d6constraint = this;
00672 int row = row_offset;
00673
00674 for (int i=0;i<3 ;i++ )
00675 {
00676 if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
00677 {
00678 btVector3 axis = d6constraint->getAxis(i);
00679 int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT);
00680 if(!(flags & BT_6DOF_FLAGS_CFM_NORM))
00681 {
00682 m_angularLimits[i].m_normalCFM = info->cfm[0];
00683 }
00684 if(!(flags & BT_6DOF_FLAGS_CFM_STOP))
00685 {
00686 m_angularLimits[i].m_stopCFM = info->cfm[0];
00687 }
00688 if(!(flags & BT_6DOF_FLAGS_ERP_STOP))
00689 {
00690 m_angularLimits[i].m_stopERP = info->erp;
00691 }
00692 row += get_limit_motor_info2(d6constraint->getRotationalLimitMotor(i),
00693 transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1);
00694 }
00695 }
00696
00697 return row;
00698 }
00699
00700
00701
00702
00703 void btGeneric6DofConstraint::updateRHS(btScalar timeStep)
00704 {
00705 (void)timeStep;
00706
00707 }
00708
00709
00710 void btGeneric6DofConstraint::setFrames(const btTransform& frameA, const btTransform& frameB)
00711 {
00712 m_frameInA = frameA;
00713 m_frameInB = frameB;
00714 buildJacobian();
00715 calculateTransforms();
00716 }
00717
00718
00719
00720 btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
00721 {
00722 return m_calculatedAxis[axis_index];
00723 }
00724
00725
00726 btScalar btGeneric6DofConstraint::getRelativePivotPosition(int axisIndex) const
00727 {
00728 return m_calculatedLinearDiff[axisIndex];
00729 }
00730
00731
00732 btScalar btGeneric6DofConstraint::getAngle(int axisIndex) const
00733 {
00734 return m_calculatedAxisAngleDiff[axisIndex];
00735 }
00736
00737
00738
00739 void btGeneric6DofConstraint::calcAnchorPos(void)
00740 {
00741 btScalar imA = m_rbA.getInvMass();
00742 btScalar imB = m_rbB.getInvMass();
00743 btScalar weight;
00744 if(imB == btScalar(0.0))
00745 {
00746 weight = btScalar(1.0);
00747 }
00748 else
00749 {
00750 weight = imA / (imA + imB);
00751 }
00752 const btVector3& pA = m_calculatedTransformA.getOrigin();
00753 const btVector3& pB = m_calculatedTransformB.getOrigin();
00754 m_AnchorPos = pA * weight + pB * (btScalar(1.0) - weight);
00755 return;
00756 }
00757
00758
00759
00760 void btGeneric6DofConstraint::calculateLinearInfo()
00761 {
00762 m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();
00763 m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;
00764 for(int i = 0; i < 3; i++)
00765 {
00766 m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i];
00767 m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
00768 }
00769 }
00770
00771
00772
00773 int btGeneric6DofConstraint::get_limit_motor_info2(
00774 btRotationalLimitMotor * limot,
00775 const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
00776 btConstraintInfo2 *info, int row, btVector3& ax1, int rotational,int rotAllowed)
00777 {
00778 int srow = row * info->rowskip;
00779 int powered = limot->m_enableMotor;
00780 int limit = limot->m_currentLimit;
00781 if (powered || limit)
00782 {
00783 btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
00784 btScalar *J2 = rotational ? info->m_J2angularAxis : 0;
00785 J1[srow+0] = ax1[0];
00786 J1[srow+1] = ax1[1];
00787 J1[srow+2] = ax1[2];
00788 if(rotational)
00789 {
00790 J2[srow+0] = -ax1[0];
00791 J2[srow+1] = -ax1[1];
00792 J2[srow+2] = -ax1[2];
00793 }
00794 if((!rotational))
00795 {
00796 if (m_useOffsetForConstraintFrame)
00797 {
00798 btVector3 tmpA, tmpB, relA, relB;
00799
00800 relB = m_calculatedTransformB.getOrigin() - transB.getOrigin();
00801
00802 btVector3 projB = ax1 * relB.dot(ax1);
00803
00804 btVector3 orthoB = relB - projB;
00805
00806 relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();
00807 btVector3 projA = ax1 * relA.dot(ax1);
00808 btVector3 orthoA = relA - projA;
00809
00810 btScalar desiredOffs = limot->m_currentPosition - limot->m_currentLimitError;
00811
00812 btVector3 totalDist = projA + ax1 * desiredOffs - projB;
00813
00814 relA = orthoA + totalDist * m_factA;
00815 relB = orthoB - totalDist * m_factB;
00816 tmpA = relA.cross(ax1);
00817 tmpB = relB.cross(ax1);
00818 if(m_hasStaticBody && (!rotAllowed))
00819 {
00820 tmpA *= m_factA;
00821 tmpB *= m_factB;
00822 }
00823 int i;
00824 for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i];
00825 for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i];
00826 } else
00827 {
00828 btVector3 ltd;
00829 btVector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin();
00830 ltd = c.cross(ax1);
00831 info->m_J1angularAxis[srow+0] = ltd[0];
00832 info->m_J1angularAxis[srow+1] = ltd[1];
00833 info->m_J1angularAxis[srow+2] = ltd[2];
00834
00835 c = m_calculatedTransformB.getOrigin() - transB.getOrigin();
00836 ltd = -c.cross(ax1);
00837 info->m_J2angularAxis[srow+0] = ltd[0];
00838 info->m_J2angularAxis[srow+1] = ltd[1];
00839 info->m_J2angularAxis[srow+2] = ltd[2];
00840 }
00841 }
00842
00843
00844 if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0;
00845 info->m_constraintError[srow] = btScalar(0.f);
00846 if (powered)
00847 {
00848 info->cfm[srow] = limot->m_normalCFM;
00849 if(!limit)
00850 {
00851 btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
00852
00853 btScalar mot_fact = getMotorFactor( limot->m_currentPosition,
00854 limot->m_loLimit,
00855 limot->m_hiLimit,
00856 tag_vel,
00857 info->fps * limot->m_stopERP);
00858 info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
00859 info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
00860 info->m_upperLimit[srow] = limot->m_maxMotorForce;
00861 }
00862 }
00863 if(limit)
00864 {
00865 btScalar k = info->fps * limot->m_stopERP;
00866 if(!rotational)
00867 {
00868 info->m_constraintError[srow] += k * limot->m_currentLimitError;
00869 }
00870 else
00871 {
00872 info->m_constraintError[srow] += -k * limot->m_currentLimitError;
00873 }
00874 info->cfm[srow] = limot->m_stopCFM;
00875 if (limot->m_loLimit == limot->m_hiLimit)
00876 {
00877 info->m_lowerLimit[srow] = -SIMD_INFINITY;
00878 info->m_upperLimit[srow] = SIMD_INFINITY;
00879 }
00880 else
00881 {
00882 if (limit == 1)
00883 {
00884 info->m_lowerLimit[srow] = 0;
00885 info->m_upperLimit[srow] = SIMD_INFINITY;
00886 }
00887 else
00888 {
00889 info->m_lowerLimit[srow] = -SIMD_INFINITY;
00890 info->m_upperLimit[srow] = 0;
00891 }
00892
00893 if (limot->m_bounce > 0)
00894 {
00895
00896 btScalar vel;
00897 if (rotational)
00898 {
00899 vel = angVelA.dot(ax1);
00900
00901
00902 vel -= angVelB.dot(ax1);
00903 }
00904 else
00905 {
00906 vel = linVelA.dot(ax1);
00907
00908
00909 vel -= linVelB.dot(ax1);
00910 }
00911
00912
00913 if (limit == 1)
00914 {
00915 if (vel < 0)
00916 {
00917 btScalar newc = -limot->m_bounce* vel;
00918 if (newc > info->m_constraintError[srow])
00919 info->m_constraintError[srow] = newc;
00920 }
00921 }
00922 else
00923 {
00924 if (vel > 0)
00925 {
00926 btScalar newc = -limot->m_bounce * vel;
00927 if (newc < info->m_constraintError[srow])
00928 info->m_constraintError[srow] = newc;
00929 }
00930 }
00931 }
00932 }
00933 }
00934 return 1;
00935 }
00936 else return 0;
00937 }
00938
00939
00940
00941
00942
00943
00946 void btGeneric6DofConstraint::setParam(int num, btScalar value, int axis)
00947 {
00948 if((axis >= 0) && (axis < 3))
00949 {
00950 switch(num)
00951 {
00952 case BT_CONSTRAINT_STOP_ERP :
00953 m_linearLimits.m_stopERP[axis] = value;
00954 m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
00955 break;
00956 case BT_CONSTRAINT_STOP_CFM :
00957 m_linearLimits.m_stopCFM[axis] = value;
00958 m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
00959 break;
00960 case BT_CONSTRAINT_CFM :
00961 m_linearLimits.m_normalCFM[axis] = value;
00962 m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
00963 break;
00964 default :
00965 btAssertConstrParams(0);
00966 }
00967 }
00968 else if((axis >=3) && (axis < 6))
00969 {
00970 switch(num)
00971 {
00972 case BT_CONSTRAINT_STOP_ERP :
00973 m_angularLimits[axis - 3].m_stopERP = value;
00974 m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
00975 break;
00976 case BT_CONSTRAINT_STOP_CFM :
00977 m_angularLimits[axis - 3].m_stopCFM = value;
00978 m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
00979 break;
00980 case BT_CONSTRAINT_CFM :
00981 m_angularLimits[axis - 3].m_normalCFM = value;
00982 m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
00983 break;
00984 default :
00985 btAssertConstrParams(0);
00986 }
00987 }
00988 else
00989 {
00990 btAssertConstrParams(0);
00991 }
00992 }
00993
00995 btScalar btGeneric6DofConstraint::getParam(int num, int axis) const
00996 {
00997 btScalar retVal = 0;
00998 if((axis >= 0) && (axis < 3))
00999 {
01000 switch(num)
01001 {
01002 case BT_CONSTRAINT_STOP_ERP :
01003 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
01004 retVal = m_linearLimits.m_stopERP[axis];
01005 break;
01006 case BT_CONSTRAINT_STOP_CFM :
01007 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
01008 retVal = m_linearLimits.m_stopCFM[axis];
01009 break;
01010 case BT_CONSTRAINT_CFM :
01011 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
01012 retVal = m_linearLimits.m_normalCFM[axis];
01013 break;
01014 default :
01015 btAssertConstrParams(0);
01016 }
01017 }
01018 else if((axis >=3) && (axis < 6))
01019 {
01020 switch(num)
01021 {
01022 case BT_CONSTRAINT_STOP_ERP :
01023 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
01024 retVal = m_angularLimits[axis - 3].m_stopERP;
01025 break;
01026 case BT_CONSTRAINT_STOP_CFM :
01027 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
01028 retVal = m_angularLimits[axis - 3].m_stopCFM;
01029 break;
01030 case BT_CONSTRAINT_CFM :
01031 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
01032 retVal = m_angularLimits[axis - 3].m_normalCFM;
01033 break;
01034 default :
01035 btAssertConstrParams(0);
01036 }
01037 }
01038 else
01039 {
01040 btAssertConstrParams(0);
01041 }
01042 return retVal;
01043 }
01044
01045
01046
01047 void btGeneric6DofConstraint::setAxis(const btVector3& axis1,const btVector3& axis2)
01048 {
01049 btVector3 zAxis = axis1.normalized();
01050 btVector3 yAxis = axis2.normalized();
01051 btVector3 xAxis = yAxis.cross(zAxis);
01052
01053 btTransform frameInW;
01054 frameInW.setIdentity();
01055 frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0],
01056 xAxis[1], yAxis[1], zAxis[1],
01057 xAxis[2], yAxis[2], zAxis[2]);
01058
01059
01060 m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW;
01061 m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;
01062
01063 calculateTransforms();
01064 }