add warmstart

src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h

@@ -55,6 +55,7 @@ public:
 		: m_userPersistentData(0),
 		  m_contactPointFlags(0),
 		  m_appliedImpulse(0.f),
+		  m_prevRHS(0.f),
 		  m_appliedImpulseLateral1(0.f),
 		  m_appliedImpulseLateral2(0.f),
 		  m_contactMotion1(0.f),
@@ -79,6 +80,7 @@ public:
 										 m_userPersistentData(0),
 										 m_contactPointFlags(0),
 										 m_appliedImpulse(0.f),
+										 m_prevRHS(0.f),
 										 m_appliedImpulseLateral1(0.f),
 										 m_appliedImpulseLateral2(0.f),
 										 m_contactMotion1(0.f),
@@ -114,7 +116,8 @@ public:
 	int m_contactPointFlags;
 
 	btScalar m_appliedImpulse;
-	btScalar m_appliedImpulseLateral1;
+	btScalar m_prevRHS;
+  btScalar m_appliedImpulseLateral1;
 	btScalar m_appliedImpulseLateral2;
 	btScalar m_contactMotion1;
 	btScalar m_contactMotion2;

src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp

@@ -325,7 +325,8 @@ const char* btPersistentManifold::serialize(const class btPersistentManifold* ma
 	{
 		const btManifoldPoint& pt = manifold->getContactPoint(i);
 		dataOut->m_pointCacheAppliedImpulse[i] = pt.m_appliedImpulse;
-		dataOut->m_pointCacheAppliedImpulseLateral1[i] = pt.m_appliedImpulseLateral1;
+		dataOut->m_pointCachePrevRHS[i] = pt.m_prevRHS;
+    dataOut->m_pointCacheAppliedImpulseLateral1[i] = pt.m_appliedImpulseLateral1;
 		dataOut->m_pointCacheAppliedImpulseLateral2[i] = pt.m_appliedImpulseLateral2;
 		pt.m_localPointA.serialize(dataOut->m_pointCacheLocalPointA[i]);
 		pt.m_localPointB.serialize(dataOut->m_pointCacheLocalPointB[i]);
@@ -337,7 +338,6 @@ const char* btPersistentManifold::serialize(const class btPersistentManifold* ma
 		dataOut->m_pointCacheFrictionCFM[i] = pt.m_frictionCFM;
 		dataOut->m_pointCacheContactERP[i] = pt.m_contactERP;
 		dataOut->m_pointCacheContactCFM[i] = pt.m_contactCFM;
-		dataOut->m_pointCacheContactPointFlags[i] = pt.m_contactPointFlags;
 		dataOut->m_pointCacheIndex0[i] = pt.m_index0;
 		dataOut->m_pointCacheIndex1[i] = pt.m_index1;
 		dataOut->m_pointCachePartId0[i] = pt.m_partId0;
@@ -371,6 +371,7 @@ void btPersistentManifold::deSerialize(const struct btPersistentManifoldDoubleDa
 		btManifoldPoint& pt = m_pointCache[i];
 
 		pt.m_appliedImpulse = manifoldDataPtr->m_pointCacheAppliedImpulse[i];
+		pt.m_prevRHS = manifoldDataPtr->m_pointCachePrevRHS[i];
 		pt.m_appliedImpulseLateral1 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral1[i];
 		pt.m_appliedImpulseLateral2 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral2[i];
 		pt.m_localPointA.deSerializeDouble(manifoldDataPtr->m_pointCacheLocalPointA[i]);
@@ -416,6 +417,7 @@ void btPersistentManifold::deSerialize(const struct btPersistentManifoldFloatDat
 		btManifoldPoint& pt = m_pointCache[i];
 
 		pt.m_appliedImpulse = manifoldDataPtr->m_pointCacheAppliedImpulse[i];
+		pt.m_prevRHS = manifoldDataPtr->m_pointCachePrevRHS[i];
 		pt.m_appliedImpulseLateral1 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral1[i];
 		pt.m_appliedImpulseLateral2 = manifoldDataPtr->m_pointCacheAppliedImpulseLateral2[i];
 		pt.m_localPointA.deSerialize(manifoldDataPtr->m_pointCacheLocalPointA[i]);
@@ -444,4 +446,4 @@ void btPersistentManifold::deSerialize(const struct btPersistentManifoldFloatDat
 		pt.m_contactMotion1 = manifoldDataPtr->m_pointCacheContactMotion1[i];
 		pt.m_contactMotion2 = manifoldDataPtr->m_pointCacheContactMotion2[i];
 	}
-}
+}

src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h

@@ -173,6 +173,7 @@ public:
 			//get rid of duplicated userPersistentData pointer
 			m_pointCache[lastUsedIndex].m_userPersistentData = 0;
 			m_pointCache[lastUsedIndex].m_appliedImpulse = 0.f;
+			m_pointCache[lastUsedIndex].m_prevRHS = 0.f;
 			m_pointCache[lastUsedIndex].m_contactPointFlags = 0;
 			m_pointCache[lastUsedIndex].m_appliedImpulseLateral1 = 0.f;
 			m_pointCache[lastUsedIndex].m_appliedImpulseLateral2 = 0.f;
@@ -195,6 +196,7 @@ public:
 #ifdef MAINTAIN_PERSISTENCY
 		int lifeTime = m_pointCache[insertIndex].getLifeTime();
 		btScalar appliedImpulse = m_pointCache[insertIndex].m_appliedImpulse;
+		btScalar prevRHS = m_pointCache[insertIndex].m_prevRHS;
 		btScalar appliedLateralImpulse1 = m_pointCache[insertIndex].m_appliedImpulseLateral1;
 		btScalar appliedLateralImpulse2 = m_pointCache[insertIndex].m_appliedImpulseLateral2;
 
@@ -223,6 +225,7 @@ public:
 			m_pointCache[insertIndex] = newPoint;
 			m_pointCache[insertIndex].m_userPersistentData = cache;
 			m_pointCache[insertIndex].m_appliedImpulse = appliedImpulse;
+			m_pointCache[insertIndex].m_prevRHS = prevRHS;
 			m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1;
 			m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2;
 		}
@@ -276,7 +279,8 @@ struct btPersistentManifoldDoubleData
 	btVector3DoubleData	m_pointCacheLateralFrictionDir2[4];
 	double m_pointCacheDistance[4];
 	double m_pointCacheAppliedImpulse[4];
-	double m_pointCacheCombinedFriction[4];
+	double m_pointCachePrevRHS[4];
+        double m_pointCacheCombinedFriction[4];
 	double m_pointCacheCombinedRollingFriction[4];
 	double m_pointCacheCombinedSpinningFriction[4];
 	double m_pointCacheCombinedRestitution[4];
@@ -322,6 +326,7 @@ struct btPersistentManifoldFloatData
 	btVector3FloatData	m_pointCacheLateralFrictionDir2[4];
 	float m_pointCacheDistance[4];
 	float m_pointCacheAppliedImpulse[4];
+	float m_pointCachePrevRHS[4];
 	float m_pointCacheCombinedFriction[4];
 	float m_pointCacheCombinedRollingFriction[4];
 	float m_pointCacheCombinedSpinningFriction[4];

src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp

@@ -342,40 +342,6 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(btMultiBodySolverConstra
 		solverConstraint.m_friction = 0.f;  //cp.m_combinedFriction;
 	}
 
-	///warm starting (or zero if disabled)
-	/*
-     if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
-     {
-     solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
-     
-     if (solverConstraint.m_appliedImpulse)
-     {
-     if (multiBodyA)
-     {
-     btScalar impulse = solverConstraint.m_appliedImpulse;
-     btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
-     multiBodyA->applyDeltaVee(deltaV,impulse);
-     applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
-     } else
-     {
-     if (rb0)
-					bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
-     }
-     if (multiBodyB)
-     {
-     btScalar impulse = solverConstraint.m_appliedImpulse;
-     btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
-     multiBodyB->applyDeltaVee(deltaV,impulse);
-     applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
-     } else
-     {
-     if (rb1)
-					bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
-     }
-     }
-     } else
-     */
-
 	solverConstraint.m_appliedImpulse = 0.f;
 	solverConstraint.m_appliedPushImpulse = 0.f;
 

src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp

@@ -22,6 +22,8 @@ subject to the following restrictions:
 #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
 
 #include "LinearMath/btQuickprof.h"
+#include "BulletDynamics/Featherstone/btMultiBodySolverConstraint.h"
+#include "LinearMath/btScalar.h"
 
 btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
 {
@@ -491,11 +493,7 @@ btScalar btMultiBodyConstraintSolver::resolveConeFrictionConstraintRows(const bt
 	return deltaVel;
 }
 
-void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint,
+void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySolverConstraint& solverConstraint, const btVector3& contactNormal, const btScalar& appliedImpulse, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, btScalar& relaxation, bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
-																  const btVector3& contactNormal,
-																  btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
-																  btScalar& relaxation,
-																  bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
 {
 	BT_PROFILE("setupMultiBodyContactConstraint");
 	btVector3 rel_pos1;
@@ -781,48 +779,6 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		}
 	}
 
-	///warm starting (or zero if disabled)
-	//disable warmstarting for btMultiBody, it has issues gaining energy (==explosion)
-	if (/* DISABLES CODE */ (0)) //infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
-	{
-		solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
-
-		if (solverConstraint.m_appliedImpulse)
-		{
-			if (multiBodyA)
-			{
-				btScalar impulse = solverConstraint.m_appliedImpulse;
-				btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
-				multiBodyA->applyDeltaVeeMultiDof(deltaV, impulse);
-
-				applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelAindex, ndofA);
-			}
-			else
-			{
-				if (rb0)
-					bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1 * bodyA->internalGetInvMass() * rb0->getLinearFactor(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse);
-			}
-			if (multiBodyB)
-			{
-				btScalar impulse = solverConstraint.m_appliedImpulse;
-				btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
-				multiBodyB->applyDeltaVeeMultiDof(deltaV, impulse);
-				applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelBindex, ndofB);
-			}
-			else
-			{
-				if (rb1)
-					bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2 * bodyB->internalGetInvMass() * rb1->getLinearFactor(), -solverConstraint.m_angularComponentB, -(btScalar)solverConstraint.m_appliedImpulse);
-			}
-		}
-	}
-	else
-	{
-		solverConstraint.m_appliedImpulse = 0.f;
-	}
-
-	solverConstraint.m_appliedPushImpulse = 0.f;
-
 	{
 		btScalar positionalError = 0.f;
 		btScalar velocityError = restitution - rel_vel;  // * damping;	//note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction
@@ -874,6 +830,53 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 
 		solverConstraint.m_cfm = cfm * solverConstraint.m_jacDiagABInv;
 	}
+        
+	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+	{
+		if (btFabs(cp.m_prevRHS) > 1e-5)
+		{
+			solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse / cp.m_prevRHS * solverConstraint.m_rhs * infoGlobal.m_warmstartingFactor;
+			if (solverConstraint.m_appliedImpulse < 0)
+				solverConstraint.m_appliedImpulse = 0;
+		}
+		else
+		{
+			solverConstraint.m_appliedImpulse = 0.f;
+		}
+
+		if (solverConstraint.m_appliedImpulse)
+		{
+			if (multiBodyA)
+			{
+				btScalar impulse = solverConstraint.m_appliedImpulse;
+				btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+				multiBodyA->applyDeltaVeeMultiDof2(deltaV, impulse);
+
+				applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelAindex, ndofA);
+			}
+			else
+			{
+				if (rb0)
+					bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1 * bodyA->internalGetInvMass() * rb0->getLinearFactor(), solverConstraint.m_angularComponentA, solverConstraint.m_appliedImpulse);
+			}
+			if (multiBodyB)
+			{
+				btScalar impulse = solverConstraint.m_appliedImpulse;
+				btScalar* deltaV = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
+				multiBodyB->applyDeltaVeeMultiDof2(deltaV, impulse);
+				applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelBindex, ndofB);
+			}
+			else
+			{
+				if (rb1)
+					bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2 * bodyB->internalGetInvMass() * rb1->getLinearFactor(), -solverConstraint.m_angularComponentB, -(btScalar)solverConstraint.m_appliedImpulse);
+			}
+		}
+	}
+	else
+	{
+		solverConstraint.m_appliedImpulse = 0.f;
+	}
 }
 
 void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint& solverConstraint,
@@ -1130,7 +1133,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu
 	}
 }
 
-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)
+btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis, const btScalar& appliedImpulse, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
 {
 	BT_PROFILE("addMultiBodyFrictionConstraint");
 	btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing();
@@ -1161,7 +1164,7 @@ btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionCo
 
 	solverConstraint.m_originalContactPoint = &cp;
 
-	setupMultiBodyContactConstraint(solverConstraint, normalAxis, cp, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip);
+	setupMultiBodyContactConstraint(solverConstraint, normalAxis, 0, cp, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip);
 	return solverConstraint;
 }
 
@@ -1297,7 +1300,7 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 			solverConstraint.m_originalContactPoint = &cp;
 
 			bool isFriction = false;
-			setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB, cp, infoGlobal, relaxation, isFriction);
+			setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB, cp.m_appliedImpulse, cp, infoGlobal, relaxation, isFriction);
 
 			//			const btVector3& pos1 = cp.getPositionWorldOnA();
 			//			const btVector3& pos2 = cp.getPositionWorldOnB();
@@ -1371,13 +1374,13 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 				{
 					applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
 					applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir1, btCollisionObject::CF_ANISOTROPIC_FRICTION);
-					addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal);
+					addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, cp.m_appliedImpulseLateral1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal);
 
 					if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 					{
 						applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
 						applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_FRICTION);
-						addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal);
+					  addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, cp.m_appliedImpulseLateral2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal);
 					}
 
 					if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
@@ -1388,26 +1391,25 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 			}
 			else
 			{
-				addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM);
+				addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1, cp.m_appliedImpulseLateral1, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM);
 
 				if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
-					addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM);
+					addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2, cp.m_appliedImpulseLateral2, manifold, frictionIndex, cp, colObj0, colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM);
-
-				//setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
-				//todo:
-				solverConstraint.m_appliedImpulse = 0.f;
-				solverConstraint.m_appliedPushImpulse = 0.f;
 			}
 
 #endif  //ENABLE_FRICTION
 		}
+		else
+		{
+			// Reset quantities related to warmstart as 0.
+			cp.m_appliedImpulse = 0;
+			cp.m_prevRHS = 0;
+		}
 	}
 }
 
 void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal)
 {
-	//btPersistentManifold* manifold = 0;
-
 	for (int i = 0; i < numManifolds; i++)
 	{
 		btPersistentManifold* manifold = manifoldPtr[i];
@@ -1434,6 +1436,51 @@ void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifol
 
 		c->createConstraintRows(m_multiBodyNonContactConstraints, m_data, infoGlobal);
 	}
+
+	// Warmstart for noncontact constraints
+	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+	{
+		for (int i = 0; i < m_multiBodyNonContactConstraints.size(); i++)
+		{
+			btMultiBodySolverConstraint& solverConstraint =
+				m_multiBodyNonContactConstraints[i];
+			solverConstraint.m_appliedImpulse =
+				solverConstraint.m_orgConstraint->getAppliedImpulse(solverConstraint.m_orgDofIndex) *
+				infoGlobal.m_warmstartingFactor;
+
+			btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
+			btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
+			if (solverConstraint.m_appliedImpulse)
+			{
+				if (multiBodyA)
+				{
+					int ndofA = multiBodyA->getNumDofs() + 6;
+					btScalar* deltaV =
+						&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+					btScalar impulse = solverConstraint.m_appliedImpulse;
+					multiBodyA->applyDeltaVeeMultiDof2(deltaV, impulse);
+					applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelAindex, ndofA);
+				}
+				if (multiBodyB)
+				{
+					int ndofB = multiBodyB->getNumDofs() + 6;
+					btScalar* deltaV =
+						&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
+					btScalar impulse = solverConstraint.m_appliedImpulse;
+					multiBodyB->applyDeltaVeeMultiDof2(deltaV, impulse);
+					applyDeltaVee(deltaV, impulse, solverConstraint.m_deltaVelBindex, ndofB);
+				}
+			}
+		}
+	}
+	else
+	{
+		for (int i = 0; i < m_multiBodyNonContactConstraints.size(); i++)
+		{
+			btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
+			solverConstraint.m_appliedImpulse = 0;
+		}
+	}
 }
 
 btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
@@ -1678,10 +1725,22 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO
 	}
 #endif
 #endif
-
+	writeBackMultiBodyContacts(0, numPoolConstraints);
 	return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
 }
 
+void btMultiBodyConstraintSolver::writeBackMultiBodyContacts(int iBegin, int iEnd)
+{
+	for (int j = iBegin; j < iEnd; j++)
+	{
+		const btMultiBodySolverConstraint& solveManifold = m_multiBodyNormalContactConstraints[j];
+		btManifoldPoint* pt = (btManifoldPoint*)solveManifold.m_originalContactPoint;
+		btAssert(pt);
+		pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
+		pt->m_prevRHS = solveManifold.m_rhs;
+	}
+}
+
 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)
 {
 	//printf("solveMultiBodyGroup: numBodies=%d, numConstraints=%d, numManifolds=%d, numMultiBodyConstraints=%d\n", numBodies, numConstraints, numManifolds, numMultiBodyConstraints);

src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h

@@ -49,7 +49,7 @@ protected:
 
 	void convertContacts(btPersistentManifold * *manifoldPtr, int numManifolds, const btContactSolverInfo& infoGlobal);
 
-	btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0, btScalar cfmSlip = 0);
+	btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis, const btScalar& appliedImpulse, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0, btScalar cfmSlip = 0);
 
 	btMultiBodySolverConstraint& addMultiBodyTorsionalFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp,
 																		 btScalar combinedTorsionalFriction,
@@ -66,7 +66,9 @@ protected:
 
 	void setupMultiBodyContactConstraint(btMultiBodySolverConstraint & solverConstraint,
 										 const btVector3& contactNormal,
-										 btManifoldPoint& cp, const btContactSolverInfo& infoGlobal,
+                     const btScalar& appliedImpulse,
+										 btManifoldPoint& cp,
+                     const btContactSolverInfo& infoGlobal,
 										 btScalar& relaxation,
 										 bool isFriction, btScalar desiredVelocity = 0, btScalar cfmSlip = 0);
 
@@ -82,10 +84,10 @@ protected:
 	void convertMultiBodyContact(btPersistentManifold * manifold, const btContactSolverInfo& infoGlobal);
 	virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
 	//	virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
-
 	virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
 	void applyDeltaVee(btScalar * deltaV, btScalar impulse, int velocityIndex, int ndof);
 	void writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint & constraint, btScalar deltaTime);
+	void writeBackMultiBodyContacts(int iBegin, int iEnd);
 
 public:
 	BT_DECLARE_ALIGNED_ALLOCATOR();