diff options
Diffstat (limited to 'thirdparty/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h')
| -rw-r--r-- | thirdparty/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h | 647 |
1 files changed, 0 insertions, 647 deletions
diff --git a/thirdparty/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/thirdparty/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h deleted file mode 100644 index bea8629c32..0000000000 --- a/thirdparty/bullet/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h +++ /dev/null @@ -1,647 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -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. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -/// 2009 March: btGeneric6DofConstraint refactored by Roman Ponomarev -/// Added support for generic constraint solver through getInfo1/getInfo2 methods - -/* -2007-09-09 -btGeneric6DofConstraint Refactored by Francisco Le?n -email: projectileman@yahoo.com -http://gimpact.sf.net -*/ - - -#ifndef BT_GENERIC_6DOF_CONSTRAINT_H -#define BT_GENERIC_6DOF_CONSTRAINT_H - -#include "LinearMath/btVector3.h" -#include "btJacobianEntry.h" -#include "btTypedConstraint.h" - -class btRigidBody; - - - -#ifdef BT_USE_DOUBLE_PRECISION -#define btGeneric6DofConstraintData2 btGeneric6DofConstraintDoubleData2 -#define btGeneric6DofConstraintDataName "btGeneric6DofConstraintDoubleData2" -#else -#define btGeneric6DofConstraintData2 btGeneric6DofConstraintData -#define btGeneric6DofConstraintDataName "btGeneric6DofConstraintData" -#endif //BT_USE_DOUBLE_PRECISION - - -//! Rotation Limit structure for generic joints -class btRotationalLimitMotor -{ -public: - //! limit_parameters - //!@{ - btScalar m_loLimit;//!< joint limit - btScalar m_hiLimit;//!< joint limit - btScalar m_targetVelocity;//!< target motor velocity - btScalar m_maxMotorForce;//!< max force on motor - btScalar m_maxLimitForce;//!< max force on limit - btScalar m_damping;//!< Damping. - btScalar m_limitSoftness;//! Relaxation factor - btScalar m_normalCFM;//!< Constraint force mixing factor - btScalar m_stopERP;//!< Error tolerance factor when joint is at limit - btScalar m_stopCFM;//!< Constraint force mixing factor when joint is at limit - btScalar m_bounce;//!< restitution factor - bool m_enableMotor; - - //!@} - - //! temp_variables - //!@{ - btScalar m_currentLimitError;//! How much is violated this limit - btScalar m_currentPosition; //! current value of angle - int m_currentLimit;//!< 0=free, 1=at lo limit, 2=at hi limit - btScalar m_accumulatedImpulse; - //!@} - - btRotationalLimitMotor() - { - m_accumulatedImpulse = 0.f; - m_targetVelocity = 0; - m_maxMotorForce = 0.1f; - m_maxLimitForce = 300.0f; - m_loLimit = 1.0f; - m_hiLimit = -1.0f; - m_normalCFM = 0.f; - m_stopERP = 0.2f; - m_stopCFM = 0.f; - m_bounce = 0.0f; - m_damping = 1.0f; - m_limitSoftness = 0.5f; - m_currentLimit = 0; - m_currentLimitError = 0; - m_enableMotor = false; - } - - btRotationalLimitMotor(const btRotationalLimitMotor & limot) - { - m_targetVelocity = limot.m_targetVelocity; - m_maxMotorForce = limot.m_maxMotorForce; - m_limitSoftness = limot.m_limitSoftness; - m_loLimit = limot.m_loLimit; - m_hiLimit = limot.m_hiLimit; - m_normalCFM = limot.m_normalCFM; - m_stopERP = limot.m_stopERP; - m_stopCFM = limot.m_stopCFM; - m_bounce = limot.m_bounce; - m_currentLimit = limot.m_currentLimit; - m_currentLimitError = limot.m_currentLimitError; - m_enableMotor = limot.m_enableMotor; - } - - - - //! Is limited - bool isLimited() const - { - if(m_loLimit > m_hiLimit) return false; - return true; - } - - //! Need apply correction - bool needApplyTorques() const - { - if(m_currentLimit == 0 && m_enableMotor == false) return false; - return true; - } - - //! calculates error - /*! - calculates m_currentLimit and m_currentLimitError. - */ - int testLimitValue(btScalar test_value); - - //! apply the correction impulses for two bodies - btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1); - -}; - - - -class btTranslationalLimitMotor -{ -public: - btVector3 m_lowerLimit;//!< the constraint lower limits - btVector3 m_upperLimit;//!< the constraint upper limits - btVector3 m_accumulatedImpulse; - //! Linear_Limit_parameters - //!@{ - btScalar m_limitSoftness;//!< Softness for linear limit - btScalar m_damping;//!< Damping for linear limit - btScalar m_restitution;//! Bounce parameter for linear limit - btVector3 m_normalCFM;//!< Constraint force mixing factor - btVector3 m_stopERP;//!< Error tolerance factor when joint is at limit - btVector3 m_stopCFM;//!< Constraint force mixing factor when joint is at limit - //!@} - bool m_enableMotor[3]; - btVector3 m_targetVelocity;//!< target motor velocity - btVector3 m_maxMotorForce;//!< max force on motor - btVector3 m_currentLimitError;//! How much is violated this limit - btVector3 m_currentLinearDiff;//! Current relative offset of constraint frames - int m_currentLimit[3];//!< 0=free, 1=at lower limit, 2=at upper limit - - btTranslationalLimitMotor() - { - m_lowerLimit.setValue(0.f,0.f,0.f); - m_upperLimit.setValue(0.f,0.f,0.f); - m_accumulatedImpulse.setValue(0.f,0.f,0.f); - m_normalCFM.setValue(0.f, 0.f, 0.f); - m_stopERP.setValue(0.2f, 0.2f, 0.2f); - m_stopCFM.setValue(0.f, 0.f, 0.f); - - m_limitSoftness = 0.7f; - m_damping = btScalar(1.0f); - m_restitution = btScalar(0.5f); - for(int i=0; i < 3; i++) - { - m_enableMotor[i] = false; - m_targetVelocity[i] = btScalar(0.f); - m_maxMotorForce[i] = btScalar(0.f); - } - } - - btTranslationalLimitMotor(const btTranslationalLimitMotor & other ) - { - m_lowerLimit = other.m_lowerLimit; - m_upperLimit = other.m_upperLimit; - m_accumulatedImpulse = other.m_accumulatedImpulse; - - m_limitSoftness = other.m_limitSoftness ; - m_damping = other.m_damping; - m_restitution = other.m_restitution; - m_normalCFM = other.m_normalCFM; - m_stopERP = other.m_stopERP; - m_stopCFM = other.m_stopCFM; - - for(int i=0; i < 3; i++) - { - m_enableMotor[i] = other.m_enableMotor[i]; - m_targetVelocity[i] = other.m_targetVelocity[i]; - m_maxMotorForce[i] = other.m_maxMotorForce[i]; - } - } - - //! Test limit - /*! - - free means upper < lower, - - locked means upper == lower - - limited means upper > lower - - limitIndex: first 3 are linear, next 3 are angular - */ - inline bool isLimited(int limitIndex) const - { - return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]); - } - inline bool needApplyForce(int limitIndex) const - { - if(m_currentLimit[limitIndex] == 0 && m_enableMotor[limitIndex] == false) return false; - return true; - } - int testLimitValue(int limitIndex, btScalar test_value); - - - btScalar solveLinearAxis( - btScalar timeStep, - btScalar jacDiagABInv, - btRigidBody& body1,const btVector3 &pointInA, - btRigidBody& body2,const btVector3 &pointInB, - int limit_index, - const btVector3 & axis_normal_on_a, - const btVector3 & anchorPos); - - -}; - -enum bt6DofFlags -{ - BT_6DOF_FLAGS_CFM_NORM = 1, - BT_6DOF_FLAGS_CFM_STOP = 2, - BT_6DOF_FLAGS_ERP_STOP = 4 -}; -#define BT_6DOF_FLAGS_AXIS_SHIFT 3 // bits per axis - - -/// btGeneric6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space -/*! -btGeneric6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'. -currently this limit supports rotational motors<br> -<ul> -<li> For Linear limits, use btGeneric6DofConstraint.setLinearUpperLimit, btGeneric6DofConstraint.setLinearLowerLimit. You can set the parameters with the btTranslationalLimitMotor structure accsesible through the btGeneric6DofConstraint.getTranslationalLimitMotor method. -At this moment translational motors are not supported. May be in the future. </li> - -<li> For Angular limits, use the btRotationalLimitMotor structure for configuring the limit. -This is accessible through btGeneric6DofConstraint.getLimitMotor method, -This brings support for limit parameters and motors. </li> - -<li> Angulars limits have these possible ranges: -<table border=1 > -<tr> - <td><b>AXIS</b></td> - <td><b>MIN ANGLE</b></td> - <td><b>MAX ANGLE</b></td> -</tr><tr> - <td>X</td> - <td>-PI</td> - <td>PI</td> -</tr><tr> - <td>Y</td> - <td>-PI/2</td> - <td>PI/2</td> -</tr><tr> - <td>Z</td> - <td>-PI</td> - <td>PI</td> -</tr> -</table> -</li> -</ul> - -*/ -ATTRIBUTE_ALIGNED16(class) btGeneric6DofConstraint : public btTypedConstraint -{ -protected: - - //! relative_frames - //!@{ - btTransform m_frameInA;//!< the constraint space w.r.t body A - btTransform m_frameInB;//!< the constraint space w.r.t body B - //!@} - - //! Jacobians - //!@{ - btJacobianEntry m_jacLinear[3];//!< 3 orthogonal linear constraints - btJacobianEntry m_jacAng[3];//!< 3 orthogonal angular constraints - //!@} - - //! Linear_Limit_parameters - //!@{ - btTranslationalLimitMotor m_linearLimits; - //!@} - - - //! hinge_parameters - //!@{ - btRotationalLimitMotor m_angularLimits[3]; - //!@} - - -protected: - //! temporal variables - //!@{ - btScalar m_timeStep; - btTransform m_calculatedTransformA; - btTransform m_calculatedTransformB; - btVector3 m_calculatedAxisAngleDiff; - btVector3 m_calculatedAxis[3]; - btVector3 m_calculatedLinearDiff; - btScalar m_factA; - btScalar m_factB; - bool m_hasStaticBody; - - btVector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes - - bool m_useLinearReferenceFrameA; - bool m_useOffsetForConstraintFrame; - - int m_flags; - - //!@} - - btGeneric6DofConstraint& operator=(btGeneric6DofConstraint& other) - { - btAssert(0); - (void) other; - return *this; - } - - - 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); - - int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB); - - void buildLinearJacobian( - btJacobianEntry & jacLinear,const btVector3 & normalWorld, - const btVector3 & pivotAInW,const btVector3 & pivotBInW); - - void buildAngularJacobian(btJacobianEntry & jacAngular,const btVector3 & jointAxisW); - - // tests linear limits - void calculateLinearInfo(); - - //! calcs the euler angles between the two bodies. - void calculateAngleInfo(); - - - -public: - - BT_DECLARE_ALIGNED_ALLOCATOR(); - - ///for backwards compatibility during the transition to 'getInfo/getInfo2' - bool m_useSolveConstraintObsolete; - - btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA); - btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB); - - //! Calcs global transform of the offsets - /*! - Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies. - \sa btGeneric6DofConstraint.getCalculatedTransformA , btGeneric6DofConstraint.getCalculatedTransformB, btGeneric6DofConstraint.calculateAngleInfo - */ - void calculateTransforms(const btTransform& transA,const btTransform& transB); - - void calculateTransforms(); - - //! Gets the global transform of the offset for body A - /*! - \sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo. - */ - const btTransform & getCalculatedTransformA() const - { - return m_calculatedTransformA; - } - - //! Gets the global transform of the offset for body B - /*! - \sa btGeneric6DofConstraint.getFrameOffsetA, btGeneric6DofConstraint.getFrameOffsetB, btGeneric6DofConstraint.calculateAngleInfo. - */ - const btTransform & getCalculatedTransformB() const - { - return m_calculatedTransformB; - } - - const btTransform & getFrameOffsetA() const - { - return m_frameInA; - } - - const btTransform & getFrameOffsetB() const - { - return m_frameInB; - } - - - btTransform & getFrameOffsetA() - { - return m_frameInA; - } - - btTransform & getFrameOffsetB() - { - return m_frameInB; - } - - - //! performs Jacobian calculation, and also calculates angle differences and axis - virtual void buildJacobian(); - - virtual void getInfo1 (btConstraintInfo1* info); - - void getInfo1NonVirtual (btConstraintInfo1* info); - - virtual void getInfo2 (btConstraintInfo2* info); - - void getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB); - - - void updateRHS(btScalar timeStep); - - //! Get the rotation axis in global coordinates - /*! - \pre btGeneric6DofConstraint.buildJacobian must be called previously. - */ - btVector3 getAxis(int axis_index) const; - - //! Get the relative Euler angle - /*! - \pre btGeneric6DofConstraint::calculateTransforms() must be called previously. - */ - btScalar getAngle(int axis_index) const; - - //! Get the relative position of the constraint pivot - /*! - \pre btGeneric6DofConstraint::calculateTransforms() must be called previously. - */ - btScalar getRelativePivotPosition(int axis_index) const; - - void setFrames(const btTransform & frameA, const btTransform & frameB); - - //! Test angular limit. - /*! - Calculates angular correction and returns true if limit needs to be corrected. - \pre btGeneric6DofConstraint::calculateTransforms() must be called previously. - */ - bool testAngularLimitMotor(int axis_index); - - void setLinearLowerLimit(const btVector3& linearLower) - { - m_linearLimits.m_lowerLimit = linearLower; - } - - void getLinearLowerLimit(btVector3& linearLower) const - { - linearLower = m_linearLimits.m_lowerLimit; - } - - void setLinearUpperLimit(const btVector3& linearUpper) - { - m_linearLimits.m_upperLimit = linearUpper; - } - - void getLinearUpperLimit(btVector3& linearUpper) const - { - linearUpper = m_linearLimits.m_upperLimit; - } - - void setAngularLowerLimit(const btVector3& angularLower) - { - for(int i = 0; i < 3; i++) - m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]); - } - - void getAngularLowerLimit(btVector3& angularLower) const - { - for(int i = 0; i < 3; i++) - angularLower[i] = m_angularLimits[i].m_loLimit; - } - - void setAngularUpperLimit(const btVector3& angularUpper) - { - for(int i = 0; i < 3; i++) - m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]); - } - - void getAngularUpperLimit(btVector3& angularUpper) const - { - for(int i = 0; i < 3; i++) - angularUpper[i] = m_angularLimits[i].m_hiLimit; - } - - //! Retrieves the angular limit informacion - btRotationalLimitMotor * getRotationalLimitMotor(int index) - { - return &m_angularLimits[index]; - } - - //! Retrieves the limit informacion - btTranslationalLimitMotor * getTranslationalLimitMotor() - { - return &m_linearLimits; - } - - //first 3 are linear, next 3 are angular - void setLimit(int axis, btScalar lo, btScalar hi) - { - if(axis<3) - { - m_linearLimits.m_lowerLimit[axis] = lo; - m_linearLimits.m_upperLimit[axis] = hi; - } - else - { - lo = btNormalizeAngle(lo); - hi = btNormalizeAngle(hi); - m_angularLimits[axis-3].m_loLimit = lo; - m_angularLimits[axis-3].m_hiLimit = hi; - } - } - - //! Test limit - /*! - - free means upper < lower, - - locked means upper == lower - - limited means upper > lower - - limitIndex: first 3 are linear, next 3 are angular - */ - bool isLimited(int limitIndex) const - { - if(limitIndex<3) - { - return m_linearLimits.isLimited(limitIndex); - - } - return m_angularLimits[limitIndex-3].isLimited(); - } - - virtual void calcAnchorPos(void); // overridable - - int get_limit_motor_info2( btRotationalLimitMotor * limot, - const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB, - btConstraintInfo2 *info, int row, btVector3& ax1, int rotational, int rotAllowed = false); - - // access for UseFrameOffset - bool getUseFrameOffset() const { return m_useOffsetForConstraintFrame; } - void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; } - - bool getUseLinearReferenceFrameA() const { return m_useLinearReferenceFrameA; } - void setUseLinearReferenceFrameA(bool linearReferenceFrameA) { m_useLinearReferenceFrameA = linearReferenceFrameA; } - - ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). - ///If no axis is provided, it uses the default axis for this constraint. - virtual void setParam(int num, btScalar value, int axis = -1); - ///return the local value of parameter - virtual btScalar getParam(int num, int axis = -1) const; - - void setAxis( const btVector3& axis1, const btVector3& axis2); - - virtual int getFlags() const - { - return m_flags; - } - - virtual int calculateSerializeBufferSize() const; - - ///fills the dataBuffer and returns the struct name (and 0 on failure) - virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; - - -}; - - -struct btGeneric6DofConstraintData -{ - btTypedConstraintData m_typeConstraintData; - btTransformFloatData m_rbAFrame; // constraint axii. Assumes z is hinge axis. - btTransformFloatData m_rbBFrame; - - btVector3FloatData m_linearUpperLimit; - btVector3FloatData m_linearLowerLimit; - - btVector3FloatData m_angularUpperLimit; - btVector3FloatData m_angularLowerLimit; - - int m_useLinearReferenceFrameA; - int m_useOffsetForConstraintFrame; -}; - -struct btGeneric6DofConstraintDoubleData2 -{ - btTypedConstraintDoubleData m_typeConstraintData; - btTransformDoubleData m_rbAFrame; // constraint axii. Assumes z is hinge axis. - btTransformDoubleData m_rbBFrame; - - btVector3DoubleData m_linearUpperLimit; - btVector3DoubleData m_linearLowerLimit; - - btVector3DoubleData m_angularUpperLimit; - btVector3DoubleData m_angularLowerLimit; - - int m_useLinearReferenceFrameA; - int m_useOffsetForConstraintFrame; -}; - -SIMD_FORCE_INLINE int btGeneric6DofConstraint::calculateSerializeBufferSize() const -{ - return sizeof(btGeneric6DofConstraintData2); -} - - ///fills the dataBuffer and returns the struct name (and 0 on failure) -SIMD_FORCE_INLINE const char* btGeneric6DofConstraint::serialize(void* dataBuffer, btSerializer* serializer) const -{ - - btGeneric6DofConstraintData2* dof = (btGeneric6DofConstraintData2*)dataBuffer; - btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer); - - m_frameInA.serialize(dof->m_rbAFrame); - m_frameInB.serialize(dof->m_rbBFrame); - - - int i; - for (i=0;i<3;i++) - { - dof->m_angularLowerLimit.m_floats[i] = m_angularLimits[i].m_loLimit; - dof->m_angularUpperLimit.m_floats[i] = m_angularLimits[i].m_hiLimit; - dof->m_linearLowerLimit.m_floats[i] = m_linearLimits.m_lowerLimit[i]; - dof->m_linearUpperLimit.m_floats[i] = m_linearLimits.m_upperLimit[i]; - } - - dof->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA? 1 : 0; - dof->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame ? 1 : 0; - - return btGeneric6DofConstraintDataName; -} - - - - - -#endif //BT_GENERIC_6DOF_CONSTRAINT_H |