Merge pull request #568 from erwincoumans/master

allow soft (compliant) contact for btRigidBody and btMultiBody using ERP/CFM.

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"
+
+#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

examples/SharedMemory/PhysicsClientExample.cpp

@@ -445,8 +445,8 @@ void	PhysicsClientExample::initPhysics()
 	m_selectedBody = -1;
 	m_prevSelectedBody = -1;
 
-    //m_physicsClientHandle  = b3ConnectSharedMemory(m_sharedMemoryKey);
-	m_physicsClientHandle  = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY);
+    m_physicsClientHandle  = b3ConnectSharedMemory(m_sharedMemoryKey);
+	//m_physicsClientHandle  = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY);
 	//m_physicsClientHandle = b3ConnectPhysicsDirect();
 
     if (!b3CanSubmitCommand(m_physicsClientHandle))

src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h

@@ -35,7 +35,12 @@ typedef sce::PhysicsEffects::PfxConstraintRow btConstraintRow;
 	typedef btConstraintRow PfxConstraintRow;
 #endif //PFX_USE_FREE_VECTORMATH
 
-
+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.
 /// used to improve stability and performance of rigidbody dynamics response.
@@ -44,14 +49,15 @@ class btManifoldPoint
 		public:
 			btManifoldPoint()
 				:m_userPersistentData(0),
-				m_lateralFrictionInitialized(false),
-                m_appliedImpulse(0.f),
+				m_contactPointFlags(0),
+				m_appliedImpulse(0.f),
                 m_appliedImpulseLateral1(0.f),
 				m_appliedImpulseLateral2(0.f),
 				m_contactMotion1(0.f),
 				m_contactMotion2(0.f),
-				m_contactCFM1(0.f),
-				m_contactCFM2(0.f),
+				m_contactCFM(0.f),
+				m_contactERP(0.f),
+				m_frictionCFM(0.f),
 				m_lifeTime(0)
 			{
 			}
@@ -67,14 +73,15 @@ class btManifoldPoint
 					m_combinedRollingFriction(btScalar(0.)),
 					m_combinedRestitution(btScalar(0.)),
 					m_userPersistentData(0),
-					m_lateralFrictionInitialized(false),
-                    m_appliedImpulse(0.f),
+					m_contactPointFlags(0),
+					m_appliedImpulse(0.f),
                     m_appliedImpulseLateral1(0.f),
 					m_appliedImpulseLateral2(0.f),
 					m_contactMotion1(0.f),
 					m_contactMotion2(0.f),
-					m_contactCFM1(0.f),
-					m_contactCFM2(0.f),
+					m_contactCFM(0.f),
+					m_contactERP(0.f),
+					m_frictionCFM(0.f),
 					m_lifeTime(0)
 			{
 				
@@ -101,15 +108,18 @@ class btManifoldPoint
 			int			m_index1;
 				
 			mutable void*	m_userPersistentData;
-			bool			m_lateralFrictionInitialized;
-
+			//bool			m_lateralFrictionInitialized;
+			int				m_contactPointFlags;
+			
 			btScalar		m_appliedImpulse;
 			btScalar		m_appliedImpulseLateral1;
 			btScalar		m_appliedImpulseLateral2;
 			btScalar		m_contactMotion1;
 			btScalar		m_contactMotion2;
-			btScalar		m_contactCFM1;
-			btScalar		m_contactCFM2;
+			btScalar		m_contactCFM;
+			btScalar		m_contactERP;
+
+			btScalar		m_frictionCFM;
 
 			int				m_lifeTime;//lifetime of the contactpoint in frames
 			

src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h

@@ -163,7 +163,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_lateralFrictionInitialized = false;
+			m_pointCache[lastUsedIndex].m_contactPointFlags = 0;
 			m_pointCache[lastUsedIndex].m_appliedImpulseLateral1 = 0.f;
 			m_pointCache[lastUsedIndex].m_appliedImpulseLateral2 = 0.f;
 			m_pointCache[lastUsedIndex].m_lifeTime = 0;
@@ -190,7 +190,6 @@ public:
 		void* cache = m_pointCache[insertIndex].m_userPersistentData;
 		
 		m_pointCache[insertIndex] = newPoint;
-
 		m_pointCache[insertIndex].m_userPersistentData = cache;
 		m_pointCache[insertIndex].m_appliedImpulse = appliedImpulse;
 		m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1;

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -775,7 +775,12 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
 			//rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
 			//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;
 
 			btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
 			solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
@@ -802,7 +807,7 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
 					}
 #endif //COMPUTE_IMPULSE_DENOM
 
-					btScalar denom = relaxation/(denom0+denom1);
+					btScalar denom = relaxation/(denom0+denom1+cfm);
 					solverConstraint.m_jacDiagABInv = denom;
 				}
 
@@ -884,20 +889,16 @@ 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;
 
-						velocityError -= penetration / infoGlobal.m_timeStep;
+						velocityError -= penetration *invTimeStep;
 					} else
 					{
-						positionalError = -penetration * erp/infoGlobal.m_timeStep;
+						positionalError = -penetration * erp*invTimeStep;
+
 					}
 
 					btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
@@ -915,7 +916,7 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
 						solverConstraint.m_rhs = velocityImpulse;
 						solverConstraint.m_rhsPenetration = penetrationImpulse;
 					}
-					solverConstraint.m_cfm = 0.f;
+					solverConstraint.m_cfm = cfm*solverConstraint.m_jacDiagABInv;
 					solverConstraint.m_lowerLimit = 0;
 					solverConstraint.m_upperLimit = 1e10f;
 				}
@@ -1094,7 +1095,7 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 			///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2)
 			///this will give a conveyor belt effect
 			///
-			if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
+			if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags&BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED))
 			{
 				cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
 				btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
@@ -1132,16 +1133,16 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 
 					if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
 					{
-						cp.m_lateralFrictionInitialized = true;
+						cp.m_contactPointFlags|=BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED;
 					}
 				}
 
 			} else
 			{
-				addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1);
+				addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_frictionCFM);
 
 				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);
+					addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_frictionCFM);
 
 			}
 			setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);

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;
+		
+
+
 	}
 
 }
@@ -685,7 +688,7 @@ void	btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 			///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2)
 			///this will give a conveyor belt effect
 			///
-			if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
+			if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags&BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED))
 			{/*
 				cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
 				btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
@@ -724,16 +727,16 @@ void	btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 
 					if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
 					{
-						cp.m_lateralFrictionInitialized = true;
+						cp.m_contactPointFlags|=BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED;
 					}
 				}
 
 			} else
 			{
-				addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal,cp.m_contactMotion1, cp.m_contactCFM1);
+				addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,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_contactCFM2);
+					addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation, infoGlobal,cp.m_contactMotion2, cp.m_frictionCFM);
 
 				//setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
 				//todo:

src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp

@@ -22,8 +22,7 @@ subject to the following restrictions:
 
 btMLCPSolver::btMLCPSolver(	 btMLCPSolverInterface* solver)
 :m_solver(solver),
-m_fallback(0),
-m_cfm(0.000001)//0.0000001
+m_fallback(0)
 {
 }
 
@@ -436,7 +435,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
 		// add cfm to the diagonal of m_A
 		for ( int i=0; i<m_A.rows(); ++i) 
 		{
-			m_A.setElem(i,i,m_A(i,i)+ m_cfm / infoGlobal.m_timeStep);
+			m_A.setElem(i,i,m_A(i,i)+ infoGlobal.m_globalCfm/ infoGlobal.m_timeStep);
 		}
 	}
 				   
@@ -564,7 +563,7 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
 		// add cfm to the diagonal of m_A
 		for ( int i=0; i<m_A.rows(); ++i) 
 		{
-			m_A.setElem(i,i,m_A(i,i)+ m_cfm / infoGlobal.m_timeStep);
+			m_A.setElem(i,i,m_A(i,i)+ infoGlobal.m_globalCfm / infoGlobal.m_timeStep);
 		}
 	}
 

src/BulletDynamics/MLCPSolvers/btMLCPSolver.h

@@ -42,7 +42,6 @@ protected:
 	btAlignedObjectArray<btSolverConstraint*>	m_allConstraintPtrArray;
 	btMLCPSolverInterface* m_solver;
 	int m_fallback;
-	btScalar m_cfm;
 
 	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);
@@ -73,15 +72,6 @@ public:
 		m_fallback = num;
 	}
 
-	btScalar	getCfm() const
-	{
-		return m_cfm;
-	}
-	void setCfm(btScalar cfm)
-	{
-		m_cfm = cfm;
-	}
-
 	virtual btConstraintSolverType	getSolverType() const
 	{
 		return BT_MLCP_SOLVER;