expose maximum applied impulse for btMultiBody, introduced to reduce/avoid 'exploding' simulations

use btScalar instead of float, to enable double precision build, fix for Issue 744

Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp

@@ -323,11 +323,6 @@ btMultiBody* FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMult
 			{
 	//			btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(bod,n_links-1,2,3);
 			
-				if (0)
-				{
-					btMultiBodyJointMotor* con = new btMultiBodyJointMotor(bod,i,1,500000); 
-					world->addMultiBodyConstraint(con);
-				}
 				if (createConstraints)
 				{	
 					btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(bod,i,-1,1);
@@ -338,6 +333,12 @@ btMultiBody* FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMult
 			{
 				if (createConstraints)
 				{	
+					if (1)
+					{
+						btMultiBodyJointMotor* con = new btMultiBodyJointMotor(bod,i,0,500000); 
+						world->addMultiBodyConstraint(con);
+					}
+
 					btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(bod,i,-1,1);
 					world->addMultiBodyConstraint(con);
 				}

Demos/OpenGL/DemoApplication.cpp

@@ -597,7 +597,7 @@ void	DemoApplication::shootBox(const btVector3& destination)
 int gPickingConstraintId = 0;
 btVector3 gOldPickingPos;
 btVector3 gHitPos(-1,-1,-1);
-float gOldPickingDist  = 0.f;
+btScalar gOldPickingDist  = 0.f;
 btRigidBody* pickedBody = 0;//for deactivation state
 
 

src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -92,8 +92,8 @@ btMultiBody::btMultiBody(int n_links,
 		m_baseCollider(0),
 		m_linearDamping(0.04f),
 		m_angularDamping(0.04f),
-		m_useGyroTerm(true)
+		m_useGyroTerm(true),
-
+		m_maxAppliedImpulse(1000.f)
 {
 	 links.resize(n_links);
 

src/BulletDynamics/Featherstone/btMultiBody.h

@@ -280,16 +280,18 @@ public:
 			sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier;
 		}
 		btScalar l = btSqrt(sum);
+		/*
 		static btScalar maxl = -1e30f;
 		if (l>maxl)
 		{
 			maxl=l;
-			printf("maxl=%f\n",maxl);
+	//		printf("maxl=%f\n",maxl);
 		}
-		if (l>100)
+		*/
+		if (l>m_maxAppliedImpulse)
 		{
-			printf("exceeds 100: l=%f\n",maxl);
+//			printf("exceeds 100: l=%f\n",maxl);
-			multiplier *= 100.f/l;
+			multiplier *= m_maxAppliedImpulse/l;
 		}
 
         for (int i = 0; i < 6 + getNumLinks(); ++i)
@@ -373,6 +375,14 @@ public:
 	{
 		m_useGyroTerm = useGyro;
 	}
+	btScalar	getMaxAppliedImpulse() const
+	{
+		return m_maxAppliedImpulse;
+	}
+	void	setMaxAppliedImpulse(btScalar maxImp)
+	{
+		m_maxAppliedImpulse = maxImp;
+	}
 
 private:
     btMultiBody(const btMultiBody &);  // not implemented
@@ -437,7 +447,7 @@ private:
 	btScalar	m_linearDamping;
 	btScalar	m_angularDamping;
 	bool	m_useGyroTerm;
-
+	btScalar	m_maxAppliedImpulse;
 };
 
 #endif

src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp

@@ -62,7 +62,7 @@ btScalar btMultiBodyConstraint::fillConstraintRowMultiBodyMultiBody(btMultiBodyS
 		for (int i=0;i<ndofA;i++)
 			data.m_jacobians[constraintRow.m_jacAindex+i] = jacOrgA[i];
 		
-		float* delta = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
+		btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
 		multiBodyA->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacAindex],delta,data.scratch_r, data.scratch_v);
 	} 
 
@@ -105,8 +105,8 @@ btScalar btMultiBodyConstraint::fillConstraintRowMultiBodyMultiBody(btMultiBodyS
 			lambdaA = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
 			for (int i = 0; i < ndofA; ++i)
 			{
-				float j = jacA[i] ;
+				btScalar j = jacA[i] ;
-				float l =lambdaA[i];
+				btScalar l =lambdaA[i];
 				denom0 += j*l;
 			}
 		} 
@@ -117,8 +117,8 @@ btScalar btMultiBodyConstraint::fillConstraintRowMultiBodyMultiBody(btMultiBodyS
 			lambdaB = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex];
 			for (int i = 0; i < ndofB; ++i)
 			{
-				float j = jacB[i] ;
+				btScalar j = jacB[i] ;
-				float l =lambdaB[i];
+				btScalar l =lambdaB[i];
 				denom1 += j*l;
 			}
 
@@ -134,7 +134,7 @@ btScalar btMultiBodyConstraint::fillConstraintRowMultiBodyMultiBody(btMultiBodyS
             }
         }
 
-		 float d = denom0+denom1;
+		 btScalar d = denom0+denom1;
 		 if (btFabs(d)>SIMD_EPSILON)
 		 {
 			 
@@ -275,9 +275,9 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
 		data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
 		btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
 
-		float* jac1=&data.m_jacobians[solverConstraint.m_jacAindex];
+		btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex];
 		multiBodyA->fillContactJacobian(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
-		float* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+		btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
 		multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
 	} else
 	{
@@ -332,8 +332,8 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
 			lambdaA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
 			for (int i = 0; i < ndofA; ++i)
 			{
-				float j = jacA[i] ;
+				btScalar j = jacA[i] ;
-				float l =lambdaA[i];
+				btScalar l =lambdaA[i];
 				denom0 += j*l;
 			}
 		} else
@@ -351,8 +351,8 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
 			lambdaB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
 			for (int i = 0; i < ndofB; ++i)
 			{
-				float j = jacB[i] ;
+				btScalar j = jacB[i] ;
-				float l =lambdaB[i];
+				btScalar l =lambdaB[i];
 				denom1 += j*l;
 			}
 
@@ -375,7 +375,7 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
             }
         }
 
-		 float d = denom0+denom1;
+		 btScalar d = denom0+denom1;
 		 if (btFabs(d)>SIMD_EPSILON)
 		 {
 			 

src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp

@@ -276,9 +276,9 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
 		btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
 
-		float* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex];
+		btScalar* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex];
 		multiBodyA->fillContactJacobian(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
-		float* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+		btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
 		multiBodyA->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
 	} else
 	{
@@ -333,8 +333,8 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 			lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
 			for (int i = 0; i < ndofA; ++i)
 			{
-				float j = jacA[i] ;
+				btScalar j = jacA[i] ;
-				float l =lambdaA[i];
+				btScalar l =lambdaA[i];
 				denom0 += j*l;
 			}
 		} else
@@ -352,8 +352,8 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 			lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
 			for (int i = 0; i < ndofB; ++i)
 			{
-				float j = jacB[i] ;
+				btScalar j = jacB[i] ;
-				float l =lambdaB[i];
+				btScalar l =lambdaB[i];
 				denom1 += j*l;
 			}
 
@@ -376,7 +376,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
             }
         }
 
-		 float d = denom0+denom1;
+		 btScalar d = denom0+denom1;
 		 if (btFabs(d)>SIMD_EPSILON)
 		 {