1 files changed, 26 insertions, 17 deletions

diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
index 755544f0dee..15a4c92de20 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp
@@ -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);
@@ -725,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)
@@ -747,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)
@@ -828,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.)
@@ -877,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
@@ -929,7 +936,9 @@ void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist,
 	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());
@@ -976,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);