getInfo2NonVirtual() merged to getInfo2NonVirtualUsingFrameOffset() to reduce code size

src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp

@@ -217,16 +217,17 @@ void btSliderConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
 
 void btSliderConstraint::getInfo2(btConstraintInfo2* info)
 {
-	if(m_useOffsetForConstraintFrame)
+//	if(m_useOffsetForConstraintFrame)
-	{
+//	{
 		getInfo2NonVirtualUsingFrameOffset(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(),m_rbB.getLinearVelocity(), m_rbA.getInvMass(),m_rbB.getInvMass());
-	}
+//	}
-	else
+//	else
-	{
+//	{
-		getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(),m_rbB.getLinearVelocity(), m_rbA.getInvMass(),m_rbB.getInvMass());
+//		getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(),m_rbB.getLinearVelocity(), m_rbA.getInvMass(),m_rbB.getInvMass());
-	}
+//	}
 }
 
+#if 0 // replaced with getInfo2NonVirtualUsingFrameOffset
 void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB, const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass  )
 {
 	//prepare constraint
@@ -403,7 +404,7 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
 		{
 			if(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN)
 			{
-				info->cfm[nrow] = m_cfmDirLin;
+				info->cfm[srow] = m_cfmDirLin;
 			}
 			btScalar tag_vel = getTargetLinMotorVelocity();
 			btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * currERP);
@@ -520,7 +521,7 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
 			info->m_constraintError[srow] += k * limit_err;
 			if(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG)
 			{
-				info->cfm[nrow] = m_cfmLimAng;
+				info->cfm[srow] = m_cfmLimAng;
 			}
 			if(lostop == histop) 
 			{
@@ -573,6 +574,7 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra
 		} // if(limit)
 	} // if angular limit or powered
 }
+#endif
 
 
 void btSliderConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
@@ -919,15 +921,25 @@ void btSliderConstraint::getInfo2NonVirtualUsingFrameOffset(btConstraintInfo2* i
 		factA = btScalar(0.5f);
 	}
 	factB = btScalar(1.0f) - factA;
-	// get the desired direction of slider axis
+	btVector3 ax1, p, q;
-	// as weighted sum of X-orthos of frameA and frameB in WCS
 	btVector3 ax1A = trA.getBasis().getColumn(0);
 	btVector3 ax1B = trB.getBasis().getColumn(0);
-	btVector3 ax1 = ax1A * factA + ax1B * factB;
+	if(m_useOffsetForConstraintFrame)
+	{
+		// get the desired direction of slider axis
+		// as weighted sum of X-orthos of frameA and frameB in WCS
+		ax1 = ax1A * factA + ax1B * factB;
 		ax1.normalize();
 		// construct two orthos to slider axis
-	btVector3 p, q;
 		btPlaneSpace1 (ax1, p, q);
+	}
+	else
+	{ // old way - use frameA
+		ax1 = trA.getBasis().getColumn(0);
+		// get 2 orthos to slider axis (Y, Z)
+		p = trA.getBasis().getColumn(1);
+		q = trA.getBasis().getColumn(2);
+	}
 	// make rotations around these orthos equal
 	// the slider axis should be the only unconstrained
 	// rotational axis, the angular velocity of the two bodies perpendicular to
@@ -993,7 +1005,9 @@ void btSliderConstraint::getInfo2NonVirtualUsingFrameOffset(btConstraintInfo2* i
 	int s2 = nrow * s;
 	nrow++;
 	int s3 = nrow * s;
-	btVector3 tmpA, tmpB, relA, relB;
+	btVector3 tmpA, tmpB, relA, relB, c;
+	if(m_useOffsetForConstraintFrame)
+	{
 		// get vector from bodyB to frameB in WCS
 		relB = trB.getOrigin() - bodyB_trans.getOrigin();
 		// get its projection to slider axis
@@ -1041,6 +1055,21 @@ void btSliderConstraint::getInfo2NonVirtualUsingFrameOffset(btConstraintInfo2* i
 		for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = -tmpB[i];
 		for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
 		for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
+	}
+	else
+	{	// old way - maybe incorrect if bodies are not on the slider axis
+		// see discussion "Bug in slider constraint" http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=9&t=4024&start=0
+		c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin();
+		btVector3 tmp = c.cross(p);
+		for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = factA*tmp[i];
+		for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = factB*tmp[i];
+		tmp = c.cross(q);
+		for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = factA*tmp[i];
+		for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = factB*tmp[i];
+
+		for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
+		for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
+	}
 	// compute two elements of right hand side
 
 	//	k = info->fps * info->erp * getSoftnessOrthoLin();
@@ -1086,6 +1115,8 @@ void btSliderConstraint::getInfo2NonVirtualUsingFrameOffset(btConstraintInfo2* i
 		// constraint force is applied at must lie along the same ax1 axis.
 		// a torque couple will result in limited slider-jointed free
 		// bodies from gaining angular momentum.
+		if(m_useOffsetForConstraintFrame)
+		{
 			// this is needed only when bodyA and bodyB are both dynamic.
 			if(!hasStaticBody)
 			{
@@ -1098,6 +1129,18 @@ void btSliderConstraint::getInfo2NonVirtualUsingFrameOffset(btConstraintInfo2* i
 				info->m_J2angularAxis[srow+1] = -tmpB[1];
 				info->m_J2angularAxis[srow+2] = -tmpB[2];
 			}
+		}
+		else
+		{ // The old way. May be incorrect if bodies are not on the slider axis
+			btVector3 ltd;	// Linear Torque Decoupling vector (a torque)
+			ltd = c.cross(ax1);
+			info->m_J1angularAxis[srow+0] = factA*ltd[0];
+			info->m_J1angularAxis[srow+1] = factA*ltd[1];
+			info->m_J1angularAxis[srow+2] = factA*ltd[2];
+			info->m_J2angularAxis[srow+0] = factB*ltd[0];
+			info->m_J2angularAxis[srow+1] = factB*ltd[1];
+			info->m_J2angularAxis[srow+2] = factB*ltd[2];
+		}
 		// right-hand part
 		btScalar lostop = getLowerLinLimit();
 		btScalar histop = getUpperLinLimit();
@@ -1113,7 +1156,7 @@ void btSliderConstraint::getInfo2NonVirtualUsingFrameOffset(btConstraintInfo2* i
 		{
 			if(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN)
 			{
-				info->cfm[nrow] = m_cfmDirLin;
+				info->cfm[srow] = m_cfmDirLin;
 			}
 			btScalar tag_vel = getTargetLinMotorVelocity();
 			btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * currERP);
@@ -1229,7 +1272,7 @@ void btSliderConstraint::getInfo2NonVirtualUsingFrameOffset(btConstraintInfo2* i
 			info->m_constraintError[srow] += k * limit_err;
 			if(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG)
 			{
-				info->cfm[nrow] = m_cfmLimAng;
+				info->cfm[srow] = m_cfmLimAng;
 			}
 			if(lostop == histop) 
 			{