Support btMultiBody soft contact using ERP and CFM. Also support custom relaxation parameter to allow successive over relaxation.

Added demos for rigid and multi body soft (compliant) contact.
Will also add simplified Hertz compliant contact, by dynamically modifying the ERP/CFM to mimic a non-linear spring.
Note that btManifoldPoint is growing too big, we need to implement proper contact constraints derived from btTypedConstraint.

examples/ExampleBrowser/CMakeLists.txt

@@ -137,6 +137,7 @@ SET(App_ExampleBrowser_SRCS
   ../Vehicles/Hinge2Vehicle.cpp
   ../Vehicles/Hinge2Vehicle.h
   ../MultiBody/Pendulum.cpp
+  ../MultiBody/MultiBodySoftContact.cpp
   ../MultiBody/TestJointTorqueSetup.cpp
   ../MultiBody/TestJointTorqueSetup.h
   ../MultiBody/InvertedPendulumPDControl.cpp
@@ -144,6 +145,7 @@ SET(App_ExampleBrowser_SRCS
  ../MultiBody/MultiBodyConstraintFeedback.cpp
   ../MultiBody/MultiDofDemo.cpp
   ../MultiBody/MultiDofDemo.h
+  ../RigidBody/RigidBodySoftContact.cpp
   ../Constraints/TestHingeTorque.cpp
   ../Constraints/TestHingeTorque.h
   ../Constraints/ConstraintDemo.cpp

examples/ExampleBrowser/ExampleEntries.cpp

@@ -22,10 +22,11 @@
 #include "../Constraints/ConstraintPhysicsSetup.h"
 #include "../MultiBody/TestJointTorqueSetup.h"
 #include "../MultiBody/Pendulum.h"
-
+#include "../MultiBody/MultiBodySoftContact.h"
 #include "../MultiBody/MultiBodyConstraintFeedback.h"
 #include "../MultiBody/MultiDofDemo.h"
 #include "../MultiBody/InvertedPendulumPDControl.h"
+#include "../RigidBody/RigidBodySoftContact.h"
 #include "../VoronoiFracture/VoronoiFractureDemo.h"
 #include "../SoftDemo/SoftDemo.h"
 #include "../Constraints/ConstraintDemo.h"
@@ -103,6 +104,7 @@ static ExampleEntry gDefaultExamples[]=
 	
 	ExampleEntry(1,"Gyroscopic", "Show the Dzhanibekov effect using various settings of the gyroscopic term. You can select the gyroscopic term computation using btRigidBody::setFlags, with arguments BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT (using explicit integration, which adds energy and can lead to explosions), BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD, BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY. If you don't set any of these flags, there is no gyroscopic term used.", GyroscopicCreateFunc),
 
+	ExampleEntry(1,"Soft Contact", "Using the error correction parameter (ERP) and constraint force mixing (CFM) values for contacts to simulate compliant contact.",RigidBodySoftContactCreateFunc),
 
 	
 
@@ -113,12 +115,13 @@ static ExampleEntry gDefaultExamples[]=
 	
  ExampleEntry(1,"Constraint Feedback", "The example shows how to receive joint reaction forces in a btMultiBody. Also the applied impulse is available for a btMultiBodyJointMotor", MultiBodyConstraintFeedbackCreateFunc),
 	ExampleEntry(1,"Inverted Pendulum PD","Keep an inverted pendulum up using open loop PD control", InvertedPendulumPDControlCreateFunc),
+	  ExampleEntry(1,"MultiBody Soft Contact", "Using the error correction parameter (ERP) and constraint force mixing (CFM) values for contacts to simulate compliant contact.",MultiBodySoftContactCreateFunc,0),
+
 
 	 ExampleEntry(0,"Inverse Dynamics"),
       ExampleEntry(1,"Inverse Dynamics URDF", "Create a btMultiBody from URDF. Create an inverse MultiBodyTree model from that. Use either decoupled PD control or computed torque control using the inverse model to track joint position targets", InverseDynamicsExampleCreateFunc,BT_ID_LOAD_URDF),
       ExampleEntry(1,"Inverse Dynamics Prog", "Create a btMultiBody programatically. Create an inverse MultiBodyTree model from that. Use either decoupled PD control or computed torque control using the inverse model to track joint position targets", InverseDynamicsExampleCreateFunc,BT_ID_PROGRAMMATICALLY),
-
-
+	
 	
 	ExampleEntry(0,"Tutorial"),
 	ExampleEntry(1,"Constant Velocity","Free moving rigid body, without external or constraint forces", TutorialCreateFunc,TUT_VELOCITY),

examples/ExampleBrowser/premake4.lua

@@ -101,8 +101,10 @@
 		"../MultiBody/MultiDofDemo.cpp",
 		"../MultiBody/TestJointTorqueSetup.cpp",
 		"../MultiBody/Pendulum.cpp",
+		"../MultiBody/MultiBodySoftContact.cpp",
 		"../MultiBody/MultiBodyConstraintFeedback.cpp",
 		"../MultiBody/InvertedPendulumPDControl.cpp",
+		"../RigidBody/RigidBodySoftContact.cpp",
 		"../ThirdPartyLibs/stb_image/*",
 		"../ThirdPartyLibs/Wavefront/tiny_obj_loader.*",
 		"../ThirdPartyLibs/tinyxml/*",

examples/MultiBody/MultiBodySoftContact.cpp

@@ -0,0 +1,182 @@
+
+#include "MultiBodySoftContact.h"
+
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h"
+							
+#include "../CommonInterfaces/CommonMultiBodyBase.h"
+#include "../Utils/b3ResourcePath.h"
+
+static btScalar radius(0.2);
+
+struct MultiBodySoftContact : public CommonMultiBodyBase
+{
+    btMultiBody* m_multiBody;
+	btAlignedObjectArray<btMultiBodyJointFeedback*> m_jointFeedbacks;
+
+    bool m_once;
+public:
+
+    MultiBodySoftContact(struct GUIHelperInterface* helper);
+    virtual ~MultiBodySoftContact();
+
+    virtual void initPhysics();
+
+    virtual void stepSimulation(float deltaTime);
+
+	virtual void resetCamera()
+	{
+		float dist = 5;
+		float pitch = 270;
+		float yaw = 21;
+		float targetPos[3]={0,0,0};
+		m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
+	}
+
+
+};
+
+MultiBodySoftContact::MultiBodySoftContact(struct GUIHelperInterface* helper)
+:CommonMultiBodyBase(helper),
+m_once(true)
+{
+}
+
+MultiBodySoftContact::~MultiBodySoftContact()
+{
+
+}
+
+
+extern ContactAddedCallback		gContactAddedCallback;
+static bool btMultiBodySoftContactCallback(btManifoldPoint& cp,	const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1)
+{
+    cp.m_contactCFM = 0.3;
+	cp.m_contactERP = 0.2;
+	cp.m_contactPointFlags |= BT_CONTACT_FLAG_HAS_CONTACT_CFM;
+    cp.m_contactPointFlags |= BT_CONTACT_FLAG_HAS_CONTACT_ERP;
+	return true; 
+
+}
+
+
+
+
+void MultiBodySoftContact::initPhysics()
+{
+	  gContactAddedCallback = btMultiBodySoftContactCallback;
+    int upAxis = 2;
+
+	m_guiHelper->setUpAxis(upAxis);
+
+    btVector4 colors[4] =
+    {
+        btVector4(1,0,0,1),
+        btVector4(0,1,0,1),
+        btVector4(0,1,1,1),
+        btVector4(1,1,0,1),
+    };
+    int curColor = 0;
+
+
+    
+
+	this->createEmptyDynamicsWorld();
+		m_dynamicsWorld->setGravity(btVector3(0,0,-10));
+
+	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+    m_dynamicsWorld->getDebugDrawer()->setDebugMode(
+        //btIDebugDraw::DBG_DrawConstraints
+        +btIDebugDraw::DBG_DrawWireframe
+        +btIDebugDraw::DBG_DrawContactPoints
+        +btIDebugDraw::DBG_DrawAabb
+        );//+btIDebugDraw::DBG_DrawConstraintLimits);
+
+	
+
+    //create a static ground object
+    if (1)
+        {
+            btVector3 groundHalfExtents(50,50,50);
+            btBoxShape* box = new btBoxShape(groundHalfExtents);
+            box->initializePolyhedralFeatures();
+
+            m_guiHelper->createCollisionShapeGraphicsObject(box);
+            btTransform start; start.setIdentity();
+            btVector3 groundOrigin(0,0,-50.5);
+			start.setOrigin(groundOrigin);
+		//	start.setRotation(groundOrn);
+            btRigidBody* body =  createRigidBody(0,start,box);
+            btVector4 color = colors[curColor];
+			curColor++;
+			curColor&=3;
+            m_guiHelper->createRigidBodyGraphicsObject(body,color);
+			int flags = body->getCollisionFlags();
+			body->setCollisionFlags(flags|btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
+
+        }
+
+	{
+		btCollisionShape* childShape = new btSphereShape(btScalar(0.5));
+		m_guiHelper->createCollisionShapeGraphicsObject(childShape);
+
+		btScalar mass = 1;
+		btVector3 baseInertiaDiag;
+		bool isFixed = (mass == 0);
+		childShape->calculateLocalInertia(mass,baseInertiaDiag);
+	  btMultiBody *pMultiBody = new btMultiBody(0, 1, baseInertiaDiag, false, false);
+	  btTransform startTrans;
+	  startTrans.setIdentity();
+	  startTrans.setOrigin(btVector3(0,0,3));
+
+	  pMultiBody->setBaseWorldTransform(startTrans);
+
+	   btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(pMultiBody, -1);
+		col->setCollisionShape(childShape);
+		pMultiBody->setBaseCollider(col);
+			bool isDynamic = (mass > 0 && !isFixed);
+		short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
+		short collisionFilterMask = isDynamic? 	short(btBroadphaseProxy::AllFilter) : 	short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
+
+        m_dynamicsWorld->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//, 2,1+2);
+
+
+		pMultiBody->finalizeMultiDof();
+	
+		m_dynamicsWorld->addMultiBody(pMultiBody);
+	
+		btAlignedObjectArray<btQuaternion> scratch_q;
+		btAlignedObjectArray<btVector3> scratch_m;
+		pMultiBody->forwardKinematics(scratch_q,scratch_m);
+		btAlignedObjectArray<btQuaternion> world_to_local;
+		btAlignedObjectArray<btVector3> local_origin;
+		pMultiBody->updateCollisionObjectWorldTransforms(world_to_local,local_origin);
+	}
+	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+
+
+}
+
+void MultiBodySoftContact::stepSimulation(float deltaTime)
+{
+	
+    if (0)//m_once)
+    {
+       m_once=false;
+        m_multiBody->addJointTorque(0, 10.0);
+    
+        btScalar torque = m_multiBody->getJointTorque(0);
+        b3Printf("t = %f,%f,%f\n",torque,torque,torque);//[0],torque[1],torque[2]);
+    }
+    
+    m_dynamicsWorld->stepSimulation(deltaTime);
+
+	
+    
+}
+
+
+class CommonExampleInterface*    MultiBodySoftContactCreateFunc(struct CommonExampleOptions& options)
+{
+	return new MultiBodySoftContact(options.m_guiHelper);
+}

examples/MultiBody/MultiBodySoftContact.h

@@ -0,0 +1,7 @@
+#ifndef MULTI_BODY_SOFT_CONTACT_H
+#define MULTI_BODY_SOFT_CONTACT_H
+
+class CommonExampleInterface*    MultiBodySoftContactCreateFunc(struct CommonExampleOptions& options);
+
+#endif //MULTI_BODY_SOFT_CONTACT_H
+

examples/RigidBody/RigidBodySoftContact.cpp

@@ -0,0 +1,194 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2015 Google Inc. http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+
+#include "RigidBodySoftContact.h"f
+
+#include "btBulletDynamicsCommon.h"
+#define ARRAY_SIZE_Y 1
+#define ARRAY_SIZE_X 1
+#define ARRAY_SIZE_Z 1
+
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btAlignedObjectArray.h"
+
+#include "../CommonInterfaces/CommonRigidBodyBase.h"
+#include "BulletDynamics/MLCPSolvers/btMLCPSolver.h"
+#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
+
+
+struct RigidBodySoftContact : public CommonRigidBodyBase
+{
+	RigidBodySoftContact(struct GUIHelperInterface* helper)
+		:CommonRigidBodyBase(helper)
+	{
+	}
+	virtual ~RigidBodySoftContact(){}
+	virtual void initPhysics();
+	virtual void renderScene();
+	void resetCamera()
+	{
+		float dist = 3;
+		float pitch = 52;
+		float yaw = 35;
+		float targetPos[3]={0,0.46,0};
+		m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
+	}
+};
+
+extern ContactAddedCallback		gContactAddedCallback;
+static bool btRigidBodySoftContactCallback(btManifoldPoint& cp,	const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1)
+{
+    cp.m_contactCFM = 0.3;
+	cp.m_contactERP = 0.2;
+	cp.m_contactPointFlags |= BT_CONTACT_FLAG_HAS_CONTACT_CFM;
+    cp.m_contactPointFlags |= BT_CONTACT_FLAG_HAS_CONTACT_ERP;
+	return true; 
+
+}
+
+
+
+void RigidBodySoftContact::initPhysics()
+{
+    gContactAddedCallback = btRigidBodySoftContactCallback;
+    
+	m_guiHelper->setUpAxis(1);
+
+	//createEmptyDynamicsWorld();
+	{
+		///collision configuration contains default setup for memory, collision setup
+		m_collisionConfiguration = new btDefaultCollisionConfiguration();
+		//m_collisionConfiguration->setConvexConvexMultipointIterations();
+
+		///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
+		m_dispatcher = new	btCollisionDispatcher(m_collisionConfiguration);
+
+		m_broadphase = new btDbvtBroadphase();
+
+		///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
+		btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
+		//btMLCPSolver* sol = new btMLCPSolver(new btSolveProjectedGaussSeidel());
+		m_solver = sol;
+
+		m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration);
+
+		m_dynamicsWorld->setGravity(btVector3(0, -10, 0));
+	}
+	
+	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+
+	if (m_dynamicsWorld->getDebugDrawer())
+		m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints);
+	m_dynamicsWorld->getSolverInfo().m_erp2 = 0.f;
+	m_dynamicsWorld->getSolverInfo().m_globalCfm = 0.f;
+	m_dynamicsWorld->getSolverInfo().m_numIterations = 3;
+	m_dynamicsWorld->getSolverInfo().m_solverMode = SOLVER_SIMD;// | SOLVER_RANDMIZE_ORDER;
+	m_dynamicsWorld->getSolverInfo().m_splitImpulse = false;
+	
+	///create a few basic rigid bodies
+	btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
+	
+
+	//groundShape->initializePolyhedralFeatures();
+//	btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);
+	
+	m_collisionShapes.push_back(groundShape);
+
+	btTransform groundTransform;
+	groundTransform.setIdentity();
+	groundTransform.setOrigin(btVector3(0,-50,0));
+
+	{
+		btScalar mass(0.);
+		btRigidBody* body = createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1));
+		int flags = body->getCollisionFlags();
+		body->setCollisionFlags(flags|btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
+	}
+
+
+	{
+		//create a few dynamic rigidbodies
+		// Re-using the same collision is better for memory usage and performance
+
+		//btBoxShape* colShape = createBoxShape(btVector3(1,1,1));
+		
+
+		btCollisionShape* childShape = new btSphereShape(btScalar(0.5));
+		btCompoundShape* colShape = new btCompoundShape();
+		colShape->addChildShape(btTransform::getIdentity(),childShape);
+
+		m_collisionShapes.push_back(colShape);
+
+		/// Create Dynamic Objects
+		btTransform startTransform;
+		startTransform.setIdentity();
+		
+		startTransform.setRotation(btQuaternion(btVector3(1,1,1),SIMD_PI/10.));
+		btScalar	mass(1.f);
+
+		//rigidbody is dynamic if and only if mass is non zero, otherwise static
+		bool isDynamic = (mass != 0.f);
+
+		btVector3 localInertia(0,0,0);
+		if (isDynamic)
+			colShape->calculateLocalInertia(mass,localInertia);
+
+
+		for (int k=0;k<ARRAY_SIZE_Y;k++)
+		{
+			for (int i=0;i<ARRAY_SIZE_X;i++)
+			{
+				for(int j = 0;j<ARRAY_SIZE_Z;j++)
+				{
+					startTransform.setOrigin(btVector3(
+										btScalar(2.0*i+0.1),
+										btScalar(3+2.0*k),
+										btScalar(2.0*j)));
+
+			
+					btRigidBody* body = createRigidBody(mass,startTransform,colShape);
+					//body->setAngularVelocity(btVector3(1,1,1));
+					
+
+				}
+			}
+		}
+	}
+
+	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+
+void RigidBodySoftContact::renderScene()
+{
+	CommonRigidBodyBase::renderScene();
+	
+}
+
+
+
+
+
+
+
+CommonExampleInterface*    RigidBodySoftContactCreateFunc(CommonExampleOptions& options)
+{
+	return new RigidBodySoftContact(options.m_guiHelper);
+}
+
+
+

examples/RigidBody/RigidBodySoftContact.h

@@ -0,0 +1,22 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2015 Google Inc. http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef RIGID_BODY_SOFT_CONTACT_H
+#define RIGID_BODY_SOFT_CONTACT_H
+
+class CommonExampleInterface*    RigidBodySoftContactCreateFunc(struct CommonExampleOptions& options);
+
+
+#endif //RIGID_BODY_SOFT_CONTACT_H

src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h

@@ -39,6 +39,7 @@ enum btContactPointFlags
 {
 	BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED=1,
 	BT_CONTACT_FLAG_HAS_CONTACT_CFM=2,
+	BT_CONTACT_FLAG_HAS_CONTACT_ERP=4,
 };
 
 /// ManifoldContactPoint collects and maintains persistent contactpoints.
@@ -55,6 +56,7 @@ class btManifoldPoint
 				m_contactMotion1(0.f),
 				m_contactMotion2(0.f),
 				m_contactCFM(0.f),
+				m_contactERP(0.f),
 				m_frictionCFM(0.f),
 				m_lifeTime(0)
 			{
@@ -78,6 +80,7 @@ class btManifoldPoint
 					m_contactMotion1(0.f),
 					m_contactMotion2(0.f),
 					m_contactCFM(0.f),
+					m_contactERP(0.f),
 					m_frictionCFM(0.f),
 					m_lifeTime(0)
 			{
@@ -114,6 +117,8 @@ class btManifoldPoint
 			btScalar		m_contactMotion1;
 			btScalar		m_contactMotion2;
 			btScalar		m_contactCFM;
+			btScalar		m_contactERP;
+
 			btScalar		m_frictionCFM;
 
 			int				m_lifeTime;//lifetime of the contactpoint in frames

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -780,6 +780,7 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
 			btScalar cfm = (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM)?cp.m_contactCFM:infoGlobal.m_globalCfm;
 			cfm *= invTimeStep;
 
+			btScalar erp = (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)?cp.m_contactERP:infoGlobal.m_erp2;
 
 			btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
 			solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
@@ -888,12 +889,7 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
 					btScalar	velocityError = restitution - 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;

src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp

@@ -221,9 +221,15 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 	if (bodyB)
 		rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
 
-	relaxation = 1.f;
-
+	relaxation = infoGlobal.m_sor;
 	
+	btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep;
+	btScalar cfm = (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM)?cp.m_contactCFM:infoGlobal.m_globalCfm;
+	cfm *= invTimeStep;
+
+	btScalar erp = (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)?cp.m_contactERP:infoGlobal.m_erp2;
+
+
 
 
 	if (multiBodyA)
@@ -366,7 +372,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 
 		 
 
-		 btScalar d = denom0+denom1;
+		 btScalar d = denom0+denom1+cfm;
 		 if (d>SIMD_EPSILON)
 		 {
 			solverConstraint.m_jacDiagABInv = relaxation/(d);
@@ -477,12 +483,6 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 		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
 
-		btScalar erp = infoGlobal.m_erp2;
-		if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
-		{
-			erp = infoGlobal.m_erp;
-		}
-
 		if (penetration>0)
 		{
 			positionalError = 0;
@@ -522,7 +522,10 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 			solverConstraint.m_upperLimit = solverConstraint.m_friction;
 		}
 
-		solverConstraint.m_cfm = 0.f;			//why not use cfmSlip?
+		solverConstraint.m_cfm = cfm*solverConstraint.m_jacDiagABInv;
+		
+
+
 	}
 
 }