bullet: Update to current svn, r2636
[blender.git] / extern / bullet2 / src / BulletDynamics / ConstraintSolver / btConeTwistConstraint.cpp
index bc371e4062dd24fa498848a48c15eb101e3cb080..15a4c92de20ba45c0140db43eef823905c1f99a3 100644 (file)
@@ -53,6 +53,7 @@ btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform&
                                                                                         m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
 {
        m_rbBFrame = m_rbAFrame;
+       m_rbBFrame.setOrigin(btVector3(0., 0., 0.));
        init(); 
 }
 
@@ -136,6 +137,9 @@ void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const bt
                btVector3 a1neg = -a1;
                a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
        }
+    info->m_J2linearAxis[0] = -1;
+    info->m_J2linearAxis[info->rowskip+1] = -1;
+    info->m_J2linearAxis[2*info->rowskip+2] = -1;
        btVector3 a2 = transB.getBasis() * m_rbBFrame.getOrigin();
        {
                btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
@@ -304,7 +308,7 @@ void        btConeTwistConstraint::buildJacobian()
 
 
 
-void   btConeTwistConstraint::solveConstraintObsolete(btRigidBody& bodyA,btRigidBody& bodyB,btScalar   timeStep)
+void   btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
 {
        #ifndef __SPU__
        if (m_useSolveConstraintObsolete)
@@ -506,7 +510,7 @@ void        btConeTwistConstraint::solveConstraintObsolete(btRigidBody& bodyA,btRigidBo
                                m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) );
                                impulseMag = m_accTwistLimitImpulse - temp;
 
-                               btVector3 impulse = m_twistAxis * impulseMag;
+               //              btVector3 impulse = m_twistAxis * impulseMag;
 
                                bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag);
                                bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag);
@@ -638,6 +642,11 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
                btTransform trDeltaAB = trB * trPose * trA.inverse();
                btQuaternion qDeltaAB = trDeltaAB.getRotation();
                btVector3 swingAxis =   btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z());
+               float swingAxisLen2 = swingAxis.length2();
+               if(btFuzzyZero(swingAxisLen2))
+               {
+                  return;
+               }
                m_swingAxis = swingAxis;
                m_swingAxis.normalize();
                m_swingCorrection = qDeltaAB.getAngle();
@@ -720,7 +729,8 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
                        {
                                if(m_swingSpan1 < m_fixThresh)
                                { // hinge around Y axis
-                                       if(!(btFuzzyZero(y)))
+//                                     if(!(btFuzzyZero(y)))
+                                       if((!(btFuzzyZero(x))) || (!(btFuzzyZero(z))))
                                        {
                                                m_solveSwingLimit = true;
                                                if(m_swingSpan2 >= m_fixThresh)
@@ -742,7 +752,8 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr
                                }
                                else
                                { // hinge around Z axis
-                                       if(!btFuzzyZero(z))
+//                                     if(!btFuzzyZero(z))
+                                       if((!(btFuzzyZero(x))) || (!(btFuzzyZero(y))))
                                        {
                                                m_solveSwingLimit = true;
                                                if(m_swingSpan1 >= m_fixThresh)
@@ -823,12 +834,11 @@ void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
        {
                vSwingAxis = btVector3(qCone.x(), qCone.y(), qCone.z());
                vSwingAxis.normalize();
-               if (fabs(vSwingAxis.x()) > SIMD_EPSILON)
-               {
-                       // non-zero twist?! this should never happen.
-                       int wtf = 0; wtf = wtf;
-               }
-
+#if 0
+        // non-zero twist?! this should never happen.
+       btAssert(fabs(vSwingAxis.x()) <= SIMD_EPSILON));
+#endif
+        
                // Compute limit for given swing. tricky:
                // Given a swing axis, we're looking for the intersection with the bounding cone ellipse.
                // (Since we're dealing with angles, this ellipse is embedded on the surface of a sphere.)
@@ -872,8 +882,10 @@ void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
        else if (swingAngle < 0)
        {
                // this should never happen!
-               int wtf = 0; wtf = wtf;
-       }
+#if 0
+        btAssert(0);
+#endif
+       }
 }
 
 btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const
@@ -918,13 +930,15 @@ void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist,
 
        if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
        {
-               qMinTwist = operator-(qTwist);
+               qMinTwist = -(qTwist);
                twistAngle = qMinTwist.getAngle();
        }
        if (twistAngle < 0)
        {
                // this should never happen
-               int wtf = 0; wtf = wtf;                 
+#if 0
+        btAssert(0);
+#endif
        }
 
        vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
@@ -971,10 +985,10 @@ void btConeTwistConstraint::setMotorTarget(const btQuaternion &q)
 {
        btTransform trACur = m_rbA.getCenterOfMassTransform();
        btTransform trBCur = m_rbB.getCenterOfMassTransform();
-       btTransform trABCur = trBCur.inverse() * trACur;
-       btQuaternion qABCur = trABCur.getRotation();
-       btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame);
-       btQuaternion qConstraintCur = trConstraintCur.getRotation();
+//     btTransform trABCur = trBCur.inverse() * trACur;
+//     btQuaternion qABCur = trABCur.getRotation();
+//     btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame);
+       //btQuaternion qConstraintCur = trConstraintCur.getRotation();
 
        btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * q * m_rbAFrame.getRotation();
        setMotorTargetInConstraintSpace(qConstraint);