Blender V2.61 - r43446

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 GENERIC_6DOF_CONSTRAINT_H
00028 #define 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 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 
00350     bool        m_useSolveConstraintObsolete;
00351 
00352     btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
00353     btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB);
00354     
00356 
00360     void calculateTransforms(const btTransform& transA,const btTransform& transB);
00361 
00362     void calculateTransforms();
00363 
00365 
00368     const btTransform & getCalculatedTransformA() const
00369     {
00370         return m_calculatedTransformA;
00371     }
00372 
00374 
00377     const btTransform & getCalculatedTransformB() const
00378     {
00379         return m_calculatedTransformB;
00380     }
00381 
00382     const btTransform & getFrameOffsetA() const
00383     {
00384         return m_frameInA;
00385     }
00386 
00387     const btTransform & getFrameOffsetB() const
00388     {
00389         return m_frameInB;
00390     }
00391 
00392 
00393     btTransform & getFrameOffsetA()
00394     {
00395         return m_frameInA;
00396     }
00397 
00398     btTransform & getFrameOffsetB()
00399     {
00400         return m_frameInB;
00401     }
00402 
00403 
00405     virtual void    buildJacobian();
00406 
00407     virtual void getInfo1 (btConstraintInfo1* info);
00408 
00409     void getInfo1NonVirtual (btConstraintInfo1* info);
00410 
00411     virtual void getInfo2 (btConstraintInfo2* info);
00412 
00413     void getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB);
00414 
00415 
00416     void    updateRHS(btScalar  timeStep);
00417 
00419 
00422     btVector3 getAxis(int axis_index) const;
00423 
00425 
00428     btScalar getAngle(int axis_index) const;
00429 
00431 
00434     btScalar getRelativePivotPosition(int axis_index) const;
00435 
00436     void setFrames(const btTransform & frameA, const btTransform & frameB);
00437 
00439 
00443     bool testAngularLimitMotor(int axis_index);
00444 
00445     void    setLinearLowerLimit(const btVector3& linearLower)
00446     {
00447         m_linearLimits.m_lowerLimit = linearLower;
00448     }
00449 
00450     void    getLinearLowerLimit(btVector3& linearLower)
00451     {
00452         linearLower = m_linearLimits.m_lowerLimit;
00453     }
00454 
00455     void    setLinearUpperLimit(const btVector3& linearUpper)
00456     {
00457         m_linearLimits.m_upperLimit = linearUpper;
00458     }
00459 
00460     void    getLinearUpperLimit(btVector3& linearUpper)
00461     {
00462         linearUpper = m_linearLimits.m_upperLimit;
00463     }
00464 
00465     void    setAngularLowerLimit(const btVector3& angularLower)
00466     {
00467         for(int i = 0; i < 3; i++) 
00468             m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]);
00469     }
00470 
00471     void    getAngularLowerLimit(btVector3& angularLower)
00472     {
00473         for(int i = 0; i < 3; i++) 
00474             angularLower[i] = m_angularLimits[i].m_loLimit;
00475     }
00476 
00477     void    setAngularUpperLimit(const btVector3& angularUpper)
00478     {
00479         for(int i = 0; i < 3; i++)
00480             m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]);
00481     }
00482 
00483     void    getAngularUpperLimit(btVector3& angularUpper)
00484     {
00485         for(int i = 0; i < 3; i++)
00486             angularUpper[i] = m_angularLimits[i].m_hiLimit;
00487     }
00488 
00490     btRotationalLimitMotor * getRotationalLimitMotor(int index)
00491     {
00492         return &m_angularLimits[index];
00493     }
00494 
00496     btTranslationalLimitMotor * getTranslationalLimitMotor()
00497     {
00498         return &m_linearLimits;
00499     }
00500 
00501     //first 3 are linear, next 3 are angular
00502     void setLimit(int axis, btScalar lo, btScalar hi)
00503     {
00504         if(axis<3)
00505         {
00506             m_linearLimits.m_lowerLimit[axis] = lo;
00507             m_linearLimits.m_upperLimit[axis] = hi;
00508         }
00509         else
00510         {
00511             lo = btNormalizeAngle(lo);
00512             hi = btNormalizeAngle(hi);
00513             m_angularLimits[axis-3].m_loLimit = lo;
00514             m_angularLimits[axis-3].m_hiLimit = hi;
00515         }
00516     }
00517 
00519 
00525     bool    isLimited(int limitIndex)
00526     {
00527         if(limitIndex<3)
00528         {
00529             return m_linearLimits.isLimited(limitIndex);
00530 
00531         }
00532         return m_angularLimits[limitIndex-3].isLimited();
00533     }
00534 
00535     virtual void calcAnchorPos(void); // overridable
00536 
00537     int get_limit_motor_info2(  btRotationalLimitMotor * limot,
00538                                 const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
00539                                 btConstraintInfo2 *info, int row, btVector3& ax1, int rotational, int rotAllowed = false);
00540 
00541     // access for UseFrameOffset
00542     bool getUseFrameOffset() { return m_useOffsetForConstraintFrame; }
00543     void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; }
00544 
00547     virtual void setParam(int num, btScalar value, int axis = -1);
00549     virtual btScalar getParam(int num, int axis = -1) const;
00550 
00551     void setAxis( const btVector3& axis1, const btVector3& axis2);
00552 
00553 
00554     virtual int calculateSerializeBufferSize() const;
00555 
00557     virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const;
00558 
00559     
00560 };
00561 
00563 struct btGeneric6DofConstraintData
00564 {
00565     btTypedConstraintData   m_typeConstraintData;
00566     btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis.
00567     btTransformFloatData m_rbBFrame;
00568     
00569     btVector3FloatData  m_linearUpperLimit;
00570     btVector3FloatData  m_linearLowerLimit;
00571 
00572     btVector3FloatData  m_angularUpperLimit;
00573     btVector3FloatData  m_angularLowerLimit;
00574     
00575     int m_useLinearReferenceFrameA;
00576     int m_useOffsetForConstraintFrame;
00577 };
00578 
00579 SIMD_FORCE_INLINE   int btGeneric6DofConstraint::calculateSerializeBufferSize() const
00580 {
00581     return sizeof(btGeneric6DofConstraintData);
00582 }
00583 
00585 SIMD_FORCE_INLINE   const char* btGeneric6DofConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
00586 {
00587 
00588     btGeneric6DofConstraintData* dof = (btGeneric6DofConstraintData*)dataBuffer;
00589     btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer);
00590 
00591     m_frameInA.serializeFloat(dof->m_rbAFrame);
00592     m_frameInB.serializeFloat(dof->m_rbBFrame);
00593 
00594         
00595     int i;
00596     for (i=0;i<3;i++)
00597     {
00598         dof->m_angularLowerLimit.m_floats[i] =  float(m_angularLimits[i].m_loLimit);
00599         dof->m_angularUpperLimit.m_floats[i] =  float(m_angularLimits[i].m_hiLimit);
00600         dof->m_linearLowerLimit.m_floats[i] = float(m_linearLimits.m_lowerLimit[i]);
00601         dof->m_linearUpperLimit.m_floats[i] = float(m_linearLimits.m_upperLimit[i]);
00602     }
00603     
00604     dof->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA? 1 : 0;
00605     dof->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame ? 1 : 0;
00606 
00607     return "btGeneric6DofConstraintData";
00608 }
00609 
00610 
00611 
00612 
00613 
00614 #endif //GENERIC_6DOF_CONSTRAINT_H