Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #ifndef BT_CONETWISTCONSTRAINT_H
00037 #define BT_CONETWISTCONSTRAINT_H
00038
00039 #include "LinearMath/btVector3.h"
00040 #include "btJacobianEntry.h"
00041 #include "btTypedConstraint.h"
00042
00043 class btRigidBody;
00044
00045 enum btConeTwistFlags
00046 {
00047 BT_CONETWIST_FLAGS_LIN_CFM = 1,
00048 BT_CONETWIST_FLAGS_LIN_ERP = 2,
00049 BT_CONETWIST_FLAGS_ANG_CFM = 4
00050 };
00051
00053 ATTRIBUTE_ALIGNED16(class) btConeTwistConstraint : public btTypedConstraint
00054 {
00055 #ifdef IN_PARALLELL_SOLVER
00056 public:
00057 #endif
00058 btJacobianEntry m_jac[3];
00059
00060 btTransform m_rbAFrame;
00061 btTransform m_rbBFrame;
00062
00063 btScalar m_limitSoftness;
00064 btScalar m_biasFactor;
00065 btScalar m_relaxationFactor;
00066
00067 btScalar m_damping;
00068
00069 btScalar m_swingSpan1;
00070 btScalar m_swingSpan2;
00071 btScalar m_twistSpan;
00072
00073 btScalar m_fixThresh;
00074
00075 btVector3 m_swingAxis;
00076 btVector3 m_twistAxis;
00077
00078 btScalar m_kSwing;
00079 btScalar m_kTwist;
00080
00081 btScalar m_twistLimitSign;
00082 btScalar m_swingCorrection;
00083 btScalar m_twistCorrection;
00084
00085 btScalar m_twistAngle;
00086
00087 btScalar m_accSwingLimitImpulse;
00088 btScalar m_accTwistLimitImpulse;
00089
00090 bool m_angularOnly;
00091 bool m_solveTwistLimit;
00092 bool m_solveSwingLimit;
00093
00094 bool m_useSolveConstraintObsolete;
00095
00096
00097 btScalar m_swingLimitRatio;
00098 btScalar m_twistLimitRatio;
00099 btVector3 m_twistAxisA;
00100
00101
00102 bool m_bMotorEnabled;
00103 bool m_bNormalizedMotorStrength;
00104 btQuaternion m_qTarget;
00105 btScalar m_maxMotorImpulse;
00106 btVector3 m_accMotorImpulse;
00107
00108
00109 int m_flags;
00110 btScalar m_linCFM;
00111 btScalar m_linERP;
00112 btScalar m_angCFM;
00113
00114 protected:
00115
00116 void init();
00117
00118 void computeConeLimitInfo(const btQuaternion& qCone,
00119 btScalar& swingAngle, btVector3& vSwingAxis, btScalar& swingLimit);
00120
00121 void computeTwistLimitInfo(const btQuaternion& qTwist,
00122 btScalar& twistAngle, btVector3& vTwistAxis);
00123
00124 void adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const;
00125
00126
00127 public:
00128
00129 BT_DECLARE_ALIGNED_ALLOCATOR();
00130
00131 btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB,const btTransform& rbAFrame, const btTransform& rbBFrame);
00132
00133 btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame);
00134
00135 virtual void buildJacobian();
00136
00137 virtual void getInfo1 (btConstraintInfo1* info);
00138
00139 void getInfo1NonVirtual(btConstraintInfo1* info);
00140
00141 virtual void getInfo2 (btConstraintInfo2* info);
00142
00143 void getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB);
00144
00145 virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep);
00146
00147
00148 void updateRHS(btScalar timeStep);
00149
00150
00151 const btRigidBody& getRigidBodyA() const
00152 {
00153 return m_rbA;
00154 }
00155 const btRigidBody& getRigidBodyB() const
00156 {
00157 return m_rbB;
00158 }
00159
00160 void setAngularOnly(bool angularOnly)
00161 {
00162 m_angularOnly = angularOnly;
00163 }
00164
00165 void setLimit(int limitIndex,btScalar limitValue)
00166 {
00167 switch (limitIndex)
00168 {
00169 case 3:
00170 {
00171 m_twistSpan = limitValue;
00172 break;
00173 }
00174 case 4:
00175 {
00176 m_swingSpan2 = limitValue;
00177 break;
00178 }
00179 case 5:
00180 {
00181 m_swingSpan1 = limitValue;
00182 break;
00183 }
00184 default:
00185 {
00186 }
00187 };
00188 }
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201 void setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan, btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
00202 {
00203 m_swingSpan1 = _swingSpan1;
00204 m_swingSpan2 = _swingSpan2;
00205 m_twistSpan = _twistSpan;
00206
00207 m_limitSoftness = _softness;
00208 m_biasFactor = _biasFactor;
00209 m_relaxationFactor = _relaxationFactor;
00210 }
00211
00212 const btTransform& getAFrame() { return m_rbAFrame; };
00213 const btTransform& getBFrame() { return m_rbBFrame; };
00214
00215 inline int getSolveTwistLimit()
00216 {
00217 return m_solveTwistLimit;
00218 }
00219
00220 inline int getSolveSwingLimit()
00221 {
00222 return m_solveTwistLimit;
00223 }
00224
00225 inline btScalar getTwistLimitSign()
00226 {
00227 return m_twistLimitSign;
00228 }
00229
00230 void calcAngleInfo();
00231 void calcAngleInfo2(const btTransform& transA, const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB);
00232
00233 inline btScalar getSwingSpan1()
00234 {
00235 return m_swingSpan1;
00236 }
00237 inline btScalar getSwingSpan2()
00238 {
00239 return m_swingSpan2;
00240 }
00241 inline btScalar getTwistSpan()
00242 {
00243 return m_twistSpan;
00244 }
00245 inline btScalar getTwistAngle()
00246 {
00247 return m_twistAngle;
00248 }
00249 bool isPastSwingLimit() { return m_solveSwingLimit; }
00250
00251 void setDamping(btScalar damping) { m_damping = damping; }
00252
00253 void enableMotor(bool b) { m_bMotorEnabled = b; }
00254 void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = false; }
00255 void setMaxMotorImpulseNormalized(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = true; }
00256
00257 btScalar getFixThresh() { return m_fixThresh; }
00258 void setFixThresh(btScalar fixThresh) { m_fixThresh = fixThresh; }
00259
00260
00261
00262
00263
00264 void setMotorTarget(const btQuaternion &q);
00265
00266
00267 void setMotorTargetInConstraintSpace(const btQuaternion &q);
00268
00269 btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const;
00270
00273 virtual void setParam(int num, btScalar value, int axis = -1);
00274
00275 virtual void setFrames(const btTransform& frameA, const btTransform& frameB);
00276
00277 const btTransform& getFrameOffsetA() const
00278 {
00279 return m_rbAFrame;
00280 }
00281
00282 const btTransform& getFrameOffsetB() const
00283 {
00284 return m_rbBFrame;
00285 }
00286
00287
00289 virtual btScalar getParam(int num, int axis = -1) const;
00290
00291 virtual int calculateSerializeBufferSize() const;
00292
00294 virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
00295
00296 };
00297
00299 struct btConeTwistConstraintData
00300 {
00301 btTypedConstraintData m_typeConstraintData;
00302 btTransformFloatData m_rbAFrame;
00303 btTransformFloatData m_rbBFrame;
00304
00305
00306 float m_swingSpan1;
00307 float m_swingSpan2;
00308 float m_twistSpan;
00309 float m_limitSoftness;
00310 float m_biasFactor;
00311 float m_relaxationFactor;
00312
00313 float m_damping;
00314
00315 char m_pad[4];
00316
00317 };
00318
00319
00320
00321 SIMD_FORCE_INLINE int btConeTwistConstraint::calculateSerializeBufferSize() const
00322 {
00323 return sizeof(btConeTwistConstraintData);
00324
00325 }
00326
00327
00329 SIMD_FORCE_INLINE const char* btConeTwistConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
00330 {
00331 btConeTwistConstraintData* cone = (btConeTwistConstraintData*) dataBuffer;
00332 btTypedConstraint::serialize(&cone->m_typeConstraintData,serializer);
00333
00334 m_rbAFrame.serializeFloat(cone->m_rbAFrame);
00335 m_rbBFrame.serializeFloat(cone->m_rbBFrame);
00336
00337 cone->m_swingSpan1 = float(m_swingSpan1);
00338 cone->m_swingSpan2 = float(m_swingSpan2);
00339 cone->m_twistSpan = float(m_twistSpan);
00340 cone->m_limitSoftness = float(m_limitSoftness);
00341 cone->m_biasFactor = float(m_biasFactor);
00342 cone->m_relaxationFactor = float(m_relaxationFactor);
00343 cone->m_damping = float(m_damping);
00344
00345 return "btConeTwistConstraintData";
00346 }
00347
00348
00349 #endif //BT_CONETWISTCONSTRAINT_H