+ make compound versus soft body work (soft body uses interpolated transform)

+ fixed issue with persistent manifold, warmstarting values were not initialized properly
+ don't clear manifold in sphere-sphere collision (need warmstarting)
+ added support for 'split impulse', decouple positional error correction from velocity correction
This avoids adding momentum due to penetration correction, it can be tuned using following variables:
solverInfo.m_splitImpulse = true/false (disable/enable)
solverInfo.m_splitImpulsePenetrationThreshold (below this value, baumgarte/mixed velocity/penetration is used (cheaper, looks more plausible)
solverInfo.m_linearSlop (less jitter, when small amound of penetration is allowed)

src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h

@@ -26,7 +26,12 @@ struct btContactSolverInfoData
 	int		m_numIterations;
 	btScalar	m_maxErrorReduction;
 	btScalar	m_sor;
-	btScalar	m_erp;
+	btScalar	m_erp;//used as Baumgarte factor
+	bool		m_splitImpulse;
+	btScalar	m_splitImpulsePenetrationThreshold;
+	btScalar	m_linearSlop;
+
+
 
 };
 
@@ -41,11 +46,12 @@ struct btContactSolverInfo : public btContactSolverInfoData
 		m_restitution = btScalar(0.);
 		m_maxErrorReduction = btScalar(20.);
 		m_numIterations = 10;
-		m_erp = btScalar(0.4);
+		m_erp = btScalar(0.2);
 		m_sor = btScalar(1.3);
+		m_splitImpulse = true;
+		m_splitImpulsePenetrationThreshold = 1.f;
+		m_linearSlop = 0.0002f;
 	}
-
-	
 };
 
 #endif //CONTACT_SOLVER_INFO

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -150,9 +150,11 @@ void	initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject
 		solverBody->m_originalBody = 0;
 		solverBody->m_angularFactor = 1.f;
 	}
+	solverBody->m_pushVelocity.setValue(0.f,0.f,0.f);
+	solverBody->m_turnVelocity.setValue(0.f,0.f,0.f);
 }
 
-btScalar penetrationResolveFactor = btScalar(0.9);
+int		gNumSplitImpulseRecoveries = 0;
 
 btScalar restitutionCurve(btScalar rel_vel, btScalar restitution);
 btScalar restitutionCurve(btScalar rel_vel, btScalar restitution)
@@ -162,8 +164,65 @@ btScalar restitutionCurve(btScalar rel_vel, btScalar restitution)
 }
 
 
+void	resolveSplitPenetrationImpulseCacheFriendly(
+        btSolverBody& body1,
+        btSolverBody& body2,
+        const btSolverConstraint& contactConstraint,
+        const btContactSolverInfo& solverInfo);
+
+//SIMD_FORCE_INLINE
+void	resolveSplitPenetrationImpulseCacheFriendly(
+        btSolverBody& body1,
+        btSolverBody& body2,
+        const btSolverConstraint& contactConstraint,
+        const btContactSolverInfo& solverInfo)
+{
+        (void)solverInfo;
+
+		if (contactConstraint.m_penetration > solverInfo.m_splitImpulsePenetrationThreshold)
+        {
+
+				gNumSplitImpulseRecoveries++;
+                btScalar normalImpulse;
+
+                //  Optimized version of projected relative velocity, use precomputed cross products with normal
+                //      body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
+                //      body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
+                //      btVector3 vel = vel1 - vel2;
+                //      btScalar  rel_vel = contactConstraint.m_contactNormal.dot(vel);
+
+                btScalar rel_vel;
+                btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_pushVelocity)
+                + contactConstraint.m_relpos1CrossNormal.dot(body1.m_turnVelocity);
+                btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_pushVelocity)
+                + contactConstraint.m_relpos2CrossNormal.dot(body2.m_turnVelocity);
+
+                rel_vel = vel1Dotn-vel2Dotn;
 
 
+                btScalar positionalError = contactConstraint.m_penetration;
+                //      btScalar positionalError = contactConstraint.m_penetration;
+
+                btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;
+
+                btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
+                btScalar        velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
+                normalImpulse = penetrationImpulse+velocityImpulse;
+
+                // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
+                btScalar oldNormalImpulse = contactConstraint.m_appliedPushImpulse;
+                btScalar sum = oldNormalImpulse + normalImpulse;
+                contactConstraint.m_appliedPushImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
+
+                normalImpulse = contactConstraint.m_appliedPushImpulse - oldNormalImpulse;
+
+				body1.internalApplyPushImpulse(contactConstraint.m_contactNormal*body1.m_invMass, contactConstraint.m_angularComponentA,normalImpulse);
+               
+				body2.internalApplyPushImpulse(contactConstraint.m_contactNormal*body2.m_invMass, contactConstraint.m_angularComponentB,-normalImpulse);
+               
+        }
+
+}
 
 
 //velocity + friction
@@ -185,55 +244,52 @@ btScalar resolveSingleCollisionCombinedCacheFriendly(
 	(void)solverInfo;
 
 	btScalar normalImpulse;
-	
-	//  Optimized version of projected relative velocity, use precomputed cross products with normal
-	//	body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
-	//	body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
-	//	btVector3 vel = vel1 - vel2;
-	//	btScalar  rel_vel = contactConstraint.m_contactNormal.dot(vel);
+	bool	useSplitImpulse = false;
 
-	btScalar rel_vel;
-	btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity) 
-				+ contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
-	btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity) 
-				+ contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);
-
-	rel_vel = vel1Dotn-vel2Dotn;
-
-
-	btScalar positionalError = contactConstraint.m_penetration;
-	btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;
-
-	btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
-	btScalar	velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
-	normalImpulse = penetrationImpulse+velocityImpulse;
-	
-	// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
-	btScalar oldNormalImpulse = contactConstraint.m_appliedImpulse;
-	btScalar sum = oldNormalImpulse + normalImpulse;
-	contactConstraint.m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
-
-	btScalar oldVelocityImpulse = contactConstraint.m_appliedVelocityImpulse;
-	btScalar velocitySum = oldVelocityImpulse + velocityImpulse;
-	contactConstraint.m_appliedVelocityImpulse = btScalar(0.) > velocitySum ? btScalar(0.): velocitySum;
-
-	normalImpulse = contactConstraint.m_appliedImpulse - oldNormalImpulse;
-
-	if (body1.m_invMass)
 	{
+
+		
+		//  Optimized version of projected relative velocity, use precomputed cross products with normal
+		//	body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
+		//	body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
+		//	btVector3 vel = vel1 - vel2;
+		//	btScalar  rel_vel = contactConstraint.m_contactNormal.dot(vel);
+
+		btScalar rel_vel;
+		btScalar vel1Dotn = contactConstraint.m_contactNormal.dot(body1.m_linearVelocity) 
+					+ contactConstraint.m_relpos1CrossNormal.dot(body1.m_angularVelocity);
+		btScalar vel2Dotn = contactConstraint.m_contactNormal.dot(body2.m_linearVelocity) 
+					+ contactConstraint.m_relpos2CrossNormal.dot(body2.m_angularVelocity);
+
+		rel_vel = vel1Dotn-vel2Dotn;
+
+		btScalar positionalError = 0.f;
+		if (!solverInfo.m_splitImpulse || (contactConstraint.m_penetration<solverInfo.m_splitImpulsePenetrationThreshold))
+		{
+			positionalError = contactConstraint.m_penetration;
+		}
+
+		btScalar velocityError = contactConstraint.m_restitution - rel_vel;// * damping;
+
+		btScalar penetrationImpulse = positionalError * contactConstraint.m_jacDiagABInv;
+		btScalar	velocityImpulse = velocityError * contactConstraint.m_jacDiagABInv;
+		normalImpulse = penetrationImpulse+velocityImpulse;
+		
+		
+		// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
+		btScalar oldNormalImpulse = contactConstraint.m_appliedImpulse;
+		btScalar sum = oldNormalImpulse + normalImpulse;
+		contactConstraint.m_appliedImpulse = btScalar(0.) > sum ? btScalar(0.): sum;
+
+		normalImpulse = contactConstraint.m_appliedImpulse - oldNormalImpulse;
+
 		body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,
-			contactConstraint.m_angularComponentA,normalImpulse);
-	}
-	if (body2.m_invMass)
-	{
+				contactConstraint.m_angularComponentA,normalImpulse);
+		
 		body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,
-			contactConstraint.m_angularComponentB,-normalImpulse);
+				contactConstraint.m_angularComponentB,-normalImpulse);
 	}
 
-
-
-	
-
 	return normalImpulse;
 }
 
@@ -311,14 +367,9 @@ btScalar resolveSingleFrictionCacheFriendly(
 
 		}
 	
-		if (body1.m_invMass)
-		{
-			body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,contactConstraint.m_angularComponentA,j1);
-		}
-		if (body2.m_invMass)
-		{
-			body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,contactConstraint.m_angularComponentB,-j1);
-		}
+		body1.internalApplyImpulse(contactConstraint.m_contactNormal*body1.m_invMass,contactConstraint.m_angularComponentA,j1);
+		
+		body2.internalApplyImpulse(contactConstraint.m_contactNormal*body2.m_invMass,contactConstraint.m_angularComponentB,-j1);
 
 	} 
 	return 0.f;
@@ -360,7 +411,6 @@ btScalar resolveSingleFrictionCacheFriendly(
 		const btVector3& rel_pos2 = contactConstraint.m_rel_posB;
 
 
-		//if (contactConstraint.m_appliedVelocityImpulse > 0.f)
 		if (lat_rel_vel > SIMD_EPSILON*SIMD_EPSILON)
 		{
 			lat_rel_vel = btSqrt(lat_rel_vel);
@@ -370,7 +420,7 @@ btScalar resolveSingleFrictionCacheFriendly(
 			btVector3 temp2 = body2.m_invInertiaWorld * rel_pos2.cross(lat_vel);
 			btScalar friction_impulse = lat_rel_vel /
 				(body1.m_invMass + body2.m_invMass + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
-			btScalar normal_impulse = contactConstraint.m_appliedVelocityImpulse * combinedFriction;
+			btScalar normal_impulse = contactConstraint.m_appliedImpulse * combinedFriction;
 
 			GEN_set_min(friction_impulse, normal_impulse);
 			GEN_set_max(friction_impulse, -normal_impulse);
@@ -406,7 +456,7 @@ void	btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3&
 	solverConstraint.m_originalContactPoint = 0;
 
 	solverConstraint.m_appliedImpulse = btScalar(0.);
-	solverConstraint.m_appliedVelocityImpulse = 0.f;
+	solverConstraint.m_appliedPushImpulse = 0.f;
 	solverConstraint.m_penetration = 0.f;
 	{
 		btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal);
@@ -669,15 +719,16 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 							
 							rel_vel = cp.m_normalWorldOnB.dot(vel);
 							
+							solverConstraint.m_penetration = btMin(cp.getDistance()+infoGlobal.m_linearSlop,0.f);
+							//solverConstraint.m_penetration = cp.getDistance();
 
-							solverConstraint.m_penetration = cp.getDistance();///btScalar(infoGlobal.m_numIterations);
 							solverConstraint.m_friction = cp.m_combinedFriction;
 							solverConstraint.m_restitution =  restitutionCurve(rel_vel, cp.m_combinedRestitution);
 							if (solverConstraint.m_restitution <= btScalar(0.))
 							{
 								solverConstraint.m_restitution = 0.f;
 							};
-							
+
 							btScalar penVel = -solverConstraint.m_penetration/infoGlobal.m_timeStep;
 							solverConstraint.m_penetration *= -(infoGlobal.m_erp/infoGlobal.m_timeStep);
 
@@ -685,24 +736,25 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 							{
 								solverConstraint.m_penetration = btScalar(0.);
 							}
+							 
 							
 							
-
 							///warm starting (or zero if disabled)
 							if (m_solverMode & SOLVER_USE_WARMSTARTING)
 							{
 								solverConstraint.m_appliedImpulse = cp.m_appliedImpulse;
+								if (rb0)
+									m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA].internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass(),solverConstraint.m_angularComponentA,cp.m_appliedImpulse);
+								if (rb1)
+									m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB].internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass(),solverConstraint.m_angularComponentB,-cp.m_appliedImpulse);
 							} else
 							{
 								solverConstraint.m_appliedImpulse = 0.f;
 							}
-							solverConstraint.m_appliedVelocityImpulse = 0.f;
-							
-					
-						
+
+							solverConstraint.m_appliedPushImpulse = 0.f;
 						}
 
-					
 						{
 							btVector3 frictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
 							btScalar lat_rel_vel = frictionDir1.length2();
@@ -844,35 +896,41 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(
 				 for (j=0;j<numFrictionPoolConstraints;j++)
 				{
 					const btSolverConstraint& solveManifold = m_tmpSolverFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
-						
+					btScalar totalImpulse = m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse+
+								m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedPushImpulse;			
 
 						resolveSingleFrictionCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
 							m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal,
-							m_tmpSolverConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse);
+							totalImpulse);
 				}
 			}
 			
 
 
 		}
-	}
-
-	{
-		int numPoolConstraints = m_tmpSolverConstraintPool.size();
-		int j;
-		for (j=0;j<numPoolConstraints;j++)
+	
+		if (infoGlobal.m_splitImpulse)
 		{
-			
-			const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[j];
-			btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
-			btAssert(pt);
-			pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
-			//do a callback here?
+			for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
+			{
+				{
+					int numPoolConstraints = m_tmpSolverConstraintPool.size();
+					int j;
+					for (j=0;j<numPoolConstraints;j++)
+					{
+						const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[m_orderTmpConstraintPool[j]];
+
+						resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],
+							m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold,infoGlobal);
+					}
+				}
+			}
 
 		}
+
 	}
-	
-		return 0.f;
+
+	return 0.f;
 }
 
 
@@ -883,12 +941,32 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendly(btCollisio
 	solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
 	solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr,  numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc);
 
-		
-	for ( i=0;i<m_tmpSolverBodyPool.size();i++)
+	int numPoolConstraints = m_tmpSolverConstraintPool.size();
+	int j;
+	for (j=0;j<numPoolConstraints;j++)
 	{
-		m_tmpSolverBodyPool[i].writebackVelocity();
+		
+		const btSolverConstraint& solveManifold = m_tmpSolverConstraintPool[j];
+		btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
+		btAssert(pt);
+		pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
+		//do a callback here?
+
 	}
 
+	if (infoGlobal.m_splitImpulse)
+	{		
+		for ( i=0;i<m_tmpSolverBodyPool.size();i++)
+		{
+			m_tmpSolverBodyPool[i].writebackVelocity(infoGlobal.m_timeStep);
+		}
+	} else
+	{
+		for ( i=0;i<m_tmpSolverBodyPool.size();i++)
+        {
+                m_tmpSolverBodyPool[i].writebackVelocity();
+        }
+	}
 
 //	printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size());
 
@@ -1312,3 +1390,4 @@ void	btSequentialImpulseConstraintSolver::reset()
 	m_btSeed2 = 0;
 }
 
+

src/BulletDynamics/ConstraintSolver/btSolverBody.h

@@ -21,6 +21,7 @@ class	btRigidBody;
 #include "LinearMath/btMatrix3x3.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "LinearMath/btAlignedAllocator.h"
+#include "LinearMath/btTransformUtil.h"
 
 
 ///btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
@@ -36,6 +37,10 @@ ATTRIBUTE_ALIGNED16 (struct)	btSolverBody
 	btVector3		m_linearVelocity;
 	btVector3		m_centerOfMassPosition;
 	
+	btVector3		m_pushVelocity;
+	btVector3		m_turnVelocity;
+	
+	
 	SIMD_FORCE_INLINE void	getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const
 	{
 		velocity = m_linearVelocity + m_angularVelocity.cross(rel_pos);
@@ -44,9 +49,22 @@ ATTRIBUTE_ALIGNED16 (struct)	btSolverBody
 	//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
 	SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
 	{
-		m_linearVelocity += linearComponent*impulseMagnitude;
-		m_angularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+		if (m_invMass)
+		{
+			m_linearVelocity += linearComponent*impulseMagnitude;
+			m_angularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+		}
 	}
+	
+	SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
+	{
+		if (m_invMass)
+		{
+			m_pushVelocity += linearComponent*impulseMagnitude;
+			m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+		}
+	}
+	
 
 	void	writebackVelocity()
 	{
@@ -54,6 +72,24 @@ ATTRIBUTE_ALIGNED16 (struct)	btSolverBody
 		{
 			m_originalBody->setLinearVelocity(m_linearVelocity);
 			m_originalBody->setAngularVelocity(m_angularVelocity);
+			
+			//m_originalBody->setCompanionId(-1);
+		}
+	}
+	
+
+	void	writebackVelocity(btScalar timeStep)
+	{
+		if (m_invMass)
+		{
+			m_originalBody->setLinearVelocity(m_linearVelocity);
+			m_originalBody->setAngularVelocity(m_angularVelocity);
+			
+			//correct the position/orientation based on push/turn recovery
+			btTransform newTransform;
+			btTransformUtil::integrateTransform(m_originalBody->getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform);
+			m_originalBody->setWorldTransform(newTransform);
+			
 			//m_originalBody->setCompanionId(-1);
 		}
 	}
@@ -74,3 +110,4 @@ ATTRIBUTE_ALIGNED16 (struct)	btSolverBody
 
 #endif //BT_SOLVER_BODY_H
 
+

src/BulletDynamics/ConstraintSolver/btSolverConstraint.h

@@ -36,7 +36,9 @@ ATTRIBUTE_ALIGNED16 (struct)	btSolverConstraint
 	btVector3	m_angularComponentA;
 
 	btVector3	m_angularComponentB;
-	mutable btScalar	m_appliedVelocityImpulse;
+
+	mutable btScalar	m_appliedPushImpulse;
+	
 	mutable btScalar	m_appliedImpulse;
 	int			m_solverBodyIdA;
 	int			m_solverBodyIdB;
@@ -69,3 +71,4 @@ ATTRIBUTE_ALIGNED16 (struct)	btSolverConstraint
 #endif //BT_SOLVER_CONSTRAINT_H
 
 
+