00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027 #ifndef BT_GENERIC_6DOF_CONSTRAINT_H
00028 #define BT_GENERIC_6DOF_CONSTRAINT_H
00029
00030 #include "LinearMath/btVector3.h"
00031 #include "btJacobianEntry.h"
00032 #include "btTypedConstraint.h"
00033
00034 class btRigidBody;
00035
00036
00037
00038
00040 class btRotationalLimitMotor
00041 {
00042 public:
00045 btScalar m_loLimit;
00046 btScalar m_hiLimit;
00047 btScalar m_targetVelocity;
00048 btScalar m_maxMotorForce;
00049 btScalar m_maxLimitForce;
00050 btScalar m_damping;
00051 btScalar m_limitSoftness;
00052 btScalar m_normalCFM;
00053 btScalar m_stopERP;
00054 btScalar m_stopCFM;
00055 btScalar m_bounce;
00056 bool m_enableMotor;
00057
00059
00062 btScalar m_currentLimitError;
00063 btScalar m_currentPosition;
00064 int m_currentLimit;
00065 btScalar m_accumulatedImpulse;
00067
00068 btRotationalLimitMotor()
00069 {
00070 m_accumulatedImpulse = 0.f;
00071 m_targetVelocity = 0;
00072 m_maxMotorForce = 0.1f;
00073 m_maxLimitForce = 300.0f;
00074 m_loLimit = 1.0f;
00075 m_hiLimit = -1.0f;
00076 m_normalCFM = 0.f;
00077 m_stopERP = 0.2f;
00078 m_stopCFM = 0.f;
00079 m_bounce = 0.0f;
00080 m_damping = 1.0f;
00081 m_limitSoftness = 0.5f;
00082 m_currentLimit = 0;
00083 m_currentLimitError = 0;
00084 m_enableMotor = false;
00085 }
00086
00087 btRotationalLimitMotor(const btRotationalLimitMotor & limot)
00088 {
00089 m_targetVelocity = limot.m_targetVelocity;
00090 m_maxMotorForce = limot.m_maxMotorForce;
00091 m_limitSoftness = limot.m_limitSoftness;
00092 m_loLimit = limot.m_loLimit;
00093 m_hiLimit = limot.m_hiLimit;
00094 m_normalCFM = limot.m_normalCFM;
00095 m_stopERP = limot.m_stopERP;
00096 m_stopCFM = limot.m_stopCFM;
00097 m_bounce = limot.m_bounce;
00098 m_currentLimit = limot.m_currentLimit;
00099 m_currentLimitError = limot.m_currentLimitError;
00100 m_enableMotor = limot.m_enableMotor;
00101 }
00102
00103
00104
00106 bool isLimited()
00107 {
00108 if(m_loLimit > m_hiLimit) return false;
00109 return true;
00110 }
00111
00113 bool needApplyTorques()
00114 {
00115 if(m_currentLimit == 0 && m_enableMotor == false) return false;
00116 return true;
00117 }
00118
00120
00123 int testLimitValue(btScalar test_value);
00124
00126 btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1);
00127
00128 };
00129
00130
00131
00132 class btTranslationalLimitMotor
00133 {
00134 public:
00135 btVector3 m_lowerLimit;
00136 btVector3 m_upperLimit;
00137 btVector3 m_accumulatedImpulse;
00140 btScalar m_limitSoftness;
00141 btScalar m_damping;
00142 btScalar m_restitution;
00143 btVector3 m_normalCFM;
00144 btVector3 m_stopERP;
00145 btVector3 m_stopCFM;
00146
00147 bool m_enableMotor[3];
00148 btVector3 m_targetVelocity;
00149 btVector3 m_maxMotorForce;
00150 btVector3 m_currentLimitError;
00151 btVector3 m_currentLinearDiff;
00152 int m_currentLimit[3];
00153
00154 btTranslationalLimitMotor()
00155 {
00156 m_lowerLimit.setValue(0.f,0.f,0.f);
00157 m_upperLimit.setValue(0.f,0.f,0.f);
00158 m_accumulatedImpulse.setValue(0.f,0.f,0.f);
00159 m_normalCFM.setValue(0.f, 0.f, 0.f);
00160 m_stopERP.setValue(0.2f, 0.2f, 0.2f);
00161 m_stopCFM.setValue(0.f, 0.f, 0.f);
00162
00163 m_limitSoftness = 0.7f;
00164 m_damping = btScalar(1.0f);
00165 m_restitution = btScalar(0.5f);
00166 for(int i=0; i < 3; i++)
00167 {
00168 m_enableMotor[i] = false;
00169 m_targetVelocity[i] = btScalar(0.f);
00170 m_maxMotorForce[i] = btScalar(0.f);
00171 }
00172 }
00173
00174 btTranslationalLimitMotor(const btTranslationalLimitMotor & other )
00175 {
00176 m_lowerLimit = other.m_lowerLimit;
00177 m_upperLimit = other.m_upperLimit;
00178 m_accumulatedImpulse = other.m_accumulatedImpulse;
00179
00180 m_limitSoftness = other.m_limitSoftness ;
00181 m_damping = other.m_damping;
00182 m_restitution = other.m_restitution;
00183 m_normalCFM = other.m_normalCFM;
00184 m_stopERP = other.m_stopERP;
00185 m_stopCFM = other.m_stopCFM;
00186
00187 for(int i=0; i < 3; i++)
00188 {
00189 m_enableMotor[i] = other.m_enableMotor[i];
00190 m_targetVelocity[i] = other.m_targetVelocity[i];
00191 m_maxMotorForce[i] = other.m_maxMotorForce[i];
00192 }
00193 }
00194
00196
00202 inline bool isLimited(int limitIndex)
00203 {
00204 return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
00205 }
00206 inline bool needApplyForce(int limitIndex)
00207 {
00208 if(m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false;
00209 return true;
00210 }
00211 int testLimitValue(int limitIndex, btScalar test_value);
00212
00213
00214 btScalar solveLinearAxis(
00215 btScalar timeStep,
00216 btScalar jacDiagABInv,
00217 btRigidBody& body1,const btVector3 &pointInA,
00218 btRigidBody& body2,const btVector3 &pointInB,
00219 int limit_index,
00220 const btVector3 & axis_normal_on_a,
00221 const btVector3 & anchorPos);
00222
00223
00224 };
00225
00226 enum bt6DofFlags
00227 {
00228 BT_6DOF_FLAGS_CFM_NORM = 1,
00229 BT_6DOF_FLAGS_CFM_STOP = 2,
00230 BT_6DOF_FLAGS_ERP_STOP = 4
00231 };
00232 #define BT_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis
00233
00234
00236
00271 ATTRIBUTE_ALIGNED16(class) btGeneric6DofConstraint : public btTypedConstraint
00272 {
00273 protected:
00274
00277 btTransform m_frameInA;
00278 btTransform m_frameInB;
00279
00280
00283 btJacobianEntry m_jacLinear[3];
00284 btJacobianEntry m_jacAng[3];
00285
00286
00289 btTranslationalLimitMotor m_linearLimits;
00291
00292
00295 btRotationalLimitMotor m_angularLimits[3];
00297
00298
00299 protected:
00302 btScalar m_timeStep;
00303 btTransform m_calculatedTransformA;
00304 btTransform m_calculatedTransformB;
00305 btVector3 m_calculatedAxisAngleDiff;
00306 btVector3 m_calculatedAxis[3];
00307 btVector3 m_calculatedLinearDiff;
00308 btScalar m_factA;
00309 btScalar m_factB;
00310 bool m_hasStaticBody;
00311
00312 btVector3 m_AnchorPos;
00313
00314 bool m_useLinearReferenceFrameA;
00315 bool m_useOffsetForConstraintFrame;
00316
00317 int m_flags;
00318
00320
00321 btGeneric6DofConstraint& operator=(btGeneric6DofConstraint& other)
00322 {
00323 btAssert(0);
00324 (void) other;
00325 return *this;
00326 }
00327
00328
00329 int setAngularLimits(btConstraintInfo2 *info, int row_offset,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
00330
00331 int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
00332
00333 void buildLinearJacobian(
00334 btJacobianEntry & jacLinear,const btVector3 & normalWorld,
00335 const btVector3 & pivotAInW,const btVector3 & pivotBInW);
00336
00337 void buildAngularJacobian(btJacobianEntry & jacAngular,const btVector3 & jointAxisW);
00338
00339
00340 void calculateLinearInfo();
00341
00343 void calculateAngleInfo();
00344
00345
00346
00347 public:
00348
00349 BT_DECLARE_ALIGNED_ALLOCATOR();
00350
00352 bool m_useSolveConstraintObsolete;
00353
00354 btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
00355 btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB);
00356
00358
00362 void calculateTransforms(const btTransform& transA,const btTransform& transB);
00363
00364 void calculateTransforms();
00365
00367
00370 const btTransform & getCalculatedTransformA() const
00371 {
00372 return m_calculatedTransformA;
00373 }
00374
00376
00379 const btTransform & getCalculatedTransformB() const
00380 {
00381 return m_calculatedTransformB;
00382 }
00383
00384 const btTransform & getFrameOffsetA() const
00385 {
00386 return m_frameInA;
00387 }
00388
00389 const btTransform & getFrameOffsetB() const
00390 {
00391 return m_frameInB;
00392 }
00393
00394
00395 btTransform & getFrameOffsetA()
00396 {
00397 return m_frameInA;
00398 }
00399
00400 btTransform & getFrameOffsetB()
00401 {
00402 return m_frameInB;
00403 }
00404
00405
00407 virtual void buildJacobian();
00408
00409 virtual void getInfo1 (btConstraintInfo1* info);
00410
00411 void getInfo1NonVirtual (btConstraintInfo1* info);
00412
00413 virtual void getInfo2 (btConstraintInfo2* info);
00414
00415 void getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
00416
00417
00418 void updateRHS(btScalar timeStep);
00419
00421
00424 btVector3 getAxis(int axis_index) const;
00425
00427
00430 btScalar getAngle(int axis_index) const;
00431
00433
00436 btScalar getRelativePivotPosition(int axis_index) const;
00437
00438 void setFrames(const btTransform & frameA, const btTransform & frameB);
00439
00441
00445 bool testAngularLimitMotor(int axis_index);
00446
00447 void setLinearLowerLimit(const btVector3& linearLower)
00448 {
00449 m_linearLimits.m_lowerLimit = linearLower;
00450 }
00451
00452 void getLinearLowerLimit(btVector3& linearLower)
00453 {
00454 linearLower = m_linearLimits.m_lowerLimit;
00455 }
00456
00457 void setLinearUpperLimit(const btVector3& linearUpper)
00458 {
00459 m_linearLimits.m_upperLimit = linearUpper;
00460 }
00461
00462 void getLinearUpperLimit(btVector3& linearUpper)
00463 {
00464 linearUpper = m_linearLimits.m_upperLimit;
00465 }
00466
00467 void setAngularLowerLimit(const btVector3& angularLower)
00468 {
00469 for(int i = 0; i < 3; i++)
00470 m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]);
00471 }
00472
00473 void getAngularLowerLimit(btVector3& angularLower)
00474 {
00475 for(int i = 0; i < 3; i++)
00476 angularLower[i] = m_angularLimits[i].m_loLimit;
00477 }
00478
00479 void setAngularUpperLimit(const btVector3& angularUpper)
00480 {
00481 for(int i = 0; i < 3; i++)
00482 m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]);
00483 }
00484
00485 void getAngularUpperLimit(btVector3& angularUpper)
00486 {
00487 for(int i = 0; i < 3; i++)
00488 angularUpper[i] = m_angularLimits[i].m_hiLimit;
00489 }
00490
00492 btRotationalLimitMotor * getRotationalLimitMotor(int index)
00493 {
00494 return &m_angularLimits[index];
00495 }
00496
00498 btTranslationalLimitMotor * getTranslationalLimitMotor()
00499 {
00500 return &m_linearLimits;
00501 }
00502
00503
00504 void setLimit(int axis, btScalar lo, btScalar hi)
00505 {
00506 if(axis<3)
00507 {
00508 m_linearLimits.m_lowerLimit[axis] = lo;
00509 m_linearLimits.m_upperLimit[axis] = hi;
00510 }
00511 else
00512 {
00513 lo = btNormalizeAngle(lo);
00514 hi = btNormalizeAngle(hi);
00515 m_angularLimits[axis-3].m_loLimit = lo;
00516 m_angularLimits[axis-3].m_hiLimit = hi;
00517 }
00518 }
00519
00521
00527 bool isLimited(int limitIndex)
00528 {
00529 if(limitIndex<3)
00530 {
00531 return m_linearLimits.isLimited(limitIndex);
00532
00533 }
00534 return m_angularLimits[limitIndex-3].isLimited();
00535 }
00536
00537 virtual void calcAnchorPos(void);
00538
00539 int get_limit_motor_info2( btRotationalLimitMotor * limot,
00540 const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
00541 btConstraintInfo2 *info, int row, btVector3& ax1, int rotational, int rotAllowed = false);
00542
00543
00544 bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
00545 void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
00546
00549 virtual void setParam(int num, btScalar value, int axis = -1);
00551 virtual btScalar getParam(int num, int axis = -1) const;
00552
00553 void setAxis( const btVector3& axis1, const btVector3& axis2);
00554
00555
00556 virtual int calculateSerializeBufferSize() const;
00557
00559 virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
00560
00561
00562 };
00563
00565 struct btGeneric6DofConstraintData
00566 {
00567 btTypedConstraintData m_typeConstraintData;
00568 btTransformFloatData m_rbAFrame;
00569 btTransformFloatData m_rbBFrame;
00570
00571 btVector3FloatData m_linearUpperLimit;
00572 btVector3FloatData m_linearLowerLimit;
00573
00574 btVector3FloatData m_angularUpperLimit;
00575 btVector3FloatData m_angularLowerLimit;
00576
00577 int m_useLinearReferenceFrameA;
00578 int m_useOffsetForConstraintFrame;
00579 };
00580
00581 SIMD_FORCE_INLINE int btGeneric6DofConstraint::calculateSerializeBufferSize() const
00582 {
00583 return sizeof(btGeneric6DofConstraintData);
00584 }
00585
00587 SIMD_FORCE_INLINE const char* btGeneric6DofConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
00588 {
00589
00590 btGeneric6DofConstraintData* dof = (btGeneric6DofConstraintData*)dataBuffer;
00591 btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer);
00592
00593 m_frameInA.serializeFloat(dof->m_rbAFrame);
00594 m_frameInB.serializeFloat(dof->m_rbBFrame);
00595
00596
00597 int i;
00598 for (i=0;i<3;i++)
00599 {
00600 dof->m_angularLowerLimit.m_floats[i] = float(m_angularLimits[i].m_loLimit);
00601 dof->m_angularUpperLimit.m_floats[i] = float(m_angularLimits[i].m_hiLimit);
00602 dof->m_linearLowerLimit.m_floats[i] = float(m_linearLimits.m_lowerLimit[i]);
00603 dof->m_linearUpperLimit.m_floats[i] = float(m_linearLimits.m_upperLimit[i]);
00604 }
00605
00606 dof->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA? 1 : 0;
00607 dof->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame ? 1 : 0;
00608
00609 return "btGeneric6DofConstraintData";
00610 }
00611
00612
00613
00614
00615
00616 #endif //BT_GENERIC_6DOF_CONSTRAINT_H