diff options
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h')
| -rw-r--r-- | extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h | 231 |
1 files changed, 163 insertions, 68 deletions
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h index 8082eb1f132..4fdac113378 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h @@ -12,6 +12,10 @@ subject to the following restrictions: 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 @@ -45,7 +49,9 @@ public: btScalar m_maxLimitForce;//!< max force on limit btScalar m_damping;//!< Damping. btScalar m_limitSoftness;//! Relaxation factor - btScalar m_ERP;//!< Error tolerance factor when joint is at limit + 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; @@ -65,9 +71,11 @@ public: m_targetVelocity = 0; m_maxMotorForce = 0.1f; m_maxLimitForce = 300.0f; - m_loLimit = -SIMD_INFINITY; - m_hiLimit = SIMD_INFINITY; - m_ERP = 0.5f; + 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; @@ -83,7 +91,9 @@ public: m_limitSoftness = limot.m_limitSoftness; m_loLimit = limot.m_loLimit; m_hiLimit = limot.m_hiLimit; - m_ERP = limot.m_ERP; + 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; @@ -113,7 +123,7 @@ public: int testLimitValue(btScalar test_value); //! apply the correction impulses for two bodies - btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btSolverBody& bodyA,btRigidBody * body1,btSolverBody& bodyB); + btScalar solveAngularLimits(btScalar timeStep,btVector3& axis, btScalar jacDiagABInv,btRigidBody * body0, btRigidBody * body1); }; @@ -130,6 +140,9 @@ public: 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 @@ -143,6 +156,9 @@ public: 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); @@ -164,6 +180,10 @@ public: 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]; @@ -194,8 +214,8 @@ public: btScalar solveLinearAxis( btScalar timeStep, btScalar jacDiagABInv, - btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA, - btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB, + btRigidBody& body1,const btVector3 &pointInA, + btRigidBody& body2,const btVector3 &pointInB, int limit_index, const btVector3 & axis_normal_on_a, const btVector3 & anchorPos); @@ -203,6 +223,15 @@ public: }; +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'. @@ -217,20 +246,22 @@ This brings support for limit parameters and motors. </li> <li> Angulars limits have these possible ranges: <table border=1 > -<tr - +<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> + <td>-PI</td> + <td>PI</td> +</tr><tr> <td>Y</td> - <td>-PI/2</td> - <td>PI/2</td> + <td>-PI/2</td> + <td>PI/2</td> +</tr><tr> <td>Z</td> - <td>-PI/2</td> - <td>PI/2</td> + <td>-PI</td> + <td>PI</td> </tr> </table> </li> @@ -274,11 +305,17 @@ protected: 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) @@ -289,9 +326,9 @@ protected: } - int setAngularLimits(btConstraintInfo2 *info, int row_offset); + 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 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, @@ -313,15 +350,16 @@ public: bool m_useSolveConstraintObsolete; btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA); - - btGeneric6DofConstraint(); - + 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(); + void calculateTransforms(const btTransform& transA,const btTransform& transB); + + void calculateTransforms(); //! Gets the global transform of the offset for body A /*! @@ -368,9 +406,12 @@ public: virtual void getInfo1 (btConstraintInfo1* info); + void getInfo1NonVirtual (btConstraintInfo1* info); + virtual void getInfo2 (btConstraintInfo2* info); - virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep); + 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); @@ -392,6 +433,7 @@ public: */ btScalar getRelativePivotPosition(int axis_index) const; + void setFrames(const btTransform & frameA, const btTransform & frameB); //! Test angular limit. /*! @@ -405,25 +447,45 @@ public: m_linearLimits.m_lowerLimit = linearLower; } - void setLinearUpperLimit(const btVector3& linearUpper) - { - m_linearLimits.m_upperLimit = linearUpper; - } + void getLinearLowerLimit(btVector3& linearLower) + { + linearLower = m_linearLimits.m_lowerLimit; + } + + void setLinearUpperLimit(const btVector3& linearUpper) + { + m_linearLimits.m_upperLimit = linearUpper; + } + + void getLinearUpperLimit(btVector3& linearUpper) + { + linearUpper = m_linearLimits.m_upperLimit; + } void setAngularLowerLimit(const btVector3& angularLower) { - m_angularLimits[0].m_loLimit = angularLower.getX(); - m_angularLimits[1].m_loLimit = angularLower.getY(); - m_angularLimits[2].m_loLimit = angularLower.getZ(); + for(int i = 0; i < 3; i++) + m_angularLimits[i].m_loLimit = btNormalizeAngle(angularLower[i]); } + void getAngularLowerLimit(btVector3& angularLower) + { + for(int i = 0; i < 3; i++) + angularLower[i] = m_angularLimits[i].m_loLimit; + } + void setAngularUpperLimit(const btVector3& angularUpper) { - m_angularLimits[0].m_hiLimit = angularUpper.getX(); - m_angularLimits[1].m_hiLimit = angularUpper.getY(); - m_angularLimits[2].m_hiLimit = angularUpper.getZ(); + for(int i = 0; i < 3; i++) + m_angularLimits[i].m_hiLimit = btNormalizeAngle(angularUpper[i]); } + void getAngularUpperLimit(btVector3& angularUpper) + { + for(int i = 0; i < 3; i++) + angularUpper[i] = m_angularLimits[i].m_hiLimit; + } + //! Retrieves the angular limit informacion btRotationalLimitMotor * getRotationalLimitMotor(int index) { @@ -446,6 +508,8 @@ public: } else { + lo = btNormalizeAngle(lo); + hi = btNormalizeAngle(hi); m_angularLimits[axis-3].m_loLimit = lo; m_angularLimits[axis-3].m_hiLimit = hi; } @@ -468,52 +532,83 @@ public: return m_angularLimits[limitIndex-3].isLimited(); } - const btRigidBody& getRigidBodyA() const - { - return m_rbA; - } - const btRigidBody& getRigidBodyB() const - { - return m_rbB; - } - virtual void calcAnchorPos(void); // overridable int get_limit_motor_info2( btRotationalLimitMotor * limot, - btRigidBody * body0, btRigidBody * body1, - btConstraintInfo2 *info, int row, btVector3& ax1, int rotational); + 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() { return m_useOffsetForConstraintFrame; } + void setUseFrameOffset(bool frameOffsetOnOff) { m_useOffsetForConstraintFrame = frameOffsetOnOff; } -}; + ///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); -/// Generic 6 DOF constraint that allows to set spring motors to any translational and rotational DOF + virtual int calculateSerializeBufferSize() const; -/// DOF index used in enableSpring() and setStiffness() means: -/// 0 : translation X -/// 1 : translation Y -/// 2 : translation Z -/// 3 : rotation X (3rd Euler rotational around new position of X axis, range [-PI+epsilon, PI-epsilon] ) -/// 4 : rotation Y (2nd Euler rotational around new position of Y axis, range [-PI/2+epsilon, PI/2-epsilon] ) -/// 5 : rotation Z (1st Euler rotational around Z axis, range [-PI+epsilon, PI-epsilon] ) + ///fills the dataBuffer and returns the struct name (and 0 on failure) + virtual const char* serialize(void* dataBuffer, btSerializer* serializer) const; -class btGeneric6DofSpringConstraint : public btGeneric6DofConstraint + +}; + +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btGeneric6DofConstraintData { -protected: - bool m_springEnabled[6]; - btScalar m_equilibriumPoint[6]; - btScalar m_springStiffness[6]; - btScalar m_springDamping[6]; // between 0 and 1 (1 == no damping) - void internalUpdateSprings(btConstraintInfo2* info); -public: - btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA); - void enableSpring(int index, bool onOff); - void setStiffness(int index, btScalar stiffness); - void setDamping(int index, btScalar damping); - void setEquilibriumPoint(); // set the current constraint position/orientation as an equilibrium point for all DOF - void setEquilibriumPoint(int index); // set the current constraint position/orientation as an equilibrium point for given DOF - virtual void getInfo2 (btConstraintInfo2* info); + 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; }; +SIMD_FORCE_INLINE int btGeneric6DofConstraint::calculateSerializeBufferSize() const +{ + return sizeof(btGeneric6DofConstraintData); +} + + ///fills the dataBuffer and returns the struct name (and 0 on failure) +SIMD_FORCE_INLINE const char* btGeneric6DofConstraint::serialize(void* dataBuffer, btSerializer* serializer) const +{ + + btGeneric6DofConstraintData* dof = (btGeneric6DofConstraintData*)dataBuffer; + btTypedConstraint::serialize(&dof->m_typeConstraintData,serializer); + + m_frameInA.serializeFloat(dof->m_rbAFrame); + m_frameInB.serializeFloat(dof->m_rbBFrame); + + + int i; + for (i=0;i<3;i++) + { + dof->m_angularLowerLimit.m_floats[i] = float(m_angularLimits[i].m_loLimit); + dof->m_angularUpperLimit.m_floats[i] = float(m_angularLimits[i].m_hiLimit); + dof->m_linearLowerLimit.m_floats[i] = float(m_linearLimits.m_lowerLimit[i]); + dof->m_linearUpperLimit.m_floats[i] = float(m_linearLimits.m_upperLimit[i]); + } + + dof->m_useLinearReferenceFrameA = m_useLinearReferenceFrameA? 1 : 0; + dof->m_useOffsetForConstraintFrame = m_useOffsetForConstraintFrame ? 1 : 0; + + return "btGeneric6DofConstraintData"; +} + + + + #endif //GENERIC_6DOF_CONSTRAINT_H |