Add a virtual createConstraintRows method, to easier experiment with different kinds of btMultiBodyConstraint

Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp

@@ -209,21 +209,22 @@ void	FeatherstoneMultiBodyDemo::initPhysics()
 	}
 
 	
-	createFeatherstoneMultiBody(world, 5, btVector3 (20,29.5,-2), true, true);
+	btMultiBody* mb = createFeatherstoneMultiBody(world, 1, btVector3 (20,29.5,-2), true, true,true);
 	
-	createFeatherstoneMultiBody(world, 5, btVector3 (0,29.5,-2), false,false);
+	
+	mb = createFeatherstoneMultiBody(world, 5, btVector3 (0,29.5,-2), false,false,true);
 	
 	
 
 }
 
-void FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition,bool isFixedBase, bool usePrismatic)
+btMultiBody* FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition,bool isFixedBase, bool usePrismatic, bool canSleep)
 {
-	{
+	
 	int n_links = numLinks;
 	float mass = 13.5;
 	btVector3 inertia(91,344,253);
-		bool canSleep = true;//false;
+	
 	
 	btMultiBody * bod = new btMultiBody(n_links, mass, inertia, isFixedBase, canSleep);
 		
@@ -357,8 +358,8 @@ void FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMultiBodyDyn
 
 	}
 	world->addMultiBody(bod);
-	}
 
+	return bod;
 }
 
 

Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.h

@@ -28,6 +28,7 @@ subject to the following restrictions:
 
 #include "LinearMath/btAlignedObjectArray.h"
 
+class btMultiBody;
 class btBroadphaseInterface;
 class btCollisionShape;
 class btOverlappingPairCache;
@@ -53,7 +54,7 @@ class FeatherstoneMultiBodyDemo : public PlatformDemoApplication
 	btDefaultCollisionConfiguration* m_collisionConfiguration;
 
 	
-	void createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition,bool isFixedBase, bool usePrismatic);
+	btMultiBody* createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition,bool isFixedBase, bool usePrismatic, bool canSleep);
 
 	public:
 

src/BulletDynamics/Featherstone/btMultiBodyConstraint.h

@@ -21,6 +21,20 @@ subject to the following restrictions:
 #include "btMultiBody.h"
 
 class btMultiBody;
+struct btSolverInfo;
+
+#include "btMultiBodySolverConstraint.h"
+
+struct btMultiBodyJacobianData
+{
+	btAlignedObjectArray<btScalar>		m_jacobians;
+	btAlignedObjectArray<btScalar>		m_deltaVelocitiesUnitImpulse;
+	btAlignedObjectArray<btScalar>		m_deltaVelocities;
+	btAlignedObjectArray<btScalar>		scratch_r;
+	btAlignedObjectArray<btVector3>		scratch_v;
+	btAlignedObjectArray<btMatrix3x3>	scratch_m;
+};
+
 
 class btMultiBodyConstraint
 {
@@ -62,7 +76,9 @@ public:
 	virtual int getIslandIdA() const =0;
 	virtual int getIslandIdB() const =0;
 	
-	virtual void update()=0;
+	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+		btMultiBodyJacobianData& data,
+		const btContactSolverInfo& infoGlobal)=0;
 
 	int	getNumRows() const
 	{

src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp

@@ -19,6 +19,7 @@ subject to the following restrictions:
 
 #include "BulletDynamics/ConstraintSolver/btSolverBody.h"
 #include "btMultiBodyConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
 
 #include "LinearMath/btQuickprof.h"
 
@@ -33,7 +34,8 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
 	{
 		btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[j];
 		//if (iteration < constraint.m_overrideNumSolverIterations)
-			resolveSingleConstraintRowGenericMultiBody(constraint);
+			//resolveSingleConstraintRowGenericMultiBody(constraint);
+		resolveSingleConstraintRowGeneric(constraint);
 	}
 
 	//solve featherstone normal contact
@@ -69,9 +71,9 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionOb
 	m_multiBodyNonContactConstraints.resize(0);
 	m_multiBodyNormalContactConstraints.resize(0);
 	m_multiBodyFrictionContactConstraints.resize(0);
-	m_jacobians.resize(0);
+	m_data.m_jacobians.resize(0);
-	m_deltaVelocitiesUnitImpulse.resize(0);
+	m_data.m_deltaVelocitiesUnitImpulse.resize(0);
-	m_deltaVelocities.resize(0);
+	m_data.m_deltaVelocities.resize(0);
 
 	for (int i=0;i<numBodies;i++)
 	{
@@ -90,7 +92,7 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionOb
 void	btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
 {
     for (int i = 0; i < ndof; ++i) 
-		m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
+		m_data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
 }
 
 void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c)
@@ -108,7 +110,7 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
 	{
 		ndofA  = c.m_multiBodyA->getNumLinks() + 6;
 		for (int i = 0; i < ndofA; ++i) 
-			deltaVelADotn += m_jacobians[c.m_jacAindex+i] * m_deltaVelocities[c.m_deltaVelAindex+i];
+			deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i];
 	} else
 	{
 		bodyA = &m_tmpSolverBodyPool[c.m_solverBodyIdA];
@@ -119,7 +121,7 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
 	{
 		ndofB = c.m_multiBodyB->getNumLinks() + 6;
 		for (int i = 0; i < ndofB; ++i) 
-			deltaVelBDotn += m_jacobians[c.m_jacBindex+i] * m_deltaVelocities[c.m_deltaVelBindex+i];
+			deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i];
 	} else
 	{
 		bodyB = &m_tmpSolverBodyPool[c.m_solverBodyIdB];
@@ -148,8 +150,8 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
 
 	if (c.m_multiBodyA)
 	{
-		applyDeltaVee(&m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA);
+		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA);
-		c.m_multiBodyA->applyDeltaVee(&m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
+		c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
 	} else
 	{
 		bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
@@ -157,8 +159,8 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
 	}
 	if (c.m_multiBodyB)
 	{
-		applyDeltaVee(&m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB);
+		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB);
-		c.m_multiBodyB->applyDeltaVee(&m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
+		c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
 	} else
 	{
 		bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
@@ -180,14 +182,14 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGenericMultiBody(con
 	{
 		ndofA  = c.m_multiBodyA->getNumLinks() + 6;
 		for (int i = 0; i < ndofA; ++i) 
-			deltaVelADotn += m_jacobians[c.m_jacAindex+i] * m_deltaVelocities[c.m_deltaVelAindex+i];
+			deltaVelADotn += m_data.m_jacobians[c.m_jacAindex+i] * m_data.m_deltaVelocities[c.m_deltaVelAindex+i];
 	}
 
 	if (c.m_multiBodyB)
 	{
 		ndofB = c.m_multiBodyB->getNumLinks() + 6;
 		for (int i = 0; i < ndofB; ++i) 
-			deltaVelBDotn += m_jacobians[c.m_jacBindex+i] * m_deltaVelocities[c.m_deltaVelBindex+i];
+			deltaVelBDotn += m_data.m_jacobians[c.m_jacBindex+i] * m_data.m_deltaVelocities[c.m_deltaVelBindex+i];
 	}
 
 	
@@ -212,13 +214,13 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGenericMultiBody(con
 
 	if (c.m_multiBodyA)
 	{
-		applyDeltaVee(&m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA);
+		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA);
-		c.m_multiBodyA->applyDeltaVee(&m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
+		c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
 	}
 	if (c.m_multiBodyB)
 	{
-		applyDeltaVee(&m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB);
+		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB);
-		c.m_multiBodyB->applyDeltaVee(&m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
+		c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
 	}
 }
 
@@ -261,23 +263,23 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 
 		if (solverConstraint.m_deltaVelAindex <0)
 		{
-			solverConstraint.m_deltaVelAindex = m_deltaVelocities.size();
+			solverConstraint.m_deltaVelAindex = m_data.m_deltaVelocities.size();
 			multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
-			m_deltaVelocities.resize(m_deltaVelocities.size()+ndofA);
+			m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofA);
 		} else
 		{
-			btAssert(m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
+			btAssert(m_data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
 		}
 
-		solverConstraint.m_jacAindex = m_jacobians.size();
+		solverConstraint.m_jacAindex = m_data.m_jacobians.size();
-		m_jacobians.resize(m_jacobians.size()+ndofA);
+		m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofA);
-		m_deltaVelocitiesUnitImpulse.resize(m_deltaVelocitiesUnitImpulse.size()+ndofA);
+		m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
-		btAssert(m_jacobians.size() == m_deltaVelocitiesUnitImpulse.size());
+		btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
 
-		float* jac1=&m_jacobians[solverConstraint.m_jacAindex];
+		float* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex];
-		multiBodyA->fillContactJacobian(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, scratch_r, scratch_v, scratch_m);
+		multiBodyA->fillContactJacobian(solverConstraint.m_linkA, cp.getPositionWorldOnA(), contactNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
-		float* delta = &m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+		float* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
-		multiBodyA->calcAccelerationDeltas(&m_jacobians[solverConstraint.m_jacAindex],delta,scratch_r, scratch_v);
+		multiBodyA->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
 	} else
 	{
 		btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
@@ -293,19 +295,19 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
 		if (solverConstraint.m_deltaVelBindex <0)
 		{
-			solverConstraint.m_deltaVelBindex = m_deltaVelocities.size();
+			solverConstraint.m_deltaVelBindex = m_data.m_deltaVelocities.size();
 			multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
-			m_deltaVelocities.resize(m_deltaVelocities.size()+ndofB);
+			m_data.m_deltaVelocities.resize(m_data.m_deltaVelocities.size()+ndofB);
 		}
 
-		solverConstraint.m_jacBindex = m_jacobians.size();
+		solverConstraint.m_jacBindex = m_data.m_jacobians.size();
 
-		m_jacobians.resize(m_jacobians.size()+ndofB);
+		m_data.m_jacobians.resize(m_data.m_jacobians.size()+ndofB);
-		m_deltaVelocitiesUnitImpulse.resize(m_deltaVelocitiesUnitImpulse.size()+ndofB);
+		m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
-		btAssert(m_jacobians.size() == m_deltaVelocitiesUnitImpulse.size());
+		btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
 
-		multiBodyB->fillContactJacobian(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_jacobians[solverConstraint.m_jacBindex], scratch_r, scratch_v, scratch_m);
+		multiBodyB->fillContactJacobian(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -contactNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
-		multiBodyB->calcAccelerationDeltas(&m_jacobians[solverConstraint.m_jacBindex],&m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],scratch_r, scratch_v);
+		multiBodyB->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
 	} else
 	{
 		btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);		
@@ -327,8 +329,8 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		if (multiBodyA)
 		{
 			ndofA  = multiBodyA->getNumLinks() + 6;
-			jacA = &m_jacobians[solverConstraint.m_jacAindex];
+			jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
-			lambdaA = &m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+			lambdaA = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
 			for (int i = 0; i < ndofA; ++i)
 			{
 				float j = jacA[i] ;
@@ -346,8 +348,8 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		if (multiBodyB)
 		{
 			const int ndofB  = multiBodyB->getNumLinks() + 6;
-			jacB = &m_jacobians[solverConstraint.m_jacBindex];
+			jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
-			lambdaB = &m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
+			lambdaB = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
 			for (int i = 0; i < ndofB; ++i)
 			{
 				float j = jacB[i] ;
@@ -403,7 +405,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		if (multiBodyA)
 		{
 			ndofA  = multiBodyA->getNumLinks() + 6;
-			btScalar* jacA = &m_jacobians[solverConstraint.m_jacAindex];
+			btScalar* jacA = &m_data.m_jacobians[solverConstraint.m_jacAindex];
 			for (int i = 0; i < ndofA ; ++i) 
 				rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
 		} else
@@ -416,7 +418,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		if (multiBodyB)
 		{
 			ndofB  = multiBodyB->getNumLinks() + 6;
-			btScalar* jacB = &m_jacobians[solverConstraint.m_jacBindex];
+			btScalar* jacB = &m_data.m_jacobians[solverConstraint.m_jacBindex];
 			for (int i = 0; i < ndofB ; ++i) 
 				rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
 
@@ -449,7 +451,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 			if (multiBodyA)
 			{
 				btScalar impulse = solverConstraint.m_appliedImpulse;
-				btScalar* deltaV = &m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+				btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
 				multiBodyA->applyDeltaVee(deltaV,impulse);
 				applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
 			} else
@@ -460,7 +462,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 			if (multiBodyB)
 			{
 				btScalar impulse = solverConstraint.m_appliedImpulse;
-				btScalar* deltaV = &m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
+				btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
 				multiBodyB->applyDeltaVee(deltaV,impulse);
 				applyDeltaVee(deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
 			} else
@@ -524,257 +526,6 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 
 
 
-void btMultiBodyConstraintSolver::setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint& constraintRow, 
-																 btScalar* jacOrgA,btScalar* jacOrgB,
-																 btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff,
-																 const btContactSolverInfo& infoGlobal)
-{
-			
-	BT_PROFILE("setupMultiBodyContactConstraint");
-	
-	btMultiBody* multiBodyA = constraintRow.m_multiBodyA;
-	btMultiBody* multiBodyB = constraintRow.m_multiBodyB;
-
-	if (multiBodyA)
-	{
-		
-		const int ndofA  = multiBodyA->getNumLinks() + 6;
-
-		constraintRow.m_deltaVelAindex = multiBodyA->getCompanionId();
-
-		if (constraintRow.m_deltaVelAindex <0)
-		{
-			constraintRow.m_deltaVelAindex = m_deltaVelocities.size();
-			multiBodyA->setCompanionId(constraintRow.m_deltaVelAindex);
-			m_deltaVelocities.resize(m_deltaVelocities.size()+ndofA);
-		} else
-		{
-			btAssert(m_deltaVelocities.size() >= constraintRow.m_deltaVelAindex+ndofA);
-		}
-
-		constraintRow.m_jacAindex = m_jacobians.size();
-		m_jacobians.resize(m_jacobians.size()+ndofA);
-		m_deltaVelocitiesUnitImpulse.resize(m_deltaVelocitiesUnitImpulse.size()+ndofA);
-		btAssert(m_jacobians.size() == m_deltaVelocitiesUnitImpulse.size());
-		for (int i=0;i<ndofA;i++)
-			m_jacobians[constraintRow.m_jacAindex+i] = jacOrgA[i];
-		
-		float* delta = &m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
-		multiBodyA->calcAccelerationDeltas(&m_jacobians[constraintRow.m_jacAindex],delta,scratch_r, scratch_v);
-	} 
-
-	if (multiBodyB)
-	{
-		const int ndofB  = multiBodyB->getNumLinks() + 6;
-
-		constraintRow.m_deltaVelBindex = multiBodyB->getCompanionId();
-		if (constraintRow.m_deltaVelBindex <0)
-		{
-			constraintRow.m_deltaVelBindex = m_deltaVelocities.size();
-			multiBodyB->setCompanionId(constraintRow.m_deltaVelBindex);
-			m_deltaVelocities.resize(m_deltaVelocities.size()+ndofB);
-		}
-
-		constraintRow.m_jacBindex = m_jacobians.size();
-		m_jacobians.resize(m_jacobians.size()+ndofB);
-
-		for (int i=0;i<ndofB;i++)
-			m_jacobians[constraintRow.m_jacBindex+i] = jacOrgB[i];
-
-		m_deltaVelocitiesUnitImpulse.resize(m_deltaVelocitiesUnitImpulse.size()+ndofB);
-		btAssert(m_jacobians.size() == m_deltaVelocitiesUnitImpulse.size());
-		multiBodyB->calcAccelerationDeltas(&m_jacobians[constraintRow.m_jacBindex],&m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex],scratch_r, scratch_v);
-	} 
-	{
-						
-		btVector3 vec;
-		btScalar denom0 = 0.f;
-		btScalar denom1 = 0.f;
-		btScalar* jacB = 0;
-		btScalar* jacA = 0;
-		btScalar* lambdaA =0;
-		btScalar* lambdaB =0;
-		int ndofA  = 0;
-		if (multiBodyA)
-		{
-			ndofA  = multiBodyA->getNumLinks() + 6;
-			jacA = &m_jacobians[constraintRow.m_jacAindex];
-			lambdaA = &m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
-			for (int i = 0; i < ndofA; ++i)
-			{
-				float j = jacA[i] ;
-				float l =lambdaA[i];
-				denom0 += j*l;
-			}
-		} 
-		if (multiBodyB)
-		{
-			const int ndofB  = multiBodyB->getNumLinks() + 6;
-			jacB = &m_jacobians[constraintRow.m_jacBindex];
-			lambdaB = &m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex];
-			for (int i = 0; i < ndofB; ++i)
-			{
-				float j = jacB[i] ;
-				float l =lambdaB[i];
-				denom1 += j*l;
-			}
-
-		} 
-
-		 if (multiBodyA && (multiBodyA==multiBodyB))
-		 {
-            // ndof1 == ndof2 in this case
-            for (int i = 0; i < ndofA; ++i) 
-			{
-                denom1 += jacB[i] * lambdaA[i];
-                denom1 += jacA[i] * lambdaB[i];
-            }
-        }
-
-		 float d = denom0+denom1;
-		 if (btFabs(d)>SIMD_EPSILON)
-		 {
-			 
-			 constraintRow.m_jacDiagABInv = 1.f/(d);
-		 } else
-		 {
-			constraintRow.m_jacDiagABInv  = 1.f;
-		 }
-		
-	}
-
-	
-	//compute rhs and remaining constraintRow fields
-
-	
-
-	btScalar restitution = 0.f;
-	
-
-	btScalar rel_vel = 0.f;
-	int ndofA  = 0;
-	int ndofB  = 0;
-	{
-
-		btVector3 vel1,vel2;
-		if (multiBodyA)
-		{
-			ndofA  = multiBodyA->getNumLinks() + 6;
-			btScalar* jacA = &m_jacobians[constraintRow.m_jacAindex];
-			for (int i = 0; i < ndofA ; ++i) 
-				rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
-		} 
-		if (multiBodyB)
-		{
-			ndofB  = multiBodyB->getNumLinks() + 6;
-			btScalar* jacB = &m_jacobians[constraintRow.m_jacBindex];
-			for (int i = 0; i < ndofB ; ++i) 
-				rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
-
-		}
-
-		constraintRow.m_friction = combinedFrictionCoeff;
-
-				
-		restitution =  restitutionCurve(rel_vel, combinedRestitutionCoeff);
-		if (restitution <= btScalar(0.))
-		{
-			restitution = 0.f;
-		};
-	}
-
-	/*
-	///warm starting (or zero if disabled)
-	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
-	{
-		constraintRow.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
-
-		if (constraintRow.m_appliedImpulse)
-		{
-			if (multiBodyA)
-			{
-				btScalar impulse = constraintRow.m_appliedImpulse;
-				btScalar* deltaV = &m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
-				multiBodyA->applyDeltaVee(deltaV,impulse);
-				applyDeltaVee(deltaV,impulse,constraintRow.m_deltaVelAindex,ndofA);
-			} 
-			if (multiBodyB)
-			{
-				btScalar impulse = constraintRow.m_appliedImpulse;
-				btScalar* deltaV = &m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex];
-				multiBodyB->applyDeltaVee(deltaV,impulse);
-				applyDeltaVee(deltaV,impulse,constraintRow.m_deltaVelBindex,ndofB);
-			} 
-		}
-	} 
-	else
-	*/
-	{
-		constraintRow.m_appliedImpulse = 0.f;
-	}
-
-	constraintRow.m_appliedPushImpulse = 0.f;
-
-	{
-		
-
-		btScalar positionalError = 0.f;
-		btScalar	velocityError = restitution - rel_vel;// * damping;
-					
-
-		btScalar erp = infoGlobal.m_erp2;
-		if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
-		{
-			erp = infoGlobal.m_erp;
-		}
-
-		//    const btScalar ALLOWED_PENETRATION = btScalar(0.01);
-
-//		float baumgarte_coeff = 0.3;
-	///	float one_over_dt = 1.f/infoGlobal.m_timeStep;
-	//	btScalar minus_vnew = -penetration * baumgarte_coeff * one_over_dt;
-	//	float myrhs = minus_vnew*solverConstraint.m_jacDiagABInv;//??
-		
-	//	solverConstraint.m_rhs = minus_vnew*solverConstraint.m_jacDiagABInv;//??
-		//solverConstraint.m_rhsPenetration = 0.f;
-
-		//penetration=0.f;
-
-		if (penetration>0)
-		{
-			positionalError = 0;
-			velocityError = -penetration / infoGlobal.m_timeStep;
-
-		} else
-		{
-			positionalError = -penetration * erp/infoGlobal.m_timeStep;
-		}
-
-		btScalar  penetrationImpulse = positionalError*constraintRow.m_jacDiagABInv;
-		btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv;
-
-		if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
-		{
-			//combine position and velocity into rhs
-			constraintRow.m_rhs = penetrationImpulse+velocityImpulse;
-			constraintRow.m_rhsPenetration = 0.f;
-
-		} else
-		{
-			//split position and velocity into rhs and m_rhsPenetration
-			constraintRow.m_rhs = velocityImpulse;
-			constraintRow.m_rhsPenetration = penetrationImpulse;
-		}
-
-		
-		
-
-		constraintRow.m_cfm = 0.f;
-		constraintRow.m_lowerLimit = 0;
-		constraintRow.m_upperLimit = 1e10f;
-	}
-
-}
 
 btMultiBodySolverConstraint&	btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
 {
@@ -1013,19 +764,7 @@ void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifol
 	for (int i=0;i<m_tmpNumMultiBodyConstraints;i++)
 	{
 		btMultiBodyConstraint* c = m_tmpMultiBodyConstraints[i];
-		c->update();
+		c->createConstraintRows(m_multiBodyNonContactConstraints,m_data,	infoGlobal);
-
-		for (int row=0;row<c->getNumRows();row++)
-		{
-			
-
-			btMultiBodySolverConstraint& constraintRow = m_multiBodyNonContactConstraints.expandNonInitializing();
-			constraintRow.m_multiBodyA = c->getMultiBodyA();
-			constraintRow.m_multiBodyB = c->getMultiBodyB();
-
-			btScalar penetration = c->getPosition(row);//rhs = c->computeRhs(row,infoGlobal.m_timeStep);
-			setupMultiBodyJointLimitConstraint(constraintRow,c->jacobianA(row),c->jacobianB(row),penetration,0,0,infoGlobal);
-		}
 	}
 
 }

src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h

@@ -21,7 +21,10 @@ subject to the following restrictions:
 
 
 class btMultiBody;
-class btMultiBodyConstraint;
+
+#include "btMultiBodyConstraint.h"
+
+
 
 ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraintSolver : public btSequentialImpulseConstraintSolver
 {
@@ -33,15 +36,7 @@ protected:
 	btMultiBodyConstraintArray			m_multiBodyNormalContactConstraints;
 	btMultiBodyConstraintArray			m_multiBodyFrictionContactConstraints;
 
-
+	btMultiBodyJacobianData				m_data;
-	btAlignedObjectArray<btScalar>		m_jacobians;
-	btAlignedObjectArray<btScalar>		m_deltaVelocitiesUnitImpulse;
-	btAlignedObjectArray<btScalar>		m_deltaVelocities;
-
-
-	btAlignedObjectArray<btScalar> scratch_r;
-	btAlignedObjectArray<btVector3> scratch_v;
-	btAlignedObjectArray<btMatrix3x3> scratch_m;
 	
 	//temp storage for multi body constraints for a specific island/group called by 'solveGroup'
 	btMultiBodyConstraint**					m_tmpMultiBodyConstraints;

src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp

@@ -4,15 +4,16 @@
 #include "btMultiBodyLinkCollider.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 
+
 btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper)
 	:btMultiBodyConstraint(body,body,link,link,2,true),
 	m_lowerBound(lower),
 	m_upperBound(upper)
 {
-	// the jacobians never change, so may as well
+	// the data.m_jacobians never change, so may as well
     // initialize them here
         
-    // note: we rely on the fact that jacobians are
+    // note: we rely on the fact that data.m_jacobians are
     // always initialized to zero by the Constraint ctor
 
     // row 0: the lower bound
@@ -35,9 +36,12 @@ int btMultiBodyJointLimitConstraint::getIslandIdB() const
 	return m_bodyB->getLinkCollider(0)->getIslandTag();
 }
 
-void btMultiBodyJointLimitConstraint::update()
+
+void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+		btMultiBodyJacobianData& data,
+		const btContactSolverInfo& infoGlobal)
 {
-    // only positions need to be updated -- jacobians and force
+    // only positions need to be updated -- data.m_jacobians and force
     // directions were set in the ctor and never change.
     
     // row 0: the lower bound
@@ -45,4 +49,249 @@ void btMultiBodyJointLimitConstraint::update()
 
     // row 1: the upper bound
     setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA));
+
+	for (int row=0;row<getNumRows();row++)
+	{
+		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
+		constraintRow.m_multiBodyA = m_bodyA;
+		constraintRow.m_multiBodyB = m_bodyB;
+		btScalar penetration = getPosition(row);
+		fillConstraintRow(constraintRow,data,jacobianA(row),jacobianB(row),penetration,0,0,infoGlobal);
+	}
+
+}
+	
+void btMultiBodyJointLimitConstraint::fillConstraintRow(btMultiBodySolverConstraint& constraintRow,
+														btMultiBodyJacobianData& data,
+														btScalar* jacOrgA,btScalar* jacOrgB,
+														btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff,
+														const btContactSolverInfo& infoGlobal)
+
+{
+			
+	
+	
+	btMultiBody* multiBodyA = constraintRow.m_multiBodyA;
+	btMultiBody* multiBodyB = constraintRow.m_multiBodyB;
+
+	if (multiBodyA)
+	{
+		
+		const int ndofA  = multiBodyA->getNumLinks() + 6;
+
+		constraintRow.m_deltaVelAindex = multiBodyA->getCompanionId();
+
+		if (constraintRow.m_deltaVelAindex <0)
+		{
+			constraintRow.m_deltaVelAindex = data.m_deltaVelocities.size();
+			multiBodyA->setCompanionId(constraintRow.m_deltaVelAindex);
+			data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
+		} else
+		{
+			btAssert(data.m_deltaVelocities.size() >= constraintRow.m_deltaVelAindex+ndofA);
+		}
+
+		constraintRow.m_jacAindex = data.m_jacobians.size();
+		data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
+		data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);
+		btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
+		for (int i=0;i<ndofA;i++)
+			data.m_jacobians[constraintRow.m_jacAindex+i] = jacOrgA[i];
+		
+		float* delta = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
+		multiBodyA->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacAindex],delta,data.scratch_r, data.scratch_v);
+	} 
+
+	if (multiBodyB)
+	{
+		const int ndofB  = multiBodyB->getNumLinks() + 6;
+
+		constraintRow.m_deltaVelBindex = multiBodyB->getCompanionId();
+		if (constraintRow.m_deltaVelBindex <0)
+		{
+			constraintRow.m_deltaVelBindex = data.m_deltaVelocities.size();
+			multiBodyB->setCompanionId(constraintRow.m_deltaVelBindex);
+			data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
+		}
+
+		constraintRow.m_jacBindex = data.m_jacobians.size();
+		data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
+
+		for (int i=0;i<ndofB;i++)
+			data.m_jacobians[constraintRow.m_jacBindex+i] = jacOrgB[i];
+
+		data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
+		btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
+		multiBodyB->calcAccelerationDeltas(&data.m_jacobians[constraintRow.m_jacBindex],&data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex],data.scratch_r, data.scratch_v);
+	} 
+	{
+						
+		btVector3 vec;
+		btScalar denom0 = 0.f;
+		btScalar denom1 = 0.f;
+		btScalar* jacB = 0;
+		btScalar* jacA = 0;
+		btScalar* lambdaA =0;
+		btScalar* lambdaB =0;
+		int ndofA  = 0;
+		if (multiBodyA)
+		{
+			ndofA  = multiBodyA->getNumLinks() + 6;
+			jacA = &data.m_jacobians[constraintRow.m_jacAindex];
+			lambdaA = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
+			for (int i = 0; i < ndofA; ++i)
+			{
+				float j = jacA[i] ;
+				float l =lambdaA[i];
+				denom0 += j*l;
+			}
+		} 
+		if (multiBodyB)
+		{
+			const int ndofB  = multiBodyB->getNumLinks() + 6;
+			jacB = &data.m_jacobians[constraintRow.m_jacBindex];
+			lambdaB = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex];
+			for (int i = 0; i < ndofB; ++i)
+			{
+				float j = jacB[i] ;
+				float l =lambdaB[i];
+				denom1 += j*l;
+			}
+
+		} 
+
+		 if (multiBodyA && (multiBodyA==multiBodyB))
+		 {
+            // ndof1 == ndof2 in this case
+            for (int i = 0; i < ndofA; ++i) 
+			{
+                denom1 += jacB[i] * lambdaA[i];
+                denom1 += jacA[i] * lambdaB[i];
+            }
+        }
+
+		 float d = denom0+denom1;
+		 if (btFabs(d)>SIMD_EPSILON)
+		 {
+			 
+			 constraintRow.m_jacDiagABInv = 1.f/(d);
+		 } else
+		 {
+			constraintRow.m_jacDiagABInv  = 1.f;
+		 }
+		
+	}
+
+	
+	//compute rhs and remaining constraintRow fields
+
+	
+
+
+	btScalar rel_vel = 0.f;
+	int ndofA  = 0;
+	int ndofB  = 0;
+	{
+
+		btVector3 vel1,vel2;
+		if (multiBodyA)
+		{
+			ndofA  = multiBodyA->getNumLinks() + 6;
+			btScalar* jacA = &data.m_jacobians[constraintRow.m_jacAindex];
+			for (int i = 0; i < ndofA ; ++i) 
+				rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
+		} 
+		if (multiBodyB)
+		{
+			ndofB  = multiBodyB->getNumLinks() + 6;
+			btScalar* jacB = &data.m_jacobians[constraintRow.m_jacBindex];
+			for (int i = 0; i < ndofB ; ++i) 
+				rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
+
+		}
+
+		constraintRow.m_friction = combinedFrictionCoeff;
+
+				
+		
+	}
+
+	/*
+	///warm starting (or zero if disabled)
+	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+	{
+		constraintRow.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
+
+		if (constraintRow.m_appliedImpulse)
+		{
+			if (multiBodyA)
+			{
+				btScalar impulse = constraintRow.m_appliedImpulse;
+				btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacAindex];
+				multiBodyA->applyDeltaVee(deltaV,impulse);
+				applyDeltaVee(deltaV,impulse,constraintRow.m_deltaVelAindex,ndofA);
+			} 
+			if (multiBodyB)
+			{
+				btScalar impulse = constraintRow.m_appliedImpulse;
+				btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[constraintRow.m_jacBindex];
+				multiBodyB->applyDeltaVee(deltaV,impulse);
+				applyDeltaVee(deltaV,impulse,constraintRow.m_deltaVelBindex,ndofB);
+			} 
+		}
+	} 
+	else
+	*/
+	{
+		constraintRow.m_appliedImpulse = 0.f;
+	}
+
+	constraintRow.m_appliedPushImpulse = 0.f;
+
+	{
+		float desiredVelocity = -0.3;
+
+		btScalar positionalError = 0.f;
+		btScalar	velocityError =  - rel_vel;// * damping;
+					
+
+		btScalar erp = infoGlobal.m_erp2;
+		if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+		{
+			erp = infoGlobal.m_erp;
+		}
+
+		if (penetration>0)
+		{
+			positionalError = 0;
+			velocityError = -penetration / infoGlobal.m_timeStep;
+		} else
+		{
+			positionalError = -penetration * erp/infoGlobal.m_timeStep;
+		}
+
+		btScalar  penetrationImpulse = positionalError*constraintRow.m_jacDiagABInv;
+		btScalar velocityImpulse = velocityError *constraintRow.m_jacDiagABInv;
+
+		if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+		{
+			//combine position and velocity into rhs
+			constraintRow.m_rhs = penetrationImpulse+velocityImpulse;
+			constraintRow.m_rhsPenetration = 0.f;
+
+		} else
+		{
+			//split position and velocity into rhs and m_rhsPenetration
+			constraintRow.m_rhs = velocityImpulse;
+			constraintRow.m_rhsPenetration = penetrationImpulse;
+		}
+
+		
+		
+
+		constraintRow.m_cfm = 0.f;
+		constraintRow.m_lowerLimit = 0;
+		constraintRow.m_upperLimit = 1e10f;
+	}
+
 }

src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h

@@ -17,6 +17,7 @@ subject to the following restrictions:
 #define BT_MULTIBODY_JOINT_LIMIT_CONSTRAINT_H
 
 #include "btMultiBodyConstraint.h"
+struct btSolverInfo;
 
 class btMultiBodyJointLimitConstraint : public btMultiBodyConstraint
 {
@@ -24,6 +25,11 @@ protected:
 
 	btScalar	m_lowerBound;
 	btScalar	m_upperBound;
+	void btMultiBodyJointLimitConstraint::fillConstraintRow(btMultiBodySolverConstraint& constraintRow,
+															btMultiBodyJacobianData& data,
+																 btScalar* jacOrgA,btScalar* jacOrgB,
+																 btScalar penetration,btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff,
+																 const btContactSolverInfo& infoGlobal1);
 public:
 
 	btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper);
@@ -32,7 +38,10 @@ public:
 	virtual int getIslandIdA() const;
 	virtual int getIslandIdB() const;
 
-	virtual void update();
+	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
+		btMultiBodyJacobianData& data,
+		const btContactSolverInfo& infoGlobal);
+	
 	
 };
 

src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h

@@ -20,6 +20,8 @@ subject to the following restrictions:
 #include "LinearMath/btAlignedObjectArray.h"
 
 class btMultiBody;
+#include "BulletDynamics/ConstraintSolver/btSolverBody.h"
+#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
 
 ///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
 ATTRIBUTE_ALIGNED16 (struct)	btMultiBodySolverConstraint