00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 #ifndef BT_HINGECONSTRAINT_H
00019 #define BT_HINGECONSTRAINT_H
00020
00021 #define _BT_USE_CENTER_LIMIT_ 1
00022
00023
00024 #include "LinearMath/btVector3.h"
00025 #include "btJacobianEntry.h"
00026 #include "btTypedConstraint.h"
00027
00028 class btRigidBody;
00029
00030 #ifdef BT_USE_DOUBLE_PRECISION
00031 #define btHingeConstraintData btHingeConstraintDoubleData
00032 #define btHingeConstraintDataName "btHingeConstraintDoubleData"
00033 #else
00034 #define btHingeConstraintData btHingeConstraintFloatData
00035 #define btHingeConstraintDataName "btHingeConstraintFloatData"
00036 #endif //BT_USE_DOUBLE_PRECISION
00037
00038
00039
00040 enum btHingeFlags
00041 {
00042 BT_HINGE_FLAGS_CFM_STOP = 1,
00043 BT_HINGE_FLAGS_ERP_STOP = 2,
00044 BT_HINGE_FLAGS_CFM_NORM = 4
00045 };
00046
00047
00050 ATTRIBUTE_ALIGNED16(class) btHingeConstraint : public btTypedConstraint
00051 {
00052 #ifdef IN_PARALLELL_SOLVER
00053 public:
00054 #endif
00055 btJacobianEntry m_jac[3];
00056 btJacobianEntry m_jacAng[3];
00057
00058 btTransform m_rbAFrame;
00059 btTransform m_rbBFrame;
00060
00061 btScalar m_motorTargetVelocity;
00062 btScalar m_maxMotorImpulse;
00063
00064
00065 #ifdef _BT_USE_CENTER_LIMIT_
00066 btAngularLimit m_limit;
00067 #else
00068 btScalar m_lowerLimit;
00069 btScalar m_upperLimit;
00070 btScalar m_limitSign;
00071 btScalar m_correction;
00072
00073 btScalar m_limitSoftness;
00074 btScalar m_biasFactor;
00075 btScalar m_relaxationFactor;
00076
00077 bool m_solveLimit;
00078 #endif
00079
00080 btScalar m_kHinge;
00081
00082
00083 btScalar m_accLimitImpulse;
00084 btScalar m_hingeAngle;
00085 btScalar m_referenceSign;
00086
00087 bool m_angularOnly;
00088 bool m_enableAngularMotor;
00089 bool m_useSolveConstraintObsolete;
00090 bool m_useOffsetForConstraintFrame;
00091 bool m_useReferenceFrameA;
00092
00093 btScalar m_accMotorImpulse;
00094
00095 int m_flags;
00096 btScalar m_normalCFM;
00097 btScalar m_stopCFM;
00098 btScalar m_stopERP;
00099
00100
00101 public:
00102
00103 BT_DECLARE_ALIGNED_ALLOCATOR();
00104
00105 btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA = false);
00106
00107 btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA = false);
00108
00109 btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA = false);
00110
00111 btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame, bool useReferenceFrameA = false);
00112
00113
00114 virtual void buildJacobian();
00115
00116 virtual void getInfo1 (btConstraintInfo1* info);
00117
00118 void getInfo1NonVirtual(btConstraintInfo1* info);
00119
00120 virtual void getInfo2 (btConstraintInfo2* info);
00121
00122 void getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
00123
00124 void getInfo2Internal(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
00125 void getInfo2InternalUsingFrameOffset(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB);
00126
00127
00128 void updateRHS(btScalar timeStep);
00129
00130 const btRigidBody& getRigidBodyA() const
00131 {
00132 return m_rbA;
00133 }
00134 const btRigidBody& getRigidBodyB() const
00135 {
00136 return m_rbB;
00137 }
00138
00139 btRigidBody& getRigidBodyA()
00140 {
00141 return m_rbA;
00142 }
00143
00144 btRigidBody& getRigidBodyB()
00145 {
00146 return m_rbB;
00147 }
00148
00149 btTransform& getFrameOffsetA()
00150 {
00151 return m_rbAFrame;
00152 }
00153
00154 btTransform& getFrameOffsetB()
00155 {
00156 return m_rbBFrame;
00157 }
00158
00159 void setFrames(const btTransform& frameA, const btTransform& frameB);
00160
00161 void setAngularOnly(bool angularOnly)
00162 {
00163 m_angularOnly = angularOnly;
00164 }
00165
00166 void enableAngularMotor(bool enableMotor,btScalar targetVelocity,btScalar maxMotorImpulse)
00167 {
00168 m_enableAngularMotor = enableMotor;
00169 m_motorTargetVelocity = targetVelocity;
00170 m_maxMotorImpulse = maxMotorImpulse;
00171 }
00172
00173
00174
00175
00176 void enableMotor(bool enableMotor) { m_enableAngularMotor = enableMotor; }
00177 void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; }
00178 void setMotorTarget(const btQuaternion& qAinB, btScalar dt);
00179 void setMotorTarget(btScalar targetAngle, btScalar dt);
00180
00181
00182 void setLimit(btScalar low,btScalar high,btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
00183 {
00184 #ifdef _BT_USE_CENTER_LIMIT_
00185 m_limit.set(low, high, _softness, _biasFactor, _relaxationFactor);
00186 #else
00187 m_lowerLimit = btNormalizeAngle(low);
00188 m_upperLimit = btNormalizeAngle(high);
00189 m_limitSoftness = _softness;
00190 m_biasFactor = _biasFactor;
00191 m_relaxationFactor = _relaxationFactor;
00192 #endif
00193 }
00194
00195 void setAxis(btVector3& axisInA)
00196 {
00197 btVector3 rbAxisA1, rbAxisA2;
00198 btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2);
00199 btVector3 pivotInA = m_rbAFrame.getOrigin();
00200
00201 m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
00202 rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
00203 rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
00204
00205 btVector3 axisInB = m_rbA.getCenterOfMassTransform().getBasis() * axisInA;
00206
00207 btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
00208 btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1);
00209 btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
00210
00211 m_rbBFrame.getOrigin() = m_rbB.getCenterOfMassTransform().inverse()(m_rbA.getCenterOfMassTransform()(pivotInA));
00212
00213 m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
00214 rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
00215 rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
00216 m_rbBFrame.getBasis() = m_rbB.getCenterOfMassTransform().getBasis().inverse() * m_rbBFrame.getBasis();
00217
00218 }
00219
00220 btScalar getLowerLimit() const
00221 {
00222 #ifdef _BT_USE_CENTER_LIMIT_
00223 return m_limit.getLow();
00224 #else
00225 return m_lowerLimit;
00226 #endif
00227 }
00228
00229 btScalar getUpperLimit() const
00230 {
00231 #ifdef _BT_USE_CENTER_LIMIT_
00232 return m_limit.getHigh();
00233 #else
00234 return m_upperLimit;
00235 #endif
00236 }
00237
00238
00239 btScalar getHingeAngle();
00240
00241 btScalar getHingeAngle(const btTransform& transA,const btTransform& transB);
00242
00243 void testLimit(const btTransform& transA,const btTransform& transB);
00244
00245
00246 const btTransform& getAFrame() const { return m_rbAFrame; };
00247 const btTransform& getBFrame() const { return m_rbBFrame; };
00248
00249 btTransform& getAFrame() { return m_rbAFrame; };
00250 btTransform& getBFrame() { return m_rbBFrame; };
00251
00252 inline int getSolveLimit()
00253 {
00254 #ifdef _BT_USE_CENTER_LIMIT_
00255 return m_limit.isLimit();
00256 #else
00257 return m_solveLimit;
00258 #endif
00259 }
00260
00261 inline btScalar getLimitSign()
00262 {
00263 #ifdef _BT_USE_CENTER_LIMIT_
00264 return m_limit.getSign();
00265 #else
00266 return m_limitSign;
00267 #endif
00268 }
00269
00270 inline bool getAngularOnly()
00271 {
00272 return m_angularOnly;
00273 }
00274 inline bool getEnableAngularMotor()
00275 {
00276 return m_enableAngularMotor;
00277 }
00278 inline btScalar getMotorTargetVelosity()
00279 {
00280 return m_motorTargetVelocity;
00281 }
00282 inline btScalar getMaxMotorImpulse()
00283 {
00284 return m_maxMotorImpulse;
00285 }
00286
00287 bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
00288 void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
00289
00290
00293 virtual void setParam(int num, btScalar value, int axis = -1);
00295 virtual btScalar getParam(int num, int axis = -1) const;
00296
00297 virtual int calculateSerializeBufferSize() const;
00298
00300 virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
00301
00302
00303 };
00304
00306 struct btHingeConstraintDoubleData
00307 {
00308 btTypedConstraintData m_typeConstraintData;
00309 btTransformDoubleData m_rbAFrame;
00310 btTransformDoubleData m_rbBFrame;
00311 int m_useReferenceFrameA;
00312 int m_angularOnly;
00313 int m_enableAngularMotor;
00314 float m_motorTargetVelocity;
00315 float m_maxMotorImpulse;
00316
00317 float m_lowerLimit;
00318 float m_upperLimit;
00319 float m_limitSoftness;
00320 float m_biasFactor;
00321 float m_relaxationFactor;
00322
00323 };
00325 struct btHingeConstraintFloatData
00326 {
00327 btTypedConstraintData m_typeConstraintData;
00328 btTransformFloatData m_rbAFrame;
00329 btTransformFloatData m_rbBFrame;
00330 int m_useReferenceFrameA;
00331 int m_angularOnly;
00332
00333 int m_enableAngularMotor;
00334 float m_motorTargetVelocity;
00335 float m_maxMotorImpulse;
00336
00337 float m_lowerLimit;
00338 float m_upperLimit;
00339 float m_limitSoftness;
00340 float m_biasFactor;
00341 float m_relaxationFactor;
00342
00343 };
00344
00345
00346
00347 SIMD_FORCE_INLINE int btHingeConstraint::calculateSerializeBufferSize() const
00348 {
00349 return sizeof(btHingeConstraintData);
00350 }
00351
00353 SIMD_FORCE_INLINE const char* btHingeConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
00354 {
00355 btHingeConstraintData* hingeData = (btHingeConstraintData*)dataBuffer;
00356 btTypedConstraint::serialize(&hingeData->m_typeConstraintData,serializer);
00357
00358 m_rbAFrame.serialize(hingeData->m_rbAFrame);
00359 m_rbBFrame.serialize(hingeData->m_rbBFrame);
00360
00361 hingeData->m_angularOnly = m_angularOnly;
00362 hingeData->m_enableAngularMotor = m_enableAngularMotor;
00363 hingeData->m_maxMotorImpulse = float(m_maxMotorImpulse);
00364 hingeData->m_motorTargetVelocity = float(m_motorTargetVelocity);
00365 hingeData->m_useReferenceFrameA = m_useReferenceFrameA;
00366 #ifdef _BT_USE_CENTER_LIMIT_
00367 hingeData->m_lowerLimit = float(m_limit.getLow());
00368 hingeData->m_upperLimit = float(m_limit.getHigh());
00369 hingeData->m_limitSoftness = float(m_limit.getSoftness());
00370 hingeData->m_biasFactor = float(m_limit.getBiasFactor());
00371 hingeData->m_relaxationFactor = float(m_limit.getRelaxationFactor());
00372 #else
00373 hingeData->m_lowerLimit = float(m_lowerLimit);
00374 hingeData->m_upperLimit = float(m_upperLimit);
00375 hingeData->m_limitSoftness = float(m_limitSoftness);
00376 hingeData->m_biasFactor = float(m_biasFactor);
00377 hingeData->m_relaxationFactor = float(m_relaxationFactor);
00378 #endif
00379
00380 return btHingeConstraintDataName;
00381 }
00382
00383 #endif //BT_HINGECONSTRAINT_H