reverted to old hinge constraint, it breaks several tests/demos
authorErwin Coumans <blender@erwincoumans.com>
Sun, 5 Aug 2007 17:44:32 +0000 (17:44 +0000)
committerErwin Coumans <blender@erwincoumans.com>
Sun, 5 Aug 2007 17:44:32 +0000 (17:44 +0000)
extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h

index ad7fc3a269e875fa0f938663dfc0ab1a6eb9493b..27e3098754958e2ce7c723bc73e5fe39d609a095 100644 (file)
@@ -17,176 +17,58 @@ subject to the following restrictions:
 #include "btHingeConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "LinearMath/btTransformUtil.h"
-#include "LinearMath/btSimdMinMax.h"
 #include <new>
 
-
 btHingeConstraint::btHingeConstraint():
 m_enableAngularMotor(false)
 {
 }
 
 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
-                                                                        btVector3& axisInA,btVector3& axisInB)
-                                                                        :btTypedConstraint(rbA,rbB),
-                                                                        m_angularOnly(false),
-                                                                        m_enableAngularMotor(false)
-{
-       m_rbAFrame.getOrigin() = pivotInA;
-       
-       // since no frame is given, assume this to be zero angle and just pick rb transform axis
-       btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);
-       btScalar projection = rbAxisA1.dot(axisInA);
-       if (projection > SIMD_EPSILON)
-               rbAxisA1 = rbAxisA1*projection - axisInA;
-        else
-               rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
-       
-       btVector3 rbAxisA2 = rbAxisA1.cross(axisInA);
-
-       m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
-                                                                       rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
-                                                                       rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
-
-       btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
-       btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
-       btVector3 rbAxisB2 =  rbAxisB1.cross(axisInB);
-       
-       
-       m_rbBFrame.getOrigin() = pivotInB;
-       m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),-axisInB.getX(),
-                                                                       rbAxisB1.getY(),rbAxisB2.getY(),-axisInB.getY(),
-                                                                       rbAxisB1.getZ(),rbAxisB2.getZ(),-axisInB.getZ() );
-       
-       //start with free
-       m_lowerLimit = btScalar(1e30);
-       m_upperLimit = btScalar(-1e30);
-       m_biasFactor = 0.3f;
-       m_relaxationFactor = 1.0f;
-       m_limitSoftness = 0.9f;
-       m_solveLimit = false;
-
-}
-
-
-btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA)
-:btTypedConstraint(rbA), m_angularOnly(false), m_enableAngularMotor(false)
-{
-
-       // since no frame is given, assume this to be zero angle and just pick rb transform axis
-       // fixed axis in worldspace
-       btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);
-       btScalar projection = rbAxisA1.dot(axisInA);
-       if (projection > SIMD_EPSILON)
-               rbAxisA1 = rbAxisA1*projection - axisInA;
-       else
-               rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
-
-       btVector3 rbAxisA2 = axisInA.cross(rbAxisA1);
-
-       m_rbAFrame.getOrigin() = pivotInA;
-       m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
-                                                                       rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
-                                                                       rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
-
-
-       btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * -axisInA;
-
-       btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
-       btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
-       btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
-
-
-       m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA);
-       m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
-                                                                       rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
-                                                                       rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
-       
-       //start with free
-       m_lowerLimit = btScalar(1e30);
-       m_upperLimit = btScalar(-1e30);
-       m_biasFactor = 0.3f;
-       m_relaxationFactor = 1.0f;
-       m_limitSoftness = 0.9f;
-       m_solveLimit = false;
-}
-
-btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, 
-                                                                    const btTransform& rbAFrame, const btTransform& rbBFrame)
-:btTypedConstraint(rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
+                                                                btVector3& axisInA,btVector3& axisInB)
+:btTypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
+m_axisInA(axisInA),
+m_axisInB(-axisInB),
 m_angularOnly(false),
 m_enableAngularMotor(false)
 {
-       // flip axis
-       m_rbBFrame.getBasis()[2][0] *= btScalar(-1.);
-       m_rbBFrame.getBasis()[2][1] *= btScalar(-1.);
-       m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
-
-       //start with free
-       m_lowerLimit = btScalar(1e30);
-       m_upperLimit = btScalar(-1e30);
-       m_biasFactor = 0.3f;
-       m_relaxationFactor = 1.0f;
-       m_limitSoftness = 0.9f;
-       m_solveLimit = false;
-}                      
 
+}
 
 
-btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame)
-:btTypedConstraint(rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame),
+btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA)
+:btTypedConstraint(rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
+m_axisInA(axisInA),
+//fixed axis in worldspace
+m_axisInB(rbA.getCenterOfMassTransform().getBasis() * -axisInA),
 m_angularOnly(false),
 m_enableAngularMotor(false)
 {
-       // flip axis
-       m_rbBFrame.getBasis()[2][0] *= btScalar(-1.);
-       m_rbBFrame.getBasis()[2][1] *= btScalar(-1.);
-       m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
-
-
-       //start with free
-       m_lowerLimit = btScalar(1e30);
-       m_upperLimit = btScalar(-1e30); 
-       m_biasFactor = 0.3f;
-       m_relaxationFactor = 1.0f;
-       m_limitSoftness = 0.9f;
-       m_solveLimit = false;
+       
 }
 
 void   btHingeConstraint::buildJacobian()
 {
        m_appliedImpulse = btScalar(0.);
 
+       btVector3       normal(0,0,0);
+
        if (!m_angularOnly)
        {
-               btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
-               btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
-               btVector3 relPos = pivotBInW - pivotAInW;
-
-               btVector3 normal[3];
-               if (relPos.length2() > SIMD_EPSILON)
-               {
-                       normal[0] = relPos.normalized();
-               }
-               else
-               {
-                       normal[0].setValue(btScalar(1.0),0,0);
-               }
-
-               btPlaneSpace1(normal[0], normal[1], normal[2]);
-
                for (int i=0;i<3;i++)
                {
+                       normal[i] = 1;
                        new (&m_jac[i]) btJacobianEntry(
                                m_rbA.getCenterOfMassTransform().getBasis().transpose(),
                                m_rbB.getCenterOfMassTransform().getBasis().transpose(),
-                               pivotAInW - m_rbA.getCenterOfMassPosition(),
-                               pivotBInW - m_rbB.getCenterOfMassPosition(),
-                               normal[i],
+                               m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(),
+                               m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(),
+                               normal,
                                m_rbA.getInvInertiaDiagLocal(),
                                m_rbA.getInvMass(),
                                m_rbB.getInvInertiaDiagLocal(),
                                m_rbB.getInvMass());
+                       normal[i] = 0;
                }
        }
 
@@ -197,12 +79,12 @@ void       btHingeConstraint::buildJacobian()
        btVector3 jointAxis0local;
        btVector3 jointAxis1local;
        
-       btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
+       btPlaneSpace1(m_axisInA,jointAxis0local,jointAxis1local);
 
-       getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
+       getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
        btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
        btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
-       btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
+       btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
                
        new (&m_jacAng[0])      btJacobianEntry(jointAxis0,
                m_rbA.getCenterOfMassTransform().getBasis().transpose(),
@@ -223,71 +105,44 @@ void      btHingeConstraint::buildJacobian()
                m_rbB.getInvInertiaDiagLocal());
 
 
-       // Compute limit information
-       btScalar hingeAngle = getHingeAngle();  
-
-       //set bias, sign, clear accumulator
-       m_correction = btScalar(0.);
-       m_limitSign = btScalar(0.);
-       m_solveLimit = false;
-       m_accLimitImpulse = btScalar(0.);
-
-       if (m_lowerLimit < m_upperLimit)
-       {
-               if (hingeAngle <= m_lowerLimit*m_limitSoftness)
-               {
-                       m_correction = (m_lowerLimit - hingeAngle);
-                       m_limitSign = 1.0f;
-                       m_solveLimit = true;
-               } 
-               else if (hingeAngle >= m_upperLimit*m_limitSoftness)
-               {
-                       m_correction = m_upperLimit - hingeAngle;
-                       m_limitSign = -1.0f;
-                       m_solveLimit = true;
-               }
-       }
-
-       //Compute K = J*W*J' for hinge axis
-       btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
-       m_kHinge =   1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
-                                    getRigidBodyB().computeAngularImpulseDenominator(axisA));
 
 }
 
 void   btHingeConstraint::solveConstraint(btScalar     timeStep)
 {
 
-       btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
-       btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
+       btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
+       btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
 
+       btVector3 normal(0,0,0);
        btScalar tau = btScalar(0.3);
        btScalar damping = btScalar(1.);
 
 //linear part
        if (!m_angularOnly)
        {
-               btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
-               btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
-
-               btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
-               btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
-               btVector3 vel = vel1 - vel2;
-
                for (int i=0;i<3;i++)
                {               
-                       const btVector3& normal = m_jac[i].m_linearJointAxis;
+                       normal[i] = 1;
                        btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
 
+                       btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
+                       btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
+                       
+                       btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
+                       btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
+                       btVector3 vel = vel1 - vel2;
                        btScalar rel_vel;
                        rel_vel = normal.dot(vel);
                        //positional error (zeroth order error)
                        btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
-                       btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
+                       btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  damping * rel_vel * jacDiagABInv * damping;
                        m_appliedImpulse += impulse;
                        btVector3 impulse_vector = normal * impulse;
                        m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
                        m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
+                       
+                       normal[i] = 0;
                }
        }
 
@@ -296,8 +151,8 @@ void        btHingeConstraint::solveConstraint(btScalar     timeStep)
                ///solve angular part
 
                // get axes in world space
-               btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
-               btVector3 axisB =  getRigidBodyB().getCenterOfMassTransform().getBasis() *  m_rbBFrame.getBasis().getColumn(2);
+               btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
+               btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_axisInB;
 
                const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
                const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
@@ -319,7 +174,7 @@ void        btHingeConstraint::solveConstraint(btScalar     timeStep)
                                        getRigidBodyB().computeAngularImpulseDenominator(normal);
                                // scale for mass and relaxation
                                //todo:  expose this 0.9 factor to developer
-                               velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor;
+                               velrelOrthog *= (btScalar(1.)/denom) * btScalar(0.9);
                        }
 
                        //solve angular positional correction
@@ -335,28 +190,10 @@ void      btHingeConstraint::solveConstraint(btScalar     timeStep)
 
                        m_rbA.applyTorqueImpulse(-velrelOrthog+angularError);
                        m_rbB.applyTorqueImpulse(velrelOrthog-angularError);
-
-                       // solve limit
-                       if (m_solveLimit)
-                       {
-                               btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor  ) * m_limitSign;
-
-                               btScalar impulseMag = amplitude * m_kHinge;
-
-                               // Clamp the accumulated impulse
-                               btScalar temp = m_accLimitImpulse;
-                               m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, 0.0f );
-                               impulseMag = m_accLimitImpulse - temp;
-
-
-                               btVector3 impulse = axisA * impulseMag * m_limitSign;
-                               m_rbA.applyTorqueImpulse(impulse);
-                               m_rbB.applyTorqueImpulse(-impulse);
-                       }
                }
 
                //apply motor
-               if (m_enableAngularMotor) 
+               if (m_enableAngularMotor)
                {
                        //todo: add limits too
                        btVector3 angularLimit(0,0,0);
@@ -367,7 +204,10 @@ void       btHingeConstraint::solveConstraint(btScalar     timeStep)
                        btScalar desiredMotorVel = m_motorTargetVelocity;
                        btScalar motor_relvel = desiredMotorVel - projRelVel;
 
-                       btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;;
+                       btScalar denom3 = getRigidBodyA().computeAngularImpulseDenominator(axisA) +
+                                       getRigidBodyB().computeAngularImpulseDenominator(axisA);
+
+                       btScalar unclippedMotorImpulse = (btScalar(1.)/denom3) * motor_relvel;;
                        //todo: should clip against accumulated impulse
                        btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse;
                        clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse;
@@ -387,11 +227,3 @@ void       btHingeConstraint::updateRHS(btScalar   timeStep)
 
 }
 
-btScalar btHingeConstraint::getHingeAngle()
-{
-       const btVector3 refAxis0  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0);
-       const btVector3 refAxis1  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1);
-       const btVector3 swingAxis = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(1);
-
-       return btAtan2Fast( swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)  );
-}
index 4b42d3d2acda89ef3a4820f5f3817566fa4109ae..5c1ceafbc5bd92372ea31be2d968e5fd64317aa2 100644 (file)
@@ -13,8 +13,6 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
-/* Hinge Constraint by Dirk Gregorius. Limits added by Marcus Hennix at Starbreeze Studios */
-
 #ifndef HINGECONSTRAINT_H
 #define HINGECONSTRAINT_H
 
@@ -24,6 +22,7 @@ subject to the following restrictions:
 
 class btRigidBody;
 
+
 /// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
 /// axis defines the orientation of the hinge axis
 class btHingeConstraint : public btTypedConstraint
@@ -31,40 +30,22 @@ class btHingeConstraint : public btTypedConstraint
        btJacobianEntry m_jac[3]; //3 orthogonal linear constraints
        btJacobianEntry m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor
 
-       btTransform m_rbAFrame; // constraint axii. Assumes z is hinge axis.
-       btTransform m_rbBFrame;
-
-       btScalar        m_motorTargetVelocity;
-       btScalar        m_maxMotorImpulse;
-
-       btScalar        m_limitSoftness; 
-       btScalar        m_biasFactor; 
-       btScalar    m_relaxationFactor; 
-
-       btScalar    m_lowerLimit;       
-       btScalar    m_upperLimit;       
-       
-       btScalar        m_kHinge;
-
-       btScalar        m_limitSign;
-       btScalar        m_correction;
-
-       btScalar        m_accLimitImpulse;
+       btVector3       m_pivotInA;
+       btVector3       m_pivotInB;
+       btVector3       m_axisInA;
+       btVector3       m_axisInB;
 
        bool            m_angularOnly;
-       bool            m_enableAngularMotor;
-       bool            m_solveLimit;
 
+       btScalar                m_motorTargetVelocity;
+       btScalar                m_maxMotorImpulse;
+       bool            m_enableAngularMotor;
        
 public:
 
-       btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, btVector3& axisInA,btVector3& axisInB);
+       btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,btVector3& axisInA,btVector3& axisInB);
 
        btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA);
-       
-       btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame);
-
-       btHingeConstraint(btRigidBody& rbA,const btTransform& rbAFrame);
 
        btHingeConstraint();
 
@@ -95,33 +76,6 @@ public:
                m_maxMotorImpulse = maxMotorImpulse;
        }
 
-       void    setLimit(btScalar low,btScalar high,btScalar _softness = 0.9f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
-       {
-               m_lowerLimit = low;
-               m_upperLimit = high;
-
-               m_limitSoftness =  _softness;
-               m_biasFactor = _biasFactor;
-               m_relaxationFactor = _relaxationFactor;
-
-       }
-
-       btScalar getHingeAngle();
-
-
-       const btTransform& getAFrame() { return m_rbAFrame; };  
-       const btTransform& getBFrame() { return m_rbBFrame; };
-
-       inline int getSolveLimit()
-       {
-               return m_solveLimit;
-       }
-
-       inline btScalar getLimitSign()
-       {
-               return m_limitSign;
-       }
-               
 };
 
 #endif //HINGECONSTRAINT_H