btConeTwistConstraint.h

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 btConeTwistConstraint is Copyright (c) 2007 Starbreeze Studios
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 
00015 Written by: Marcus Hennix
00016 */
00017 
00018 
00019 
00020 /*
00021 Overview:
00022 
00023 btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc).
00024 It is a fixed translation, 3 degree-of-freedom (DOF) rotational "joint".
00025 It divides the 3 rotational DOFs into swing (movement within a cone) and twist.
00026 Swing is divided into swing1 and swing2 which can have different limits, giving an elliptical shape.
00027 (Note: the cone's base isn't flat, so this ellipse is "embedded" on the surface of a sphere.)
00028 
00029 In the contraint's frame of reference:
00030 twist is along the x-axis,
00031 and swing 1 and 2 are along the z and y axes respectively.
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]; //3 orthogonal linear constraints
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         // not yet used...
00097         btScalar        m_swingLimitRatio;
00098         btScalar        m_twistLimitRatio;
00099         btVector3   m_twistAxisA;
00100 
00101         // motor
00102         bool             m_bMotorEnabled;
00103         bool             m_bNormalizedMotorStrength;
00104         btQuaternion m_qTarget;
00105         btScalar         m_maxMotorImpulse;
00106         btVector3        m_accMotorImpulse;
00107         
00108         // parameters
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, // in
00119                 btScalar& swingAngle, btVector3& vSwingAxis, btScalar& swingLimit); // all outs
00120 
00121         void computeTwistLimitInfo(const btQuaternion& qTwist, // in
00122                 btScalar& twistAngle, btVector3& vTwistAxis); // all outs
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         // setLimit(), a few notes:
00191         // _softness:
00192         //              0->1, recommend ~0.8->1.
00193         //              describes % of limits where movement is free.
00194         //              beyond this softness %, the limit is gradually enforced until the "hard" (1.0) limit is reached.
00195         // _biasFactor:
00196         //              0->1?, recommend 0.3 +/-0.3 or so.
00197         //              strength with which constraint resists zeroth order (angular, not angular velocity) limit violation.
00198         // __relaxationFactor:
00199         //              0->1, recommend to stay near 1.
00200         //              the lower the value, the less the constraint will fight velocities which violate the angular limits.
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         // setMotorTarget:
00261         // q: the desired rotation of bodyA wrt bodyB.
00262         // note: if q violates the joint limits, the internal target is clamped to avoid conflicting impulses (very bad for stability)
00263         // note: don't forget to enableMotor()
00264         void setMotorTarget(const btQuaternion &q);
00265 
00266         // same as above, but q is the desired rotation of frameA wrt frameB in constraint space
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         //limits
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