btMultiBodyConstraintSolver writes back the applied impulse for conta…

…ct points

(added some debugging output for this in the demos, commented-out by default)

Demos3/AllBullet2Demos/main.cpp

@@ -180,11 +180,13 @@ int main(int argc, char* argv[])
 
 		app->drawGrid();
 
+		static int frameCount = 0;
+		frameCount++;
+
 		if (0)
 		{
 		char bla[1024];
-		static int frameCount = 0;
-		frameCount++;
+
 		sprintf(bla,"Simple test frame %d", frameCount);
 
 		app->drawText(bla,10,10);
@@ -197,6 +199,9 @@ int main(int argc, char* argv[])
 				unsigned long int	curTimeInMicroseconds = clock.getTimeMicroseconds();
 				unsigned long int diff = curTimeInMicroseconds-prevTimeInMicroseconds;
 				float deltaTimeInSeconds = (diff)*1.e-6;
+				//printf("---------------------------------------------------\n");
+				//printf("Framecount = %d\n",frameCount);
+
 				sCurrentDemo->stepSimulation(deltaTimeInSeconds);//1./60.f);
 				prevTimeInMicroseconds = curTimeInMicroseconds;
 			}

Demos3/bullet2/BasicDemo/BasicDemo.cpp

@@ -145,7 +145,8 @@ void	BasicDemo::renderScene()
 void	BasicDemo::stepSimulation(float dt)
 {
 	m_dynamicsWorld->stepSimulation(dt);
-/*
+
+	/*
 	//print applied force
 	//contact points
 	for (int i=0;i<m_dynamicsWorld->getDispatcher()->getNumManifolds();i++)
@@ -165,7 +166,7 @@ void	BasicDemo::stepSimulation(float dt)
 			}
 		}
 	}
-*/
+	*/
 }
 
 

Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp

@@ -610,8 +610,27 @@ void	FeatherstoneDemo1::renderScene()
 
 void	FeatherstoneDemo1::stepSimulation(float deltaTime)
 {
-	m_dynamicsWorld->stepSimulation(deltaTime,0);
+	m_dynamicsWorld->stepSimulation(deltaTime);//,0);
 //		CProfileManager::dumpAll();
+	/*
+	for (int i=0;i<m_dynamicsWorld->getDispatcher()->getNumManifolds();i++)
+	{
+		btPersistentManifold* contact = m_dynamicsWorld->getDispatcher()->getManifoldByIndexInternal(i);
+		for (int c=0;c<contact->getNumContacts();c++)
+		{
+			btManifoldPoint& pt = contact->getContactPoint(c);
+			btScalar dist = pt.getDistance();
+			if (dist< contact->getContactProcessingThreshold())
+			{
+				printf("normalImpulse[%d.%d] = %f\n",i,c,pt.m_appliedImpulse);
+				
+			} else
+			{
+				printf("?\n");
+			}
+		}
+	}
+	*/
 }
 
 

Demos3/bullet2/FeatherstoneMultiBodyDemo/MultiDofDemo.cpp

@@ -46,6 +46,25 @@ void	MultiDofDemo::stepSimulation(float deltaTime)
 	float internalTimeStep = 1./240.f;
 	m_dynamicsWorld->stepSimulation(deltaTime,10,internalTimeStep);
 //		CProfileManager::dumpAll();
+	/*
+	for (int i=0;i<m_dynamicsWorld->getDispatcher()->getNumManifolds();i++)
+	{
+		btPersistentManifold* contact = m_dynamicsWorld->getDispatcher()->getManifoldByIndexInternal(i);
+		for (int c=0;c<contact->getNumContacts();c++)
+		{
+			btManifoldPoint& pt = contact->getContactPoint(c);
+			btScalar dist = pt.getDistance();
+			if (dist< contact->getContactProcessingThreshold())
+			{
+				printf("normalImpulse[%d.%d] = %f\n",i,c,pt.m_appliedImpulse);
+				
+			} else
+			{
+				printf("?\n");
+			}
+		}
+	}
+	*/
 }
 
 

src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp

@@ -36,8 +36,10 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
 		//if (iteration < constraint.m_overrideNumSolverIterations)
 			//resolveSingleConstraintRowGenericMultiBody(constraint);
 		resolveSingleConstraintRowGeneric(constraint);
-		if(constraint.m_multiBodyA) constraint.m_multiBodyA->__posUpdated = false;
-		if(constraint.m_multiBodyB) constraint.m_multiBodyB->__posUpdated = false;
+		if(constraint.m_multiBodyA) 
+			constraint.m_multiBodyA->__posUpdated = false;
+		if(constraint.m_multiBodyB) 
+			constraint.m_multiBodyB->__posUpdated = false;
 	}
 
 	//solve featherstone normal contact
@@ -47,8 +49,10 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
 		if (iteration < infoGlobal.m_numIterations)
 			resolveSingleConstraintRowGeneric(constraint);
 
-		if(constraint.m_multiBodyA) constraint.m_multiBodyA->__posUpdated = false;
-		if(constraint.m_multiBodyB) constraint.m_multiBodyB->__posUpdated = false;
+		if(constraint.m_multiBodyA) 
+			constraint.m_multiBodyA->__posUpdated = false;
+		if(constraint.m_multiBodyB) 
+			constraint.m_multiBodyB->__posUpdated = false;
 	}
 
 	//solve featherstone frictional contact
@@ -66,8 +70,10 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
 				frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse;
 				resolveSingleConstraintRowGeneric(frictionConstraint);
 
-				if(frictionConstraint.m_multiBodyA) frictionConstraint.m_multiBodyA->__posUpdated = false;
-				if(frictionConstraint.m_multiBodyB) frictionConstraint.m_multiBodyB->__posUpdated = false;
+				if(frictionConstraint.m_multiBodyA) 
+					frictionConstraint.m_multiBodyA->__posUpdated = false;
+				if(frictionConstraint.m_multiBodyB) 
+					frictionConstraint.m_multiBodyB->__posUpdated = false;
 			}
 		}
 	}
@@ -474,7 +480,8 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 
 
 	///warm starting (or zero if disabled)
-	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+	//disable warmstarting for btMultiBody, it has issues gaining energy (==explosion)
+	if (0)//infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
 	{
 		solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
 
@@ -826,6 +833,63 @@ btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int
 	return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
 }
 
+btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
+{
+	int numPoolConstraints = m_multiBodyNormalContactConstraints.size();
+	int i,j;
+
+	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+	{
+		for (j=0;j<numPoolConstraints;j++)
+		{
+			const btMultiBodySolverConstraint& solveManifold = m_multiBodyNormalContactConstraints[j];
+			btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
+			btAssert(pt);
+			pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
+
+			pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solveManifold.m_frictionIndex].m_appliedImpulse;
+			//printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
+			if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+			{
+				pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solveManifold.m_frictionIndex+1].m_appliedImpulse;
+			}
+			//do a callback here?
+		}
+	}
+
+
+	numPoolConstraints = m_multiBodyNonContactConstraints.size();
+
+#if 0
+	//@todo: m_originalContactPoint is not initialized for btMultiBodySolverConstraint
+	for (int i=0;i<numPoolConstraints;i++)
+	{
+		const btMultiBodySolverConstraint& c = m_multiBodyNonContactConstraints[i];
+
+		btTypedConstraint* constr = (btTypedConstraint*)c.m_originalContactPoint;
+		btJointFeedback* fb = constr->getJointFeedback();
+		if (fb)
+		{
+			fb->m_appliedForceBodyA += c.m_contactNormal1*c.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep;
+			fb->m_appliedForceBodyB += c.m_contactNormal2*c.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep;
+			fb->m_appliedTorqueBodyA += c.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep;
+			fb->m_appliedTorqueBodyB += c.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*c.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */
+
+		}
+
+		constr->internalSetAppliedImpulse(c.m_appliedImpulse);
+		if (btFabs(c.m_appliedImpulse)>=constr->getBreakingImpulseThreshold())
+		{
+			constr->setEnabled(false);
+		}
+
+	}
+#endif 
+
+
+	return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies,numBodies,infoGlobal);
+}
+
 
 void  btMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher)
 {

src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h

@@ -73,7 +73,8 @@ ATTRIBUTE_ALIGNED16(class) btMultiBodyConstraintSolver : public btSequentialImpu
 
 	///this method should not be called, it was just used during porting/integration of Featherstone btMultiBody, providing backwards compatibility but no support for btMultiBodyConstraint (only contact constraints)
 	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
-
+	virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal);
+
 	virtual void solveMultiBodyGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher);
 };