implemented rolling friction, using a contact constraint. Useful to get rolling spheres to rest, even on a slightly sloped plane.

See http://www.youtube.com/watch?v=RV7sBAsKu4M and Bullet/Demos/RollingFrictionDemo
Fixes in FractureDemo (mouse picking constraint needs to be removed, otherwise constraint solver crashes/asserts)

Demos/BasicDemo/main.cpp

@@ -18,8 +18,7 @@ subject to the following restrictions:
 #include "btBulletDynamicsCommon.h"
 #include "LinearMath/btHashMap.h"
 
-#include "GLDebugDrawer.h"
+
-static GLDebugDrawer sDebugDraw;
 
 
 	
@@ -28,7 +27,7 @@ int main(int argc,char** argv)
 
 	BasicDemo ccdDemo;
 	ccdDemo.initPhysics();
-	ccdDemo.getDynamicsWorld()->setDebugDrawer(&sDebugDraw);
+
 
 #ifdef CHECK_MEMORY_LEAKS
 	ccdDemo.exitPhysics();

Demos/CMakeLists.txt

@@ -12,7 +12,7 @@ IF (USE_GLUT)
 			SET(SharedDemoSubdirs
 			OpenGL AllBulletDemos  ConvexDecompositionDemo
 			CcdPhysicsDemo ConstraintDemo SliderConstraintDemo GenericJointDemo Raytracer
-			RagdollDemo ForkLiftDemo BasicDemo RaytestDemo VoronoiFractureDemo GyroscopicDemo FractureDemo Box2dDemo BspDemo MovingConcaveDemo VehicleDemo
+			RagdollDemo ForkLiftDemo BasicDemo RollingFrictionDemo RaytestDemo VoronoiFractureDemo GyroscopicDemo FractureDemo Box2dDemo BspDemo MovingConcaveDemo VehicleDemo
 			UserCollisionAlgorithm CharacterDemo SoftDemo 
 			CollisionInterfaceDemo ConcaveConvexcastDemo SimplexDemo DynamicControlDemo
 			ConvexHullDistance
@@ -48,6 +48,7 @@ ELSE (USE_GLUT)
 		CollisionInterfaceDemo 
 		ConcaveDemo 
 		ConstraintDemo
+		RollingFrictionDemo
 		ConvexDecompositionDemo 
 		InternalEdgeDemo
 		GimpactTestDemo

Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp

@@ -223,7 +223,7 @@ void	CcdPhysicsDemo::initPhysics()
 		btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
 		btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia);
 		btRigidBody* body = new btRigidBody(rbInfo);
-
+		body->setRollingFriction(0.3);
 		//add the body to the dynamics world
 		m_dynamicsWorld->addRigidBody(body);
 	}
@@ -282,7 +282,7 @@ void	CcdPhysicsDemo::initPhysics()
 
 			btRigidBody* body = localCreateRigidBody(mass,trans,shape);
 
-	
+			body->setRollingFriction(0.1);	
 			///when using m_ccdMode
 			if (m_ccdMode==USE_CCD)
 			{

Demos/FractureDemo/btFractureDynamicsWorld.cpp

@@ -336,8 +336,22 @@ void	btFractureDynamicsWorld::removeRigidBody(btRigidBody* body)
 	if (body->getInternalType() & CUSTOM_FRACTURE_TYPE)
 	{
 		btFractureBody* fbody = (btFractureBody*)body;
+		btAlignedObjectArray<btTypedConstraint*> tmpConstraints;
+
+		for (int i=0;i<fbody->getNumConstraintRefs();i++)
+		{
+			tmpConstraints.push_back(fbody->getConstraintRef(i));
+		}
+
+		//remove all constraints attached to this rigid body too		
+		for (int i=0;i<tmpConstraints.size();i++)
+			btDiscreteDynamicsWorld::removeConstraint(tmpConstraints[i]);
+
 		m_fractureBodies.remove(fbody);
 	}
+	
+
+
 	btDiscreteDynamicsWorld::removeRigidBody(body);
 }
 

Demos/GyroscopicDemo/GyroscopicDemo.cpp

@@ -101,7 +101,7 @@ void	GyroscopicDemo::initPhysics()
 		tr.setIdentity();
 		tr.setOrigin(positions[i]);
 		body->setCenterOfMassTransform(tr);
-		body->setAngularVelocity(btVector3(0,0,1000));
+		body->setAngularVelocity(btVector3(0,0,15));
 		body->setLinearVelocity(btVector3(0,.2,0));
 		body->setFriction(btSqrt(1));
 		m_dynamicsWorld->addRigidBody(body);

Demos/OpenGL/DemoApplication.cpp

@@ -787,7 +787,7 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y)
 									dof6->setAngularLowerLimit(btVector3(0,0,0));
 									dof6->setAngularUpperLimit(btVector3(0,0,0));
 
-									m_dynamicsWorld->addConstraint(dof6);
+									m_dynamicsWorld->addConstraint(dof6,true);
 									m_pickConstraint = dof6;
 
 									dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,0);
@@ -806,7 +806,7 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y)
 								} else
 								{
 									btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body,localPivot);
-									m_dynamicsWorld->addConstraint(p2p);
+									m_dynamicsWorld->addConstraint(p2p,true);
 									m_pickConstraint = p2p;
 									p2p->m_setting.m_impulseClamp = mousePickClamping;
 									//very weak constraint for picking

Demos/premake4.lua

@@ -72,6 +72,7 @@ end
     "RagdollDemo",
     "Raytracer",
     "RaytestDemo",
+    "RollingFrictionDemo",
     "SimplexDemo",
     "SliderConstraintDemo",
     "TerrainDemo",

src/BulletCollision/CollisionDispatch/btCollisionObject.cpp

@@ -31,6 +31,7 @@ btCollisionObject::btCollisionObject()
 		m_activationState1(1),
 		m_deactivationTime(btScalar(0.)),
 		m_friction(btScalar(0.5)),
+		m_rollingFriction(0.0f),
 		m_restitution(btScalar(0.)),
 		m_internalType(CO_COLLISION_OBJECT),
 		m_userObjectPointer(0),

src/BulletCollision/CollisionDispatch/btCollisionObject.h

@@ -85,6 +85,7 @@ protected:
 
 	btScalar		m_friction;
 	btScalar		m_restitution;
+	btScalar		m_rollingFriction;
 
 	///m_internalType is reserved to distinguish Bullet's btCollisionObject, btRigidBody, btSoftBody, btGhostObject etc.
 	///do not assign your own m_internalType unless you write a new dynamics object class.
@@ -263,6 +264,16 @@ public:
 		return m_friction;
 	}
 
+	void	setRollingFriction(btScalar frict)
+	{
+		m_rollingFriction = frict;
+	}
+	btScalar	getRollingFriction() const
+	{
+		return m_rollingFriction;
+	}
+
+
 	///reserved for Bullet internal usage
 	int	getInternalType() const
 	{

src/BulletCollision/CollisionDispatch/btManifoldResult.cpp

@@ -22,6 +22,23 @@ subject to the following restrictions:
 ///This is to allow MaterialCombiner/Custom Friction/Restitution values
 ContactAddedCallback		gContactAddedCallback=0;
 
+
+
+///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback;
+inline btScalar	calculateCombinedRollingFriction(const btCollisionObject* body0,const btCollisionObject* body1)
+{
+	btScalar friction = body0->getRollingFriction() * body1->getRollingFriction();
+
+	const btScalar MAX_FRICTION  = btScalar(10.);
+	if (friction < -MAX_FRICTION)
+		friction = -MAX_FRICTION;
+	if (friction > MAX_FRICTION)
+		friction = MAX_FRICTION;
+	return friction;
+
+}
+
+
 ///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback;
 inline btScalar	calculateCombinedFriction(const btCollisionObject* body0,const btCollisionObject* body1)
 {
@@ -91,7 +108,7 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b
 
 	newPt.m_combinedFriction = calculateCombinedFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
 	newPt.m_combinedRestitution = calculateCombinedRestitution(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
-
+	newPt.m_combinedRollingFriction = calculateCombinedRollingFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
 	btPlaneSpace1(newPt.m_normalWorldOnB,newPt.m_lateralFrictionDir1,newPt.m_lateralFrictionDir2);
 	
 

src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h

@@ -64,6 +64,7 @@ class btManifoldPoint
 					m_normalWorldOnB( normal ), 
 					m_distance1( distance ),
 					m_combinedFriction(btScalar(0.)),
+					m_combinedRollingFriction(btScalar(0.)),
 					m_combinedRestitution(btScalar(0.)),
 					m_userPersistentData(0),
 					m_lateralFrictionInitialized(false),
@@ -90,6 +91,7 @@ class btManifoldPoint
 		
 			btScalar	m_distance1;
 			btScalar	m_combinedFriction;
+			btScalar	m_combinedRollingFriction;
 			btScalar	m_combinedRestitution;
 
 			//BP mod, store contact triangles.

src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h

@@ -57,6 +57,7 @@ struct btContactSolverInfoData
 	int	m_restingContactRestitutionThreshold;
 	int			m_minimumSolverBatchSize;
 	btScalar	m_maxGyroscopicForce;
+	btScalar	m_singleAxisRollingFrictionThreshold;
 
 
 };
@@ -88,6 +89,7 @@ struct btContactSolverInfo : public btContactSolverInfoData
 		m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD;// | SOLVER_RANDMIZE_ORDER;
 		m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit
 		m_maxGyroscopicForce = 100.f; ///only used to clamp forces for bodies that have their BT_ENABLE_GYROPSCOPIC_FORCE flag set (using btRigidBody::setFlag)
+		m_singleAxisRollingFrictionThreshold = 1e30f;///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows.
 	}
 };
 

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -115,6 +115,7 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
 	{
 		c.m_appliedImpulse = sum;
 	}
+
 	body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
 	body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
 }
@@ -342,6 +343,8 @@ static void	applyAnisotropicFriction(btCollisionObject* colObj,btVector3& fricti
 }
 
 
+
+
 void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis,int  solverBodyIdA,int solverBodyIdB,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
 {
 
@@ -373,10 +376,7 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
 		solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0);
 	}
 
-#ifdef COMPUTE_IMPULSE_DENOM
+	{
-	btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
-	btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
-#else
 		btVector3 vec;
 		btScalar denom0 = 0.f;
 		btScalar denom1 = 0.f;
@@ -390,22 +390,89 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
 			vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
 			denom1 = body1->getInvMass() + normalAxis.dot(vec);
 		}
-
-
-#endif //COMPUTE_IMPULSE_DENOM
 		btScalar denom = relaxation/(denom0+denom1);
 		solverConstraint.m_jacDiagABInv = denom;
+	}
 
-#ifdef _USE_JACOBIAN
+	{
-	solverConstraint.m_jac =  btJacobianEntry (
-		rel_pos1,rel_pos2,solverConstraint.m_contactNormal,
-		body0->getInvInertiaDiagLocal(),
-		body0->getInvMass(),
-		body1->getInvInertiaDiagLocal(),
-		body1->getInvMass());
-#endif //_USE_JACOBIAN
 		
 
+		btScalar rel_vel;
+		btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?solverBodyA.m_linearVelocity:btVector3(0,0,0)) 
+			+ solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0));
+		btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?solverBodyB.m_linearVelocity:btVector3(0,0,0)) 
+			+ solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0));
+
+		rel_vel = vel1Dotn+vel2Dotn;
+
+//		btScalar positionalError = 0.f;
+
+		btSimdScalar velocityError =  desiredVelocity - rel_vel;
+		btSimdScalar	velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
+		solverConstraint.m_rhs = velocityImpulse;
+		solverConstraint.m_cfm = cfmSlip;
+		solverConstraint.m_lowerLimit = 0;
+		solverConstraint.m_upperLimit = 1e10f;
+		
+	}
+}
+
+btSolverConstraint&	btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
+{
+	btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
+	solverConstraint.m_frictionIndex = frictionIndex;
+	setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, 
+							colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
+	return solverConstraint;
+}
+
+
+void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint(	btSolverConstraint& solverConstraint, const btVector3& normalAxis1,int solverBodyIdA,int  solverBodyIdB,
+									btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
+									btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, 
+									btScalar desiredVelocity, btScalar cfmSlip)
+
+{
+	btVector3 normalAxis(0,0,0);
+
+
+	solverConstraint.m_contactNormal = normalAxis;
+	btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
+	btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
+
+	btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody;
+	btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
+
+	solverConstraint.m_solverBodyIdA = solverBodyIdA;
+	solverConstraint.m_solverBodyIdB = solverBodyIdB;
+
+	solverConstraint.m_friction = cp.m_combinedRollingFriction;
+	solverConstraint.m_originalContactPoint = 0;
+
+	solverConstraint.m_appliedImpulse = 0.f;
+	solverConstraint.m_appliedPushImpulse = 0.f;
+
+	{
+		btVector3 ftorqueAxis1 = -normalAxis1;
+		solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
+		solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0);
+	}
+	{
+		btVector3 ftorqueAxis1 = normalAxis1;
+		solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
+		solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0);
+	}
+
+
+	{
+		btVector3 iMJaA = body0?body0->getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal:btVector3(0,0,0);
+		btVector3 iMJaB = body1?body1->getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal:btVector3(0,0,0);
+		btScalar sum = 0;
+		sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
+		sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
+		solverConstraint.m_jacDiagABInv = btScalar(1.)/sum;
+	}
+
 	{
 		
 
@@ -431,15 +498,21 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
 
 
 
-btSolverConstraint&	btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
+
+
+
+
+
+btSolverConstraint&	btSequentialImpulseConstraintSolver::addRollingFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
 {
-	btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
+	btSolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing();
 	solverConstraint.m_frictionIndex = frictionIndex;
-	setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, 
+	setupRollingFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, 
 							colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
 	return solverConstraint;
 }
 
+
 int	btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body)
 {
 
@@ -690,6 +763,7 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 	if (!solverBodyA || (!solverBodyA->m_originalBody && (!solverBodyB || !solverBodyB->m_originalBody)))
 		return;
 
+	int rollingFriction=1;
 	for (int j=0;j<manifold->getNumContacts();j++)
 	{
 
@@ -721,6 +795,34 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 
 			solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
 
+			btVector3 angVelA,angVelB;
+			solverBodyA->getAngularVelocity(angVelA);
+			solverBodyB->getAngularVelocity(angVelB);			
+			btVector3 relAngVel = angVelB-angVelA;
+
+			if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
+			{
+				//only a single rollingFriction per manifold
+				rollingFriction--;
+				if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
+				{
+					relAngVel.normalize();
+					addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+
+				} else
+				{
+					addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+					btVector3 axis0,axis1;
+					btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
+					addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+					addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+		
+				}
+			}
+
+
+
+			cp.m_lateralFrictionInitialized = false;
 			if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
 			{
 				cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
@@ -735,11 +837,14 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 						applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
 						applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
 						addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+
 					}
 
 					applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
 					applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
 					addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
+
+			
 					cp.m_lateralFrictionInitialized = true;
 				} else
 				{
@@ -757,16 +862,19 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 					applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
 					addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
 
+
 					cp.m_lateralFrictionInitialized = true;
 				}
 
 			} else
 			{
 				addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1);
+
 				if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
 					addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2);
 			}
 
+
 			setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
 
 		}
@@ -782,6 +890,37 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 	m_maxOverrideNumSolverIterations = 0;
 
 #ifdef BT_DEBUG
+	for (int i=0;i<numConstraints;i++)
+	{
+		btTypedConstraint* constraint = constraints[i];
+		if (!constraint->getRigidBodyA().isStaticOrKinematicObject())
+        {
+            bool found=false;
+            for (int b=0;b<numBodies;b++)
+            {
+                
+                if (&constraint->getRigidBodyA()==bodies[b])
+                {
+                    found = true;
+                    break;
+                }
+            }
+            btAssert(found);
+        }
+        if (!constraint->getRigidBodyB().isStaticOrKinematicObject())
+        {
+            bool found=false;
+            for (int b=0;b<numBodies;b++)
+            {
+                if (&constraint->getRigidBodyB()==bodies[b])
+                {
+                    found = true;
+                    break;
+                }
+            }
+            btAssert(found);
+        }
+	}
     //make sure that dynamic bodies exist for all contact manifolds
     for (int i=0;i<numManifolds;i++)
     {
@@ -1071,13 +1210,14 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 
 }
 
+
 btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/)
 {
 
 	int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
 	int numConstraintPool = m_tmpSolverContactConstraintPool.size();
 	int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
-
+	int numRollingFrictionPool = m_tmpSolverContactRollingFrictionConstraintPool.size();
 	
 	if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
 	{
@@ -1195,7 +1335,10 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 
 				}
 		
+				
+
 				///solve all friction constraints, using SIMD, if available
+
 				int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
 				for (j=0;j<numFrictionPoolConstraints;j++)
 				{
@@ -1210,6 +1353,23 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
 						resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
 					}
 				}
+
+				
+				int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
+				for (j=0;j<numRollingFrictionPoolConstraints;j++)
+				{
+					btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
+					btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
+					if (totalImpulse>btScalar(0))
+					{
+						rollingFrictionConstraint.m_lowerLimit = -(rollingFrictionConstraint.m_friction*totalImpulse);
+						rollingFrictionConstraint.m_upperLimit = rollingFrictionConstraint.m_friction*totalImpulse;
+
+						resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
+					}
+				}
+				
+
 			}			
 		}
 	} else
@@ -1396,6 +1556,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo
 	m_tmpSolverContactConstraintPool.resizeNoInitialize(0);
 	m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0);
 	m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0);
+	m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0);
+
 	m_tmpSolverBodyPool.resizeNoInitialize(0);
 	return 0.f;
 }

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h

@@ -36,6 +36,8 @@ protected:
 	btConstraintArray			m_tmpSolverContactConstraintPool;
 	btConstraintArray			m_tmpSolverNonContactConstraintPool;
 	btConstraintArray			m_tmpSolverContactFrictionConstraintPool;
+	btConstraintArray			m_tmpSolverContactRollingFrictionConstraintPool;
+
 	btAlignedObjectArray<int>	m_orderTmpConstraintPool;
 	btAlignedObjectArray<int>	m_orderNonContactConstraintPool;
 	btAlignedObjectArray<int>	m_orderFrictionConstraintPool;
@@ -47,7 +49,14 @@ protected:
 									btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, 
 									btScalar desiredVelocity=0., btScalar cfmSlip=0.);
 
+	void setupRollingFrictionConstraint(	btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int  solverBodyIdB,
+									btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
+									btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, 
+									btScalar desiredVelocity=0., btScalar cfmSlip=0.);
+
 	btSolverConstraint&	addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.);
+	btSolverConstraint&	addRollingFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0, btScalar cfmSlip=0.f);
+
 
 	void setupContactConstraint(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, 
 								const btContactSolverInfo& infoGlobal, btVector3& vel, btScalar& rel_vel, btScalar& relaxation,