bullet: Update to current svn, r2636
[blender.git] / extern / bullet2 / src / BulletDynamics / ConstraintSolver / btConeTwistConstraint.cpp
index 50a79451f5d59e33e6e02a0649b78aa98033257d..15a4c92de20ba45c0140db43eef823905c1f99a3 100644 (file)
@@ -29,14 +29,15 @@ Written by: Marcus Hennix
 #define CONETWIST_DEF_FIX_THRESH btScalar(.05f)
 
 
 #define CONETWIST_DEF_FIX_THRESH btScalar(.05f)
 
 
-
-btConeTwistConstraint::btConeTwistConstraint()
-:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE),
-m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
+SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis, const btMatrix3x3& invInertiaWorld)
 {
 {
+       btVector3 vec = axis * invInertiaWorld;
+       return axis.dot(vec);
 }
 
 
 }
 
 
+
+
 btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB, 
                                                                                         const btTransform& rbAFrame,const btTransform& rbBFrame)
                                                                                         :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
 btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB, 
                                                                                         const btTransform& rbAFrame,const btTransform& rbBFrame)
                                                                                         :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
@@ -52,6 +53,7 @@ btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform&
                                                                                         m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
 {
        m_rbBFrame = m_rbAFrame;
                                                                                         m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
 {
        m_rbBFrame = m_rbAFrame;
+       m_rbBFrame.setOrigin(btVector3(0., 0., 0.));
        init(); 
 }
 
        init(); 
 }
 
@@ -67,11 +69,13 @@ void btConeTwistConstraint::init()
        setLimit(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
        m_damping = btScalar(0.01);
        m_fixThresh = CONETWIST_DEF_FIX_THRESH;
        setLimit(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
        m_damping = btScalar(0.01);
        m_fixThresh = CONETWIST_DEF_FIX_THRESH;
+       m_flags = 0;
+       m_linCFM = btScalar(0.f);
+       m_linERP = btScalar(0.7f);
+       m_angCFM = btScalar(0.f);
 }
 
 
 }
 
 
-
-
 void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
 {
        if (m_useSolveConstraintObsolete)
 void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
 {
        if (m_useSolveConstraintObsolete)
@@ -83,7 +87,7 @@ void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
        {
                info->m_numConstraintRows = 3;
                info->nub = 3;
        {
                info->m_numConstraintRows = 3;
                info->nub = 3;
-               calcAngleInfo2();
+               calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
                if(m_solveSwingLimit)
                {
                        info->m_numConstraintRows++;
                if(m_solveSwingLimit)
                {
                        info->m_numConstraintRows++;
@@ -101,22 +105,31 @@ void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
                }
        }
 }
                }
        }
 }
-       
 
 
+void btConeTwistConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
+{
+       //always reserve 6 rows: object transform is not available on SPU
+       info->m_numConstraintRows = 6;
+       info->nub = 0;
+               
+}
+       
 
 void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
 {
 
 void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
 {
+       getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
+}
+
+void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB)
+{
+       calcAngleInfo2(transA,transB,invInertiaWorldA,invInertiaWorldB);
+       
        btAssert(!m_useSolveConstraintObsolete);
        btAssert(!m_useSolveConstraintObsolete);
-       //retrieve matrices
-       btTransform body0_trans;
-       body0_trans = m_rbA.getCenterOfMassTransform();
-    btTransform body1_trans;
-       body1_trans = m_rbB.getCenterOfMassTransform();
     // set jacobian
     info->m_J1linearAxis[0] = 1;
     info->m_J1linearAxis[info->rowskip+1] = 1;
     info->m_J1linearAxis[2*info->rowskip+2] = 1;
     // set jacobian
     info->m_J1linearAxis[0] = 1;
     info->m_J1linearAxis[info->rowskip+1] = 1;
     info->m_J1linearAxis[2*info->rowskip+2] = 1;
-       btVector3 a1 = body0_trans.getBasis() * m_rbAFrame.getOrigin();
+       btVector3 a1 = transA.getBasis() * m_rbAFrame.getOrigin();
        {
                btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
                btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
        {
                btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
                btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
@@ -124,7 +137,10 @@ void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
                btVector3 a1neg = -a1;
                a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
        }
                btVector3 a1neg = -a1;
                a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
        }
-       btVector3 a2 = body1_trans.getBasis() * m_rbBFrame.getOrigin();
+    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);
                btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
        {
                btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
                btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
@@ -132,13 +148,18 @@ void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
                a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
        }
     // set right hand side
                a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
        }
     // set right hand side
-    btScalar k = info->fps * info->erp;
+       btScalar linERP = (m_flags & BT_CONETWIST_FLAGS_LIN_ERP) ? m_linERP : info->erp;
+    btScalar k = info->fps * linERP;
     int j;
        for (j=0; j<3; j++)
     {
     int j;
        for (j=0; j<3; j++)
     {
-        info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]);
+        info->m_constraintError[j*info->rowskip] = k * (a2[j] + transB.getOrigin()[j] - a1[j] - transA.getOrigin()[j]);
                info->m_lowerLimit[j*info->rowskip] = -SIMD_INFINITY;
                info->m_upperLimit[j*info->rowskip] = SIMD_INFINITY;
                info->m_lowerLimit[j*info->rowskip] = -SIMD_INFINITY;
                info->m_upperLimit[j*info->rowskip] = SIMD_INFINITY;
+               if(m_flags & BT_CONETWIST_FLAGS_LIN_CFM)
+               {
+                       info->cfm[j*info->rowskip] = m_linCFM;
+               }
     }
        int row = 3;
     int srow = row * info->rowskip;
     }
        int row = 3;
     int srow = row * info->rowskip;
@@ -150,7 +171,7 @@ void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
                btScalar *J2 = info->m_J2angularAxis;
                if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
                {
                btScalar *J2 = info->m_J2angularAxis;
                if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
                {
-                       btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame;
+                       btTransform trA = transA*m_rbAFrame;
                        btVector3 p = trA.getBasis().getColumn(1);
                        btVector3 q = trA.getBasis().getColumn(2);
                        int srow1 = srow + info->rowskip;
                        btVector3 p = trA.getBasis().getColumn(1);
                        btVector3 q = trA.getBasis().getColumn(2);
                        int srow1 = srow + info->rowskip;
@@ -187,7 +208,10 @@ void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
                        btScalar k = info->fps * m_biasFactor;
 
                        info->m_constraintError[srow] = k * m_swingCorrection;
                        btScalar k = info->fps * m_biasFactor;
 
                        info->m_constraintError[srow] = k * m_swingCorrection;
-                       info->cfm[srow] = 0.0f;
+                       if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
+                       {
+                               info->cfm[srow] = m_angCFM;
+                       }
                        // m_swingCorrection is always positive or 0
                        info->m_lowerLimit[srow] = 0;
                        info->m_upperLimit[srow] = SIMD_INFINITY;
                        // m_swingCorrection is always positive or 0
                        info->m_lowerLimit[srow] = 0;
                        info->m_upperLimit[srow] = SIMD_INFINITY;
@@ -207,7 +231,10 @@ void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
                J2[srow+2] = -ax1[2];
                btScalar k = info->fps * m_biasFactor;
                info->m_constraintError[srow] = k * m_twistCorrection;
                J2[srow+2] = -ax1[2];
                btScalar k = info->fps * m_biasFactor;
                info->m_constraintError[srow] = k * m_twistCorrection;
-               info->cfm[srow] = 0.0f;
+               if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
+               {
+                       info->cfm[srow] = m_angCFM;
+               }
                if(m_twistSpan > 0.0f)
                {
 
                if(m_twistSpan > 0.0f)
                {
 
@@ -275,7 +302,7 @@ void        btConeTwistConstraint::buildJacobian()
                        }
                }
 
                        }
                }
 
-               calcAngleInfo2();
+               calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
        }
 }
 
        }
 }
 
@@ -283,6 +310,7 @@ void        btConeTwistConstraint::buildJacobian()
 
 void   btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
 {
 
 void   btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
 {
+       #ifndef __SPU__
        if (m_useSolveConstraintObsolete)
        {
                btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
        if (m_useSolveConstraintObsolete)
        {
                btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
@@ -297,9 +325,9 @@ void        btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
                        btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
 
                        btVector3 vel1;
                        btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
 
                        btVector3 vel1;
-                       bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
+                       bodyA.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1);
                        btVector3 vel2;
                        btVector3 vel2;
-                       bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
+                       bodyB.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2);
                        btVector3 vel = vel1 - vel2;
 
                        for (int i=0;i<3;i++)
                        btVector3 vel = vel1 - vel2;
 
                        for (int i=0;i<3;i++)
@@ -316,8 +344,8 @@ void        btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
                                
                                btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
                                btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
                                
                                btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
                                btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
-                               bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
-                               bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
+                               bodyA.internalApplyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
+                               bodyB.internalApplyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
                
                        }
                }
                
                        }
                }
@@ -328,8 +356,8 @@ void        btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
                        // compute current and predicted transforms
                        btTransform trACur = m_rbA.getCenterOfMassTransform();
                        btTransform trBCur = m_rbB.getCenterOfMassTransform();
                        // compute current and predicted transforms
                        btTransform trACur = m_rbA.getCenterOfMassTransform();
                        btTransform trBCur = m_rbB.getCenterOfMassTransform();
-                       btVector3 omegaA; bodyA.getAngularVelocity(omegaA);
-                       btVector3 omegaB; bodyB.getAngularVelocity(omegaB);
+                       btVector3 omegaA; bodyA.internalGetAngularVelocity(omegaA);
+                       btVector3 omegaB; bodyB.internalGetAngularVelocity(omegaB);
                        btTransform trAPred; trAPred.setIdentity(); 
                        btVector3 zerovec(0,0,0);
                        btTransformUtil::integrateTransform(
                        btTransform trAPred; trAPred.setIdentity(); 
                        btVector3 zerovec(0,0,0);
                        btTransformUtil::integrateTransform(
@@ -403,15 +431,15 @@ void      btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
                                btScalar  impulseMag  = impulse.length();
                                btVector3 impulseAxis =  impulse / impulseMag;
 
                                btScalar  impulseMag  = impulse.length();
                                btVector3 impulseAxis =  impulse / impulseMag;
 
-                               bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
-                               bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
+                               bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
+                               bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
 
                        }
                }
                else if (m_damping > SIMD_EPSILON) // no motor: do a little damping
                {
 
                        }
                }
                else if (m_damping > SIMD_EPSILON) // no motor: do a little damping
                {
-                       btVector3 angVelA; bodyA.getAngularVelocity(angVelA);
-                       btVector3 angVelB; bodyB.getAngularVelocity(angVelB);
+                       btVector3 angVelA; bodyA.internalGetAngularVelocity(angVelA);
+                       btVector3 angVelB; bodyB.internalGetAngularVelocity(angVelB);
                        btVector3 relVel = angVelB - angVelA;
                        if (relVel.length2() > SIMD_EPSILON)
                        {
                        btVector3 relVel = angVelB - angVelA;
                        if (relVel.length2() > SIMD_EPSILON)
                        {
@@ -423,8 +451,8 @@ void        btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
 
                                btScalar  impulseMag  = impulse.length();
                                btVector3 impulseAxis = impulse / impulseMag;
 
                                btScalar  impulseMag  = impulse.length();
                                btVector3 impulseAxis = impulse / impulseMag;
-                               bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
-                               bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
+                               bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
+                               bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
                        }
                }
 
                        }
                }
 
@@ -432,9 +460,9 @@ void        btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
                {
                        ///solve angular part
                        btVector3 angVelA;
                {
                        ///solve angular part
                        btVector3 angVelA;
-                       bodyA.getAngularVelocity(angVelA);
+                       bodyA.internalGetAngularVelocity(angVelA);
                        btVector3 angVelB;
                        btVector3 angVelB;
-                       bodyB.getAngularVelocity(angVelB);
+                       bodyB.internalGetAngularVelocity(angVelB);
 
                        // solve swing limit
                        if (m_solveSwingLimit)
 
                        // solve swing limit
                        if (m_solveSwingLimit)
@@ -463,8 +491,8 @@ void        btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
                                impulseMag = impulse.length();
                                btVector3 noTwistSwingAxis = impulse / impulseMag;
 
                                impulseMag = impulse.length();
                                btVector3 noTwistSwingAxis = impulse / impulseMag;
 
-                               bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag);
-                               bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag);
+                               bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag);
+                               bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag);
                        }
 
 
                        }
 
 
@@ -482,18 +510,21 @@ void      btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolver
                                m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) );
                                impulseMag = m_accTwistLimitImpulse - temp;
 
                                m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) );
                                impulseMag = m_accTwistLimitImpulse - temp;
 
-                               btVector3 impulse = m_twistAxis * impulseMag;
+               //              btVector3 impulse = m_twistAxis * impulseMag;
 
 
-                               bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag);
-                               bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*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);
                        }               
                }
        }
                        }               
                }
        }
-
+#else
+btAssert(0);
+#endif //__SPU__
 }
 
 
 
 }
 
 
 
+
 void   btConeTwistConstraint::updateRHS(btScalar       timeStep)
 {
        (void)timeStep;
 void   btConeTwistConstraint::updateRHS(btScalar       timeStep)
 {
        (void)timeStep;
@@ -501,7 +532,7 @@ void        btConeTwistConstraint::updateRHS(btScalar       timeStep)
 }
 
 
 }
 
 
-
+#ifndef __SPU__
 void btConeTwistConstraint::calcAngleInfo()
 {
        m_swingCorrection = btScalar(0.);
 void btConeTwistConstraint::calcAngleInfo()
 {
        m_swingCorrection = btScalar(0.);
@@ -587,13 +618,13 @@ void btConeTwistConstraint::calcAngleInfo()
                }
        }
 }
                }
        }
 }
-
+#endif //__SPU__
 
 static btVector3 vTwist(1,0,0); // twist axis in constraint's space
 
 
 
 
 static btVector3 vTwist(1,0,0); // twist axis in constraint's space
 
 
 
-void btConeTwistConstraint::calcAngleInfo2()
+void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB)
 {
        m_swingCorrection = btScalar(0.);
        m_twistLimitSign = btScalar(0.);
 {
        m_swingCorrection = btScalar(0.);
        m_twistLimitSign = btScalar(0.);
@@ -606,11 +637,16 @@ void btConeTwistConstraint::calcAngleInfo2()
                // TODO : split rotation to pure swing and pure twist
                // compute desired transforms in world
                btTransform trPose(m_qTarget);
                // TODO : split rotation to pure swing and pure twist
                // compute desired transforms in world
                btTransform trPose(m_qTarget);
-               btTransform trA = getRigidBodyA().getCenterOfMassTransform() * m_rbAFrame;
-               btTransform trB = getRigidBodyB().getCenterOfMassTransform() * m_rbBFrame;
+               btTransform trA = transA * m_rbAFrame;
+               btTransform trB = transB * m_rbBFrame;
                btTransform trDeltaAB = trB * trPose * trA.inverse();
                btQuaternion qDeltaAB = trDeltaAB.getRotation();
                btVector3 swingAxis =   btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z());
                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();
                m_swingAxis = swingAxis;
                m_swingAxis.normalize();
                m_swingCorrection = qDeltaAB.getAngle();
@@ -624,8 +660,8 @@ void btConeTwistConstraint::calcAngleInfo2()
 
        {
                // compute rotation of A wrt B (in constraint space)
 
        {
                // compute rotation of A wrt B (in constraint space)
-               btQuaternion qA = getRigidBodyA().getCenterOfMassTransform().getRotation() * m_rbAFrame.getRotation();
-               btQuaternion qB = getRigidBodyB().getCenterOfMassTransform().getRotation() * m_rbBFrame.getRotation();
+               btQuaternion qA = transA.getRotation() * m_rbAFrame.getRotation();
+               btQuaternion qB = transB.getRotation() * m_rbBFrame.getRotation();
                btQuaternion qAB = qB.inverse() * qA;
                // split rotation into cone and twist
                // (all this is done from B's perspective. Maybe I should be averaging axes...)
                btQuaternion qAB = qB.inverse() * qA;
                // split rotation into cone and twist
                // (all this is done from B's perspective. Maybe I should be averaging axes...)
@@ -664,8 +700,8 @@ void btConeTwistConstraint::calcAngleInfo2()
                                m_twistAxisA.setValue(0,0,0);
 
                                m_kSwing =  btScalar(1.) /
                                m_twistAxisA.setValue(0,0,0);
 
                                m_kSwing =  btScalar(1.) /
-                                       (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) +
-                                        getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis));
+                                       (computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldA) +
+                                        computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldB));
                        }
                }
                else
                        }
                }
                else
@@ -673,10 +709,10 @@ void btConeTwistConstraint::calcAngleInfo2()
                        // you haven't set any limits;
                        // or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?)
                        // anyway, we have either hinge or fixed joint
                        // you haven't set any limits;
                        // or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?)
                        // anyway, we have either hinge or fixed joint
-                       btVector3 ivA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0);
-                       btVector3 jvA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1);
-                       btVector3 kvA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
-                       btVector3 ivB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(0);
+                       btVector3 ivA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
+                       btVector3 jvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
+                       btVector3 kvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(2);
+                       btVector3 ivB = transB.getBasis() * m_rbBFrame.getBasis().getColumn(0);
                        btVector3 target;
                        btScalar x = ivB.dot(ivA);
                        btScalar y = ivB.dot(jvA);
                        btVector3 target;
                        btScalar x = ivB.dot(ivA);
                        btScalar y = ivB.dot(jvA);
@@ -693,7 +729,8 @@ void btConeTwistConstraint::calcAngleInfo2()
                        {
                                if(m_swingSpan1 < m_fixThresh)
                                { // hinge around Y axis
                        {
                                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)
                                        {
                                                m_solveSwingLimit = true;
                                                if(m_swingSpan2 >= m_fixThresh)
@@ -715,7 +752,8 @@ void btConeTwistConstraint::calcAngleInfo2()
                                }
                                else
                                { // hinge around Z axis
                                }
                                else
                                { // hinge around Z axis
-                                       if(!btFuzzyZero(z))
+//                                     if(!btFuzzyZero(z))
+                                       if((!(btFuzzyZero(x))) || (!(btFuzzyZero(y))))
                                        {
                                                m_solveSwingLimit = true;
                                                if(m_swingSpan1 >= m_fixThresh)
                                        {
                                                m_solveSwingLimit = true;
                                                if(m_swingSpan1 >= m_fixThresh)
@@ -767,8 +805,8 @@ void btConeTwistConstraint::calcAngleInfo2()
                                m_twistAxis = quatRotate(qB, -twistAxis);
 
                                m_kTwist = btScalar(1.) /
                                m_twistAxis = quatRotate(qB, -twistAxis);
 
                                m_kTwist = btScalar(1.) /
-                                       (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
-                                        getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
+                                       (computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldA) +
+                                        computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldB));
                        }
 
                        if (m_solveSwingLimit)
                        }
 
                        if (m_solveSwingLimit)
@@ -796,12 +834,11 @@ void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
        {
                vSwingAxis = btVector3(qCone.x(), qCone.y(), qCone.z());
                vSwingAxis.normalize();
        {
                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.)
                // 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.)
@@ -845,8 +882,10 @@ void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
        else if (swingAngle < 0)
        {
                // this should never happen!
        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
 }
 
 btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const
@@ -891,13 +930,15 @@ void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist,
 
        if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
        {
 
        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
                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());
        }
 
        vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
@@ -944,10 +985,10 @@ void btConeTwistConstraint::setMotorTarget(const btQuaternion &q)
 {
        btTransform trACur = m_rbA.getCenterOfMassTransform();
        btTransform trBCur = m_rbB.getCenterOfMassTransform();
 {
        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);
 
        btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * q * m_rbAFrame.getRotation();
        setMotorTargetInConstraintSpace(qConstraint);
@@ -1004,6 +1045,97 @@ void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &
        }
 }
 
        }
 }
 
+///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.
+void btConeTwistConstraint::setParam(int num, btScalar value, int axis)
+{
+       switch(num)
+       {
+               case BT_CONSTRAINT_ERP :
+               case BT_CONSTRAINT_STOP_ERP :
+                       if((axis >= 0) && (axis < 3)) 
+                       {
+                               m_linERP = value;
+                               m_flags |= BT_CONETWIST_FLAGS_LIN_ERP;
+                       }
+                       else
+                       {
+                               m_biasFactor = value;
+                       }
+                       break;
+               case BT_CONSTRAINT_CFM :
+               case BT_CONSTRAINT_STOP_CFM :
+                       if((axis >= 0) && (axis < 3)) 
+                       {
+                               m_linCFM = value;
+                               m_flags |= BT_CONETWIST_FLAGS_LIN_CFM;
+                       }
+                       else
+                       {
+                               m_angCFM = value;
+                               m_flags |= BT_CONETWIST_FLAGS_ANG_CFM;
+                       }
+                       break;
+               default:
+                       btAssertConstrParams(0);
+                       break;
+       }
+}
+
+///return the local value of parameter
+btScalar btConeTwistConstraint::getParam(int num, int axis) const 
+{
+       btScalar retVal = 0;
+       switch(num)
+       {
+               case BT_CONSTRAINT_ERP :
+               case BT_CONSTRAINT_STOP_ERP :
+                       if((axis >= 0) && (axis < 3)) 
+                       {
+                               btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_ERP);
+                               retVal = m_linERP;
+                       }
+                       else if((axis >= 3) && (axis < 6)) 
+                       {
+                               retVal = m_biasFactor;
+                       }
+                       else
+                       {
+                               btAssertConstrParams(0);
+                       }
+                       break;
+               case BT_CONSTRAINT_CFM :
+               case BT_CONSTRAINT_STOP_CFM :
+                       if((axis >= 0) && (axis < 3)) 
+                       {
+                               btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_CFM);
+                               retVal = m_linCFM;
+                       }
+                       else if((axis >= 3) && (axis < 6)) 
+                       {
+                               btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_ANG_CFM);
+                               retVal = m_angCFM;
+                       }
+                       else
+                       {
+                               btAssertConstrParams(0);
+                       }
+                       break;
+               default : 
+                       btAssertConstrParams(0);
+       }
+       return retVal;
+}
+
+
+void btConeTwistConstraint::setFrames(const btTransform & frameA, const btTransform & frameB)
+{
+       m_rbAFrame = frameA;
+       m_rbBFrame = frameB;
+       buildJacobian();
+       //calculateTransforms();
+}