export contact friction/damping through URDF and API

convert from contact friction/damping to cfm/erp
btCollisionObject::setContactFrictionAndDamping

data/cube_no_friction.urdf

@@ -4,8 +4,8 @@
     <contact>
       <lateral_friction value="0.0"/>
       <rolling_friction value="0.0"/>
-      <contact_cfm value="0.0"/>
+      <stiffness value="300.0"/>
-      <contact_erp value="1.0"/>
+      <damping value="10.0"/>
     </contact>
     <inertial>
       <origin rpy="0 0 0" xyz="0 0 0"/>

data/cube_small.urdf

@@ -3,10 +3,8 @@
   <link name="baseLink">
     <contact>
       <lateral_friction value="1.0"/>
-      <rolling_friction value="0.3"/>
+      <rolling_friction value="0.01"/>
       <inertia_scaling value="3.0"/>
-      <contact_cfm value="0.0"/>
-      <contact_erp value="1.0"/>
     </contact>
     <inertial>
       <origin rpy="0 0 0" xyz="0 0 0"/>

data/cube_soft.urdf

@@ -0,0 +1,32 @@
+<?xml version="0.0" ?>
+<robot name="cube.urdf">
+  <link name="baseLink">
+    <contact>
+      <lateral_friction value="1.0"/>
+      <rolling_friction value="0.0"/>
+      <stiffness value="300"/>
+      <damping value="10"/>
+    </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value="1.0"/>
+       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+				<mesh filename="cube.obj" scale="1 1 1"/>
+      </geometry>
+       <material name="white">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+	 	<box size="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>
+

examples/Importers/ImportURDFDemo/URDF2Bullet.cpp

@@ -418,7 +418,10 @@ void ConvertURDF2BulletInternal(
 				{
 					col->setRollingFriction(contactInfo.m_rollingFriction);
 				}
-				
+				if ((contactInfo.m_flags & URDF_CONTACT_HAS_STIFFNESS_DAMPING)!=0)
+				{
+				    col->setContactStiffnessAndDamping(contactInfo.m_contactStiffness,contactInfo.m_contactDamping);
+				}
 
                 if (mbLinkIndex>=0) //???? double-check +/- 1
                 {

examples/Importers/ImportURDFDemo/URDFJointTypes.h

@@ -19,7 +19,8 @@ enum URDF_LinkContactFlags
 	URDF_CONTACT_HAS_ROLLING_FRICTION=2,
     URDF_CONTACT_HAS_INERTIA_SCALING=2,
     URDF_CONTACT_HAS_CONTACT_CFM=4,
-	URDF_CONTACT_HAS_CONTACT_ERP=8
+	URDF_CONTACT_HAS_CONTACT_ERP=8,
+	URDF_CONTACT_HAS_STIFFNESS_DAMPING=16,
 };
 
 struct URDFLinkContactInfo
@@ -29,6 +30,9 @@ struct URDFLinkContactInfo
     btScalar m_inertiaScaling;
 	btScalar m_contactCfm;
 	btScalar m_contactErp;
+	btScalar m_contactStiffness;
+	btScalar m_contactDamping;
+	
 	int m_flags;
 
 	URDFLinkContactInfo()
@@ -36,7 +40,9 @@ struct URDFLinkContactInfo
 		m_rollingFriction(0),
         m_inertiaScaling(1),
 		m_contactCfm(0),
-		m_contactErp(0)
+		m_contactErp(0),
+        m_contactStiffness(1e4),
+        m_contactDamping(1)
 	{
 		m_flags = URDF_CONTACT_HAS_LATERAL_FRICTION;
 	}

examples/Importers/ImportURDFDemo/UrdfParser.cpp

@@ -647,6 +647,7 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
 		}
           }
           
+          {
               TiXmlElement *rolling_xml = ci->FirstChildElement("rolling_friction");
               if (rolling_xml)
               {
@@ -668,6 +669,54 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
                   }
               }
           }
+
+           {
+           
+              TiXmlElement *stiffness_xml = ci->FirstChildElement("stiffness");
+              if (stiffness_xml)
+              {
+                  if (m_parseSDF)
+                  {
+                      link.m_contactInfo.m_contactStiffness = urdfLexicalCast<double>(stiffness_xml->GetText());
+                      link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_STIFFNESS_DAMPING;
+                  } else
+                  {
+                      if (!stiffness_xml->Attribute("value"))
+                      {
+                          logger->reportError("Link/contact: stiffness element must have value attribute");
+                          return false;
+                      }
+                      
+                      link.m_contactInfo.m_contactStiffness = urdfLexicalCast<double>(stiffness_xml->Attribute("value"));
+                      link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_STIFFNESS_DAMPING;
+                      
+                  }
+              }
+            }
+            {
+           
+              TiXmlElement *damping_xml = ci->FirstChildElement("damping");
+              if (damping_xml)
+              {
+                  if (m_parseSDF)
+                  {
+                      link.m_contactInfo.m_contactDamping = urdfLexicalCast<double>(damping_xml->GetText());
+                      link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_STIFFNESS_DAMPING;
+                  } else
+                  {
+                      if (!damping_xml->Attribute("value"))
+                      {
+                          logger->reportError("Link/contact: damping element must have value attribute");
+                          return false;
+                      }
+                      
+                      link.m_contactInfo.m_contactDamping = urdfLexicalCast<double>(damping_xml->Attribute("value"));
+                      link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_STIFFNESS_DAMPING;
+                      
+                  }
+              }
+            }
+	  }
 	}
 
   // Inertial (optional)

examples/MultiBody/MultiBodySoftContact.cpp

@@ -36,7 +36,6 @@ public:
 
 };
 
-extern ContactAddedCallback		gContactAddedCallback;
 
 
 MultiBodySoftContact::MultiBodySoftContact(struct GUIHelperInterface* helper)
@@ -47,17 +46,6 @@ m_once(true)
 
 MultiBodySoftContact::~MultiBodySoftContact()
 {
-    gContactAddedCallback = 0;
-}
-
-
-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; 
     
 }
 
@@ -66,7 +54,7 @@ static bool btMultiBodySoftContactCallback(btManifoldPoint& cp,	const btCollisio
 
 void MultiBodySoftContact::initPhysics()
 {
-	  gContactAddedCallback = btMultiBodySoftContactCallback;
+	  
     int upAxis = 2;
 
 	m_guiHelper->setUpAxis(upAxis);
@@ -109,12 +97,13 @@ void MultiBodySoftContact::initPhysics()
 			start.setOrigin(groundOrigin);
 		//	start.setRotation(groundOrn);
             btRigidBody* body =  createRigidBody(0,start,box);
+            
+            //setContactStiffnessAndDamping will enable compliant rigid body contact
+            body->setContactStiffnessAndDamping(300,10);
             btVector4 color = colors[curColor];
 			curColor++;
 			curColor&=3;
             m_guiHelper->createRigidBodyGraphicsObject(body,color);
-			int flags = body->getCollisionFlags();
-			body->setCollisionFlags(flags|btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
 
         }
 

examples/RigidBody/RigidBodySoftContact.cpp

@@ -29,7 +29,7 @@ subject to the following restrictions:
 #include "BulletDynamics/MLCPSolvers/btMLCPSolver.h"
 #include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
 
-extern ContactAddedCallback		gContactAddedCallback;
+
 
 
 struct RigidBodySoftContact : public CommonRigidBodyBase
@@ -40,7 +40,7 @@ struct RigidBodySoftContact : public CommonRigidBodyBase
 	}
 	virtual ~RigidBodySoftContact()
     {
-        gContactAddedCallback = 0;
+        
     }
 	virtual void initPhysics();
 	virtual void renderScene();
@@ -55,21 +55,12 @@ struct RigidBodySoftContact : public CommonRigidBodyBase
 };
 
 
-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);
 
@@ -120,8 +111,9 @@ void RigidBodySoftContact::initPhysics()
 	{
 		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);
+        body->setContactStiffnessAndDamping(300,10);
+		
 	}
 
 

examples/RoboticsLearning/GripperGraspExample.cpp

@@ -153,7 +153,7 @@ public:
                 
             }
             m_robotSim.setGravity(b3MakeVector3(0,0,-10));
-            //m_robotSim.setNumSimulationSubSteps(4);
+            m_robotSim.setNumSimulationSubSteps(4);
         }
 
         if ((m_options & eTWO_POINT_GRASP)!=0)

examples/RoboticsLearning/R2D2GraspExample.cpp

@@ -117,7 +117,7 @@ public:
 			b3RobotSimLoadFileArgs args("");
 			b3RobotSimLoadFileResults results;
 			{
-				args.m_fileName = "cube.urdf";
+				args.m_fileName = "cube_soft.urdf";
 				args.m_startPosition.setValue(0,0,2.5);
 				args.m_startOrientation.setEulerZYX(0,0.2,0);
 				m_robotSim.loadFile(args,results);

src/BulletCollision/CollisionDispatch/btCollisionObject.cpp

@@ -33,6 +33,8 @@ btCollisionObject::btCollisionObject()
 		m_friction(btScalar(0.5)),
 		m_restitution(btScalar(0.)),
 		m_rollingFriction(0.0f),
+		m_contactDamping(.1),
+		m_contactStiffness(1e4),
 		m_internalType(CO_COLLISION_OBJECT),
 		m_userObjectPointer(0),
 		m_userIndex2(-1),
@@ -92,6 +94,8 @@ const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* seriali
 	dataOut->m_deactivationTime = m_deactivationTime;
 	dataOut->m_friction = m_friction;
 	dataOut->m_rollingFriction = m_rollingFriction;
+	dataOut->m_contactDamping = m_contactDamping;
+	dataOut->m_contactStiffness = m_contactStiffness;
 	dataOut->m_restitution = m_restitution;
 	dataOut->m_internalType = m_internalType;
 	

src/BulletCollision/CollisionDispatch/btCollisionObject.h

@@ -86,6 +86,10 @@ protected:
 	btScalar		m_friction;
 	btScalar		m_restitution;
 	btScalar		m_rollingFriction;
+	btScalar		m_contactDamping;
+	btScalar		m_contactStiffness;
+	
+	
 
 	///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.
@@ -129,7 +133,8 @@ public:
 		CF_CUSTOM_MATERIAL_CALLBACK = 8,//this allows per-triangle material (friction/restitution)
 		CF_CHARACTER_OBJECT = 16,
 		CF_DISABLE_VISUALIZE_OBJECT = 32, //disable debug drawing
-		CF_DISABLE_SPU_COLLISION_PROCESSING = 64//disable parallel/SPU processing
+		CF_DISABLE_SPU_COLLISION_PROCESSING = 64,//disable parallel/SPU processing
+		CF_HAS_CONTACT_STIFFNESS_DAMPING = 128
 	};
 
 	enum	CollisionObjectTypes
@@ -319,6 +324,30 @@ public:
 		return m_rollingFriction;
 	}
 
+    void	setContactStiffnessAndDamping(btScalar stiffness, btScalar damping)
+	{
+		m_updateRevision++;
+		m_contactStiffness = stiffness;
+		m_contactDamping = damping;
+		
+		m_collisionFlags |=CF_HAS_CONTACT_STIFFNESS_DAMPING;
+		
+        //avoid divisions by zero...
+		if (m_contactStiffness< SIMD_EPSILON)
+        {
+            m_contactStiffness = SIMD_EPSILON;
+        }
+	}
+	
+	btScalar	getContactStiffness() const
+	{
+		return m_contactStiffness;
+	}
+	
+	btScalar	getContactDamping() const
+	{
+		return m_contactDamping;
+	}
     
 	///reserved for Bullet internal usage
 	int	getInternalType() const
@@ -541,6 +570,8 @@ struct	btCollisionObjectDoubleData
 	double					m_deactivationTime;
 	double					m_friction;
 	double					m_rollingFriction;
+	double                  m_contactDamping;
+	double                  m_contactStiffness;
 	double					m_restitution;
 	double					m_hitFraction; 
 	double					m_ccdSweptSphereRadius;
@@ -574,7 +605,8 @@ struct	btCollisionObjectFloatData
 	float					m_deactivationTime;
 	float					m_friction;
 	float					m_rollingFriction;
-
+    float                   m_contactDamping;
+    float                   m_contactStiffness;
 	float					m_restitution;
 	float					m_hitFraction; 
 	float					m_ccdSweptSphereRadius;

src/BulletCollision/CollisionDispatch/btManifoldResult.cpp

@@ -25,7 +25,7 @@ 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	btManifoldResult::calculateCombinedRollingFriction(const btCollisionObject* body0,const btCollisionObject* body1)
 {
 	btScalar friction = body0->getRollingFriction() * body1->getRollingFriction();
 
@@ -58,6 +58,22 @@ btScalar	btManifoldResult::calculateCombinedRestitution(const btCollisionObject*
 	return body0->getRestitution() * body1->getRestitution();
 }
 
+btScalar	btManifoldResult::calculateCombinedContactDamping(const btCollisionObject* body0,const btCollisionObject* body1)
+{
+    return body0->getContactDamping() + body1->getContactDamping();
+}
+
+btScalar	btManifoldResult::calculateCombinedContactStiffness(const btCollisionObject* body0,const btCollisionObject* body1)
+{
+    
+    btScalar s0 = body0->getContactStiffness();
+    btScalar s1 = body1->getContactStiffness();
+    
+    btScalar tmp0 = btScalar(1)/s0;
+    btScalar tmp1 = btScalar(1)/s1;
+    btScalar combinedStiffness = btScalar(1) / (tmp0+tmp1);
+    return combinedStiffness;
+}
 
 
 btManifoldResult::btManifoldResult(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
@@ -109,6 +125,15 @@ 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());
+	
+	if (    (m_body0Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING) ||
+            (m_body1Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING))
+    {
+        newPt.m_combinedContactDamping1 = calculateCombinedContactDamping(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
+        newPt.m_combinedContactStiffness1 = calculateCombinedContactStiffness(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
+        newPt.m_contactPointFlags |= BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING;
+    }
+	
 	btPlaneSpace1(newPt.m_normalWorldOnB,newPt.m_lateralFrictionDir1,newPt.m_lateralFrictionDir2);
 	
 

src/BulletCollision/CollisionDispatch/btManifoldResult.h

@@ -145,6 +145,9 @@ public:
 	/// in the future we can let the user override the methods to combine restitution and friction
 	static btScalar	calculateCombinedRestitution(const btCollisionObject* body0,const btCollisionObject* body1);
 	static btScalar	calculateCombinedFriction(const btCollisionObject* body0,const btCollisionObject* body1);
+	static btScalar calculateCombinedRollingFriction(const btCollisionObject* body0,const btCollisionObject* body1);
+	static btScalar calculateCombinedContactDamping(const btCollisionObject* body0,const btCollisionObject* body1);
+	static btScalar calculateCombinedContactStiffness(const btCollisionObject* body0,const btCollisionObject* body1);
 };
 
 #endif //BT_MANIFOLD_RESULT_H

src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h

@@ -40,6 +40,7 @@ enum btContactPointFlags
 	BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED=1,
 	BT_CONTACT_FLAG_HAS_CONTACT_CFM=2,
 	BT_CONTACT_FLAG_HAS_CONTACT_ERP=4,
+    BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING = 8,
 };
 
 /// ManifoldContactPoint collects and maintains persistent contactpoints.
@@ -116,8 +117,18 @@ class btManifoldPoint
 			btScalar		m_appliedImpulseLateral2;
 			btScalar		m_contactMotion1;
 			btScalar		m_contactMotion2;
+			
+			union
+			{
                 btScalar		m_contactCFM;
+                btScalar        m_combinedContactStiffness1;
+			};
+			
+			union
+			{
                 btScalar		m_contactERP;
+                btScalar        m_combinedContactDamping1;
+			};
 
 			btScalar		m_frictionCFM;
 

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -777,10 +777,35 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
 
 			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 = 1 /       ( dt * kp + kd )
+            //erp = dt * kp / ( dt * kp + kd )
+            
+            btScalar cfm = infoGlobal.m_globalCfm;
+            btScalar erp = infoGlobal.m_erp2;
+
+            if ((cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM) || (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP))
+            {
+                if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM)
+                    cfm  = cp.m_contactCFM;
+                if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)
+                    erp = cp.m_contactERP;                
+            } else
+            {
+                if (cp.m_contactPointFlags & BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING)
+                {
+                    btScalar denom = ( infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1 );
+                    if (denom < SIMD_EPSILON)
+                    {
+                        denom = SIMD_EPSILON;
+                    }
+                    cfm = btScalar(1) / denom; 
+                    erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom;
+                }
+            }
+            
             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);
@@ -1053,7 +1078,7 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 
 			if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
 			{
-//disabled: only a single rollingFriction per manifold
+                //only a single rollingFriction per manifold
 				//rollingFriction--;
 				if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
 				{
@@ -1068,6 +1093,9 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 					addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
 					btVector3 axis0,axis1;
 					btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
+					axis0.normalize();
+					axis1.normalize();
+					
 					applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
 					applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
 					applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);

src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp

@@ -299,7 +299,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstr
     
     
     //compute rhs and remaining solverConstraint fields
-    btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop;
+    btScalar penetration = isFriction? 0 : posError;
     
     btScalar rel_vel = 0.f;
     int ndofA  = 0;

src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp

@@ -224,12 +224,35 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
 	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 = 1 /       ( dt * kp + kd )
+    //erp = dt * kp / ( dt * kp + kd )
+    
+    btScalar cfm = infoGlobal.m_globalCfm;
+    btScalar erp = infoGlobal.m_erp2;
+
+    if ((cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM) || (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP))
+    {
+        if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM)
+            cfm  = cp.m_contactCFM;
+        if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)
+            erp = cp.m_contactERP;                
+    } else
+    {
+        if (cp.m_contactPointFlags & BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING)
+        {
+            btScalar denom = ( infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1 );
+            if (denom < SIMD_EPSILON)
+            {
+                denom = SIMD_EPSILON;
+            }
+            cfm = btScalar(1) / denom; 
+            erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom;
+        }
+    }
+    
     cfm *= invTimeStep;
 
-	btScalar erp = (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)?cp.m_contactERP:infoGlobal.m_erp2;
-
-
 
 
 	if (multiBodyA)
@@ -565,12 +588,6 @@ void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMult
     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)
@@ -713,7 +730,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMult
         
         
         
-        btScalar d = denom0+denom1+cfm;
+        btScalar d = denom0+denom1+infoGlobal.m_globalCfm;
         if (d>SIMD_EPSILON)
         {
             solverConstraint.m_jacDiagABInv = relaxation/(d);
@@ -731,7 +748,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMult
     
     
     btScalar restitution = 0.f;
-    btScalar penetration = isFriction? 0 : cp.getDistance()+infoGlobal.m_linearSlop;
+    btScalar penetration = isFriction? 0 : cp.getDistance();
     
     btScalar rel_vel = 0.f;
     int ndofA  = 0;
@@ -790,15 +807,9 @@ void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMult
         
         if (penetration>0)
         {
-            positionalError = 0;
             velocityError -= penetration / infoGlobal.m_timeStep;
-            
-        } else
-        {
-            positionalError = -penetration * erp/infoGlobal.m_timeStep;
         } 
         
-        btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
         btScalar velocityImpulse = velocityError*solverConstraint.m_jacDiagABInv;
         
         solverConstraint.m_rhs = velocityImpulse;
@@ -806,7 +817,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMult
         solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
         solverConstraint.m_upperLimit = solverConstraint.m_friction;
         
-        solverConstraint.m_cfm = cfm*solverConstraint.m_jacDiagABInv;
+        solverConstraint.m_cfm = infoGlobal.m_globalCfm*solverConstraint.m_jacDiagABInv;
         
         
         
@@ -951,45 +962,6 @@ void	btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 #define ENABLE_FRICTION
 #ifdef ENABLE_FRICTION
 			solverConstraint.m_frictionIndex = frictionIndex;
-//#define ROLLING_FRICTION
-#ifdef ROLLING_FRICTION
-	int rollingFriction=1;
-			btVector3 angVelA(0,0,0),angVelB(0,0,0);
-			if (mbA)
-				angVelA = mbA->getVelocityVector()>getLink(fcA->m_link).l>getAngularVelocity();
-			if (mbB)
-				angVelB = mbB->getAngularVelocity();
-			btVector3 relAngVel = angVelB-angVelA;
-
-			if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
-			{
-//disabled: only a single rollingFriction per manifold
-//rollingFriction--;
-				if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
-				{
-					relAngVel.normalize();
-					applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
-					applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
-					if (relAngVel.length()>0.001)
-						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);
-					applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
-					applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
-					applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
-					applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
-					if (axis0.length()>0.001)
-						addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-					if (axis1.length()>0.001)
-						addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
-		
-				}
-			}
-#endif //ROLLING_FRICTION
 
 			///Bullet has several options to set the friction directions
 			///By default, each contact has only a single friction direction that is recomputed automatically every frame