btHingeConstraint.h

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
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 
00016 /* Hinge Constraint by Dirk Gregorius. Limits added by Marcus Hennix at Starbreeze Studios */
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]; //3 orthogonal linear constraints
00056         btJacobianEntry m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor
00057 
00058         btTransform m_rbAFrame; // constraint axii. Assumes z is hinge axis.
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         // extra motor API, including ability to set a target rotation (as opposed to angular velocity)
00174         // note: setMotorTarget sets angular velocity under the hood, so you must call it every tick to
00175         //       maintain a given angular target.
00176         void enableMotor(bool enableMotor)      { m_enableAngularMotor = enableMotor; }
00177         void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; }
00178         void setMotorTarget(const btQuaternion& qAinB, btScalar dt); // qAinB is rotation of body A wrt body B.
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 //              m_rbAFrame.getOrigin() = pivotInA;
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         // access for UseFrameOffset
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; // constraint axii. Assumes z is hinge axis.
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; // constraint axii. Assumes z is hinge axis.
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