btGeneric6DofConstraint.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 
00018 
00019 /*
00020 2007-09-09
00021 btGeneric6DofConstraint Refactored by Francisco Le?n
00022 email: projectileman@yahoo.com
00023 http://gimpact.sf.net
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; // point betwen pivots of bodies A and B to solve linear axes
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         // tests linear limits
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     //first 3 are linear, next 3 are angular
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); // overridable
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         // access for UseFrameOffset
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; // constraint axii. Assumes z is hinge axis.
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