"Motor at limit" jitter fixed for btGeneric6Dof constraint, fix taken from Bullet...
authorErwin Coumans <blender@erwincoumans.com>
Wed, 27 May 2009 01:29:41 +0000 (01:29 +0000)
committerErwin Coumans <blender@erwincoumans.com>
Wed, 27 May 2009 01:29:41 +0000 (01:29 +0000)
Now we need some cool constraint limit/motor/spring demos, such as a Forklift demo, moving robots, ragdolls etc. for Blender 2.49!

extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h

index 2edf28c6bac60b5fe57ca4d435d84a0010fbfcbc..cd34724932ebaa1c2fd02a11a3d72b45e9f46317 100644 (file)
@@ -420,6 +420,7 @@ void btGeneric6DofConstraint::buildAngularJacobian(
 bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
 {
        btScalar angle = m_calculatedAxisAngleDiff[axis_index];
+       m_angularLimits[axis_index].m_currentPosition = angle;
        //test limits
        m_angularLimits[axis_index].testLimitValue(angle);
        return m_angularLimits[axis_index].needApplyTorques();
@@ -544,6 +545,7 @@ int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info)
                { // re-use rotational motor code
                        limot.m_bounce = btScalar(0.f);
                        limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
+                       limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i];
                        limot.m_currentLimitError  = m_linearLimits.m_currentLimitError[i];
                        limot.m_damping  = m_linearLimits.m_damping;
                        limot.m_enableMotor  = m_linearLimits.m_enableMotor[i];
@@ -696,6 +698,7 @@ void btGeneric6DofConstraint::calculateLinearInfo()
        m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;
        for(int i = 0; i < 3; i++)
        {
+               m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i];
                m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
        }
 }
@@ -740,16 +743,21 @@ int btGeneric6DofConstraint::get_limit_motor_info2(
         }
         // if we're limited low and high simultaneously, the joint motor is
         // ineffective
-        if (limit && (limot->m_loLimit == limot->m_hiLimit)) 
-                       powered = 0;
-
+        if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0;
         info->m_constraintError[srow] = btScalar(0.f);
         if (powered)
         {
             info->cfm[srow] = 0.0f;
-            if(!limit)
+//            if(!limit)
             {
-                info->m_constraintError[srow] += limot->m_targetVelocity;
+                               btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
+
+                               btScalar mot_fact = getMotorFactor(     limot->m_currentPosition, 
+                                                                                                       limot->m_loLimit,
+                                                                                                       limot->m_hiLimit, 
+                                                                                                       tag_vel, 
+                                                                                                       info->fps * info->erp);
+                               info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
                 info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
                 info->m_upperLimit[srow] = limot->m_maxMotorForce;
             }
@@ -831,6 +839,7 @@ int btGeneric6DofConstraint::get_limit_motor_info2(
 
 
 
+
 btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA)
        : btGeneric6DofConstraint(rbA, rbB, frameInA, frameInB, useLinearReferenceFrameA)
 {
index 33e4749ffd07b1b10aca1f7d3406d31cd90bb402..d01c9cf63abca6267b128d4fbb6abfca461d77d3 100644 (file)
@@ -54,6 +54,7 @@ public:
     //! 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;
     //!@}
@@ -134,6 +135,7 @@ public:
     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()
@@ -478,6 +480,16 @@ public:
 };
 
 
+/// Generic 6 DOF constraint that allows to set spring motors to any translational and rotational DOF
+
+/// 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] )
+
 class btGeneric6DofSpringConstraint : public btGeneric6DofConstraint
 {
 protected:
@@ -494,4 +506,5 @@ public:
        virtual void getInfo2 (btConstraintInfo2* info);
 };
 
+
 #endif //GENERIC_6DOF_CONSTRAINT_H