Merge pull request #2577 from xhan0619/master

Add position error into deformable vs. rigid contact

examples/DeformableDemo/DeformableMultibody.cpp

@@ -206,15 +206,15 @@ void DeformableMultibody::initPhysics()
 
         psb->getCollisionShape()->setMargin(0.25);
         psb->generateBendingConstraints(2);
-        psb->setTotalMass(5);
+        psb->setTotalMass(1);
         psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
         psb->m_cfg.kCHR = 1; // collision hardness with rigid body
-        psb->m_cfg.kDF = .1;
+        psb->m_cfg.kDF = 2;
         psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
         psb->setCollisionFlags(0);
         getDeformableDynamicsWorld()->addSoftBody(psb);
 
-        btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(2, 0.01, false);
+        btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(30, 1, true);
         getDeformableDynamicsWorld()->addForce(psb, mass_spring);
         m_forces.push_back(mass_spring);
         

examples/DeformableDemo/DeformableRigid.cpp

@@ -219,7 +219,7 @@ void DeformableRigid::initPhysics()
         psb->setTotalMass(1);
         psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
         psb->m_cfg.kCHR = 1; // collision hardness with rigid body
-        psb->m_cfg.kDF = 2;
+        psb->m_cfg.kDF = .4;
         psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
         getDeformableDynamicsWorld()->addSoftBody(psb);
         

examples/DeformableDemo/Pinch.cpp

@@ -231,7 +231,7 @@ void Pinch::initPhysics()
     btVector3 gravity = btVector3(0, -10, 0);
 	m_dynamicsWorld->setGravity(gravity);
     getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
-    
+	getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(0.25);
     getDeformableDynamicsWorld()->setSolverCallback(dynamics);
 	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
 

examples/RoboticsLearning/GripperGraspExample.cpp

@@ -425,12 +425,12 @@ public:
 
 			m_robotSim.setGravity(btVector3(0, 0, -10));
 			b3RobotSimulatorLoadDeformableBodyArgs args(2, .01, 0.006);
-			args.m_springElasticStiffness = .1;
+			args.m_springElasticStiffness = 1;
-			args.m_springDampingStiffness = .0004;
+			args.m_springDampingStiffness = .01;
-			args.m_springBendingStiffness = 1;
+			args.m_springBendingStiffness = .1;
-			args.m_frictionCoeff = 1;
+			args.m_frictionCoeff = 10;
 			args.m_useSelfCollision = false;
-//			args.m_useFaceContact = true;
+			args.m_useFaceContact = true;
 			args.m_useBendingSprings = true;
 			args.m_startPosition.setValue(0, 0, 0);
 			args.m_startOrientation.setValue(0, 0, 1, 1);
@@ -476,7 +476,7 @@ public:
 			revoluteJoint2.m_jointType = ePoint2PointType;
 			m_robotSim.createConstraint(0, 2, 0, 4, &revoluteJoint1);
 			m_robotSim.createConstraint(0, 3, 0, 6, &revoluteJoint2);
-			m_robotSim.setNumSimulationSubSteps(8);
+			m_robotSim.setNumSimulationSubSteps(2);
         }
 
 		if ((m_options & eSOFTBODY_MULTIBODY_COUPLING) != 0)

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -8153,7 +8153,7 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
                 {
                     spring_bending_stiffness = clientCmd.m_loadSoftBodyArguments.m_springBendingStiffness;
                 }
-				btDeformableLagrangianForce* springForce = new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, false, spring_bending_stiffness);
+				btDeformableLagrangianForce* springForce = new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, true, spring_bending_stiffness);
 				deformWorld->addForce(psb, springForce);
 				m_data->m_lf.push_back(springForce);
 			}

src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp

@@ -139,7 +139,12 @@ public:
 
 		if (TestAabbAgainstAabb2(aabbMin0, aabbMax0, aabbMin1, aabbMax1))
 		{
-			btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap, childShape, m_compoundColObjWrap->getCollisionObject(), newChildWorldTrans, childTrans, -1, index);
+			btTransform preTransform = childTrans;
+			if (this->m_compoundColObjWrap->m_preTransform)
+			{
+				preTransform = preTransform *(*(this->m_compoundColObjWrap->m_preTransform));
+			}
+			btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap, childShape, m_compoundColObjWrap->getCollisionObject(), newChildWorldTrans, preTransform, -1, index);
 
 			btCollisionAlgorithm* algo = 0;
 			bool allocatedAlgorithm = false;

src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h

@@ -82,7 +82,7 @@ struct btContactSolverInfo : public btContactSolverInfoData
 		m_numIterations = 10;
 		m_erp = btScalar(0.2);
 		m_erp2 = btScalar(0.2);
-        m_deformable_erp = btScalar(0.);
+		m_deformable_erp = btScalar(0.3);
 		m_globalCfm = btScalar(0.);
 		m_frictionERP = btScalar(0.2);  //positional friction 'anchors' are disabled by default
 		m_frictionCFM = btScalar(0.);

src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp

@@ -186,9 +186,9 @@ void btDeformableBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack
 }
 
 //set constraints as projections
-void btDeformableBackwardEulerObjective::setConstraints()
+void btDeformableBackwardEulerObjective::setConstraints(const btContactSolverInfo& infoGlobal)
 {
-    m_projection.setConstraints();
+    m_projection.setConstraints(infoGlobal);
 }
 
 void btDeformableBackwardEulerObjective::applyDynamicFriction(TVStack& r)

src/BulletSoftBody/btDeformableBackwardEulerObjective.h

@@ -79,7 +79,7 @@ public:
     void updateVelocity(const TVStack& dv);
     
     //set constraints as projections
-    void setConstraints();
+    void setConstraints(const btContactSolverInfo& infoGlobal);
     
     // update the projections and project the residual
     void project(TVStack& r)

src/BulletSoftBody/btDeformableBodySolver.cpp

@@ -228,16 +228,16 @@ void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody
     m_objective->reinitialize(nodeUpdated, dt);
 }
 
-void btDeformableBodySolver::setConstraints()
+void btDeformableBodySolver::setConstraints(const btContactSolverInfo& infoGlobal)
 {
     BT_PROFILE("setConstraint");
-    m_objective->setConstraints();
+    m_objective->setConstraints(infoGlobal);
 }
 
-btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies)
+btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal)
 {
     BT_PROFILE("solveContactConstraints");
-    btScalar maxSquaredResidual = m_objective->m_projection.update(deformableBodies,numDeformableBodies);
+    btScalar maxSquaredResidual = m_objective->m_projection.update(deformableBodies,numDeformableBodies, infoGlobal);
     return maxSquaredResidual;
 }
 

src/BulletSoftBody/btDeformableBodySolver.h

@@ -65,7 +65,7 @@ public:
     virtual void solveDeformableConstraints(btScalar solverdt);
     
     // solve the contact between deformable and rigid as well as among deformables
-    btScalar solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies);
+    btScalar solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal);
     
     // solve the position error  between deformable and rigid as well as among deformables;
     btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal);
@@ -77,7 +77,7 @@ public:
     void reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt);
     
     // set up contact constraints
-    void setConstraints();
+    void setConstraints(const btContactSolverInfo& infoGlobal);
     
     // add in elastic forces and gravity to obtain v_{n+1}^* and calls predictDeformableMotion
     virtual void predictMotion(btScalar solverdt);

src/BulletSoftBody/btDeformableContactConstraint.cpp

@@ -15,9 +15,9 @@
 
 #include "btDeformableContactConstraint.h"
 /* ================   Deformable Node Anchor   =================== */
-btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& a)
+btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& a, const btContactSolverInfo& infoGlobal)
 : m_anchor(&a)
-, btDeformableContactConstraint(a.m_cti.m_normal)
+, btDeformableContactConstraint(a.m_cti.m_normal, infoGlobal)
 {
 }
 
@@ -79,7 +79,7 @@ btVector3 btDeformableNodeAnchorConstraint::getVa() const
     return va;
 }
 
-btScalar btDeformableNodeAnchorConstraint::solveConstraint()
+btScalar btDeformableNodeAnchorConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
 {
     const btSoftBody::sCti& cti = m_anchor->m_cti;
     btVector3 va = getVa();
@@ -134,13 +134,13 @@ void btDeformableNodeAnchorConstraint::applyImpulse(const btVector3& impulse)
 }
 
 /* ================   Deformable vs. Rigid   =================== */
-btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c)
+btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal)
 : m_contact(&c)
-, btDeformableContactConstraint(c.m_cti.m_normal)
+, btDeformableContactConstraint(c.m_cti.m_normal, infoGlobal)
 {
     m_total_normal_dv.setZero();
     m_total_tangent_dv.setZero();
-    // penetration is non-positive. The magnitude of penetration is the depth of penetration.
+    // The magnitude of penetration is the depth of penetration.
     m_penetration = btMin(btScalar(0),c.m_cti.m_offset);
 }
 
@@ -206,16 +206,16 @@ btVector3 btDeformableRigidContactConstraint::getVa() const
     return va;
 }
 
-btScalar btDeformableRigidContactConstraint::solveConstraint()
+btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
 {
     const btSoftBody::sCti& cti = m_contact->m_cti;
     btVector3 va = getVa();
     btVector3 vb = getVb();
     btVector3 vr = vb - va;
-    const btScalar dn = btDot(vr, cti.m_normal);
+    const btScalar dn = btDot(vr, cti.m_normal) + m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep;
     // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
     btScalar residualSquare = dn*dn;
-    btVector3 impulse = m_contact->m_c0 * vr;
+    btVector3 impulse = m_contact->m_c0 * (vr + m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep * cti.m_normal) ;
     const btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * dn);
     btVector3 impulse_tangent = impulse - impulse_normal;
     btVector3 old_total_tangent_dv = m_total_tangent_dv;
@@ -256,6 +256,8 @@ btScalar btDeformableRigidContactConstraint::solveConstraint()
     impulse = impulse_normal + impulse_tangent;
     // apply impulse to deformable nodes involved and change their velocities
     applyImpulse(impulse);
+	if (residualSquare < 1e-7)
+		return residualSquare;
     // apply impulse to the rigid/multibodies involved and change their velocities
     if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
     {
@@ -319,9 +321,9 @@ btScalar btDeformableRigidContactConstraint::solveSplitImpulse(const btContactSo
 }
 
 /* ================   Node vs. Rigid   =================== */
-btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact)
+btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal)
     : m_node(contact.m_node)
-    , btDeformableRigidContactConstraint(contact)
+    , btDeformableRigidContactConstraint(contact, infoGlobal)
     {
     }
 
@@ -357,9 +359,9 @@ void btDeformableNodeRigidContactConstraint::applySplitImpulse(const btVector3&
 };
 
 /* ================   Face vs. Rigid   =================== */
-btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact)
+btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal)
 : m_face(contact.m_face)
-, btDeformableRigidContactConstraint(contact)
+, btDeformableRigidContactConstraint(contact, infoGlobal)
 {
 }
 
@@ -412,15 +414,56 @@ void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impul
     if (im2 > 0)
         v2 -= dv * contact->m_weights[2];
 
+	btScalar relaxation = 1./btScalar(m_infoGlobal->m_numIterations);
+	btScalar m01 = (relaxation/(im0 + im1));
+	btScalar m02 = (relaxation/(im0 + im2));
+	btScalar m12 = (relaxation/(im1 + im2));
+	#ifdef USE_STRAIN_RATE_LIMITING
+	// apply strain limiting to prevent the new velocity to change the current length of the edge by more than 1%.
+	btScalar p = 0.01;
+	btVector3& x0 = face->m_n[0]->m_x;
+	btVector3& x1 = face->m_n[1]->m_x;
+	btVector3& x2 = face->m_n[2]->m_x;
+	const btVector3 x_diff[3] = {x1-x0, x2-x0, x2-x1};
+	const btVector3 v_diff[3] = {v1-v0, v2-v0, v2-v1};
+	btVector3 u[3];
+	btScalar x_diff_dot_u, dn[3];
+	btScalar dt = m_infoGlobal->m_timeStep;
+	for (int i = 0; i < 3; ++i)
+	{
+		btScalar x_diff_norm = x_diff[i].safeNorm();
+		btScalar x_diff_norm_new = (x_diff[i] + v_diff[i] * dt).safeNorm();
+		btScalar strainRate = x_diff_norm_new/x_diff_norm;
+		u[i] = v_diff[i];
+		u[i].safeNormalize();
+		if (x_diff_norm == 0 || (1-p <= strainRate && strainRate <= 1+p))
+		{
+			dn[i] = 0;
+			continue;
+		}
+		x_diff_dot_u = btDot(x_diff[i], u[i]);
+		btScalar s;
+		if (1-p > strainRate)
+		{
+			s = 1/dt * (-x_diff_dot_u - btSqrt(x_diff_dot_u*x_diff_dot_u + (p*p-2*p) * x_diff_norm * x_diff_norm));
+		}
+		else
+		{
+			s = 1/dt * (-x_diff_dot_u + btSqrt(x_diff_dot_u*x_diff_dot_u + (p*p+2*p) * x_diff_norm * x_diff_norm));
+		}
+		//		x_diff_norm_new = (x_diff[i] + s * u[i] * dt).safeNorm();
+		//		strainRate = x_diff_norm_new/x_diff_norm;
+		dn[i] = s - v_diff[i].safeNorm();
+	}
+	btVector3 dv0 = im0 * (m01 * u[0]*(-dn[0]) + m02 * u[1]*-(dn[1]));
+	btVector3 dv1 = im1 * (m01 * u[0]*(dn[0]) + m12 * u[2]*(-dn[2]));
+	btVector3 dv2 = im2 * (m12 * u[2]*(dn[2]) + m02 * u[1]*(dn[1]));
+#else
 	// apply strain limiting to prevent undamped modes
-    btScalar m01 = (btScalar(1)/(im0 + im1));
-    btScalar m02 = (btScalar(1)/(im0 + im2));
-    btScalar m12 = (btScalar(1)/(im1 + im2));
-    
 	btVector3 dv0 = im0 * (m01 * (v1-v0) + m02 * (v2-v0));
 	btVector3 dv1 = im1 * (m01 * (v0-v1) + m12 * (v2-v1));
 	btVector3 dv2 = im2 * (m12 * (v1-v2) + m02 * (v0-v2));
-    
+#endif
 	v0 += dv0;
 	v1 += dv1;
 	v2 += dv2;
@@ -447,11 +490,11 @@ void btDeformableFaceRigidContactConstraint::applySplitImpulse(const btVector3&
 }
 
 /* ================   Face vs. Node   =================== */
-btDeformableFaceNodeContactConstraint::btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact)
+btDeformableFaceNodeContactConstraint::btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact, const btContactSolverInfo& infoGlobal)
 : m_node(contact.m_node)
 , m_face(contact.m_face)
 , m_contact(&contact)
-, btDeformableContactConstraint(contact.m_normal)
+, btDeformableContactConstraint(contact.m_normal, infoGlobal)
 {
     m_total_normal_dv.setZero();
     m_total_tangent_dv.setZero();
@@ -487,7 +530,7 @@ btVector3 btDeformableFaceNodeContactConstraint::getDv(const btSoftBody::Node* n
     return dv * contact->m_weights[2];
 }
 
-btScalar btDeformableFaceNodeContactConstraint::solveConstraint()
+btScalar btDeformableFaceNodeContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal)
 {
     btVector3 va = getVa();
     btVector3 vb = getVb();

src/BulletSoftBody/btDeformableContactConstraint.h

@@ -24,31 +24,33 @@ public:
     // True if the friction is static
     // False if the friction is dynamic
     bool m_static;
+	const btContactSolverInfo* m_infoGlobal;
 
 	// normal of the contact
 	btVector3 m_normal;
 
-    btDeformableContactConstraint(const btVector3& normal): m_static(false), m_normal(normal)
+	btDeformableContactConstraint(const btVector3& normal, const btContactSolverInfo& infoGlobal): m_static(false), m_normal(normal), m_infoGlobal(&infoGlobal)
 	{
 	}
 
-    btDeformableContactConstraint(bool isStatic, const btVector3& normal): m_static(isStatic), m_normal(normal)
+	btDeformableContactConstraint(bool isStatic, const btVector3& normal, const btContactSolverInfo& infoGlobal): m_static(isStatic), m_normal(normal), m_infoGlobal(&infoGlobal)
 	{
 	}
 	
+	btDeformableContactConstraint(){}
+
 	btDeformableContactConstraint(const btDeformableContactConstraint& other)
 	: m_static(other.m_static)
 	, m_normal(other.m_normal)
+	, m_infoGlobal(other.m_infoGlobal)
 	{
-        
 	}
-    btDeformableContactConstraint(){}
 
     virtual ~btDeformableContactConstraint(){}
     
     // solve the constraint with inelastic impulse and return the error, which is the square of normal component of velocity diffrerence
     // the constraint is solved by calculating the impulse between object A and B in the contact and apply the impulse to both objects involved in the contact
-    virtual btScalar solveConstraint() = 0;
+    virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal) = 0;
     
     // solve the position error by applying an inelastic impulse that changes only the position (not velocity)
     virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal) = 0;
@@ -79,22 +81,19 @@ class btDeformableStaticConstraint : public btDeformableContactConstraint
 public:
     const btSoftBody::Node* m_node;
     
-    btDeformableStaticConstraint(){}
+    btDeformableStaticConstraint(const btSoftBody::Node* node, const btContactSolverInfo& infoGlobal): m_node(node), btDeformableContactConstraint(false, btVector3(0,0,0), infoGlobal)
-    
-    btDeformableStaticConstraint(const btSoftBody::Node* node): m_node(node), btDeformableContactConstraint(false, btVector3(0,0,0))
     {
     }
-    
+	btDeformableStaticConstraint(){}
     btDeformableStaticConstraint(const btDeformableStaticConstraint& other)
     : m_node(other.m_node)
     , btDeformableContactConstraint(other)
     {
-        
     }
     
     virtual ~btDeformableStaticConstraint(){}
     
-    virtual btScalar solveConstraint()
+    virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal)
     {
         return 0;
     }
@@ -131,13 +130,13 @@ class btDeformableNodeAnchorConstraint : public btDeformableContactConstraint
 public:
     const btSoftBody::DeformableNodeRigidAnchor* m_anchor;
 	
-    btDeformableNodeAnchorConstraint(){}
+    btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& c, const btContactSolverInfo& infoGlobal);
-    btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& c);
     btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other);
+	btDeformableNodeAnchorConstraint(){}
     virtual ~btDeformableNodeAnchorConstraint()
     {
     }
-    virtual btScalar solveConstraint();
+    virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
     virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal)
     {
         // todo xuchenhan@
@@ -170,9 +169,9 @@ public:
     btScalar m_penetration;
     const btSoftBody::DeformableRigidContact* m_contact;
 	
-    btDeformableRigidContactConstraint(){}
+    btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c, const btContactSolverInfo& infoGlobal);
-    btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c);
     btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other);
+	btDeformableRigidContactConstraint(){}
     virtual ~btDeformableRigidContactConstraint()
     {
     }
@@ -180,7 +179,7 @@ public:
     // object A is the rigid/multi body, and object B is the deformable node/face
     virtual btVector3 getVa() const;
     
-    virtual btScalar solveConstraint();
+    virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
     
     virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal);
     
@@ -198,10 +197,9 @@ public:
     // the deformable node in contact
     const btSoftBody::Node* m_node;
 	
-    btDeformableNodeRigidContactConstraint(){}
+    btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact, const btContactSolverInfo& infoGlobal);
-    btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact);
     btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other);
-    
+	btDeformableNodeRigidContactConstraint(){}
     virtual ~btDeformableNodeRigidContactConstraint()
     {
     }
@@ -228,10 +226,9 @@ class btDeformableFaceRigidContactConstraint : public btDeformableRigidContactCo
 {
 public:
     const btSoftBody::Face* m_face;
-    btDeformableFaceRigidContactConstraint(){}
+    btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal);
-    btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact);
     btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other);
-    
+	btDeformableFaceRigidContactConstraint(){}
     virtual ~btDeformableFaceRigidContactConstraint()
     {
     }
@@ -263,13 +260,11 @@ public:
     btVector3 m_total_normal_dv;
     btVector3 m_total_tangent_dv;
     
+    btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact, const btContactSolverInfo& infoGlobal);
 	btDeformableFaceNodeContactConstraint(){}
-    
-    btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact);
-    
     virtual ~btDeformableFaceNodeContactConstraint(){}
     
-    virtual btScalar solveConstraint();
+    virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal);
     
     virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal)
     {

src/BulletSoftBody/btDeformableContactProjection.cpp

@@ -17,7 +17,7 @@
 #include "btDeformableMultiBodyDynamicsWorld.h"
 #include <algorithm>
 #include <cmath>
-btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies,int numDeformableBodies)
+btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal)
 {
 	btScalar residualSquare = 0;
 	for (int i = 0; i < numDeformableBodies; ++i)
@@ -32,25 +32,25 @@ btScalar btDeformableContactProjection::update(btCollisionObject** deformableBod
 			for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k)
 			{
 				btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[j][k];
-				btScalar localResidualSquare = constraint.solveConstraint();
+				btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
 				residualSquare = btMax(residualSquare, localResidualSquare);
 			}
 			for (int k = 0; k < m_nodeAnchorConstraints[j].size(); ++k)
 			{
 				btDeformableNodeAnchorConstraint& constraint = m_nodeAnchorConstraints[j][k];
-				btScalar localResidualSquare = constraint.solveConstraint();
+				btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
 				residualSquare = btMax(residualSquare, localResidualSquare);
 			}
 			for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k)
 			{
 				btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k];
-				btScalar localResidualSquare = constraint.solveConstraint();
+				btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
 				residualSquare = btMax(residualSquare, localResidualSquare);
 			}
 			for (int k = 0; k < m_deformableConstraints[j].size(); ++k)
 			{
 				btDeformableFaceNodeContactConstraint& constraint = m_deformableConstraints[j][k];
-				btScalar localResidualSquare = constraint.solveConstraint();
+				btScalar localResidualSquare = constraint.solveConstraint(infoGlobal);
 				residualSquare = btMax(residualSquare, localResidualSquare);
 			}
 		}
@@ -108,7 +108,7 @@ btScalar btDeformableContactProjection::solveSplitImpulse(const btContactSolverI
 	return residualSquare;
 }
 
-void btDeformableContactProjection::setConstraints()
+void btDeformableContactProjection::setConstraints(const btContactSolverInfo& infoGlobal)
 {
 	BT_PROFILE("setConstraints");
 	for (int i = 0; i < m_softBodies.size(); ++i)
@@ -124,7 +124,7 @@ void btDeformableContactProjection::setConstraints()
 		{
 			if (psb->m_nodes[j].m_im == 0)
 			{
-				btDeformableStaticConstraint static_constraint(&psb->m_nodes[j]);
+				btDeformableStaticConstraint static_constraint(&psb->m_nodes[j], infoGlobal);
 				m_staticConstraints[i].push_back(static_constraint);
 			}
 		}
@@ -139,7 +139,7 @@ void btDeformableContactProjection::setConstraints()
 				continue;
 			}
 			anchor.m_c1 = anchor.m_cti.m_colObj->getWorldTransform().getBasis() * anchor.m_local;
-			btDeformableNodeAnchorConstraint constraint(anchor);
+			btDeformableNodeAnchorConstraint constraint(anchor, infoGlobal);
 			m_nodeAnchorConstraints[i].push_back(constraint);
 		}
 		
@@ -152,7 +152,7 @@ void btDeformableContactProjection::setConstraints()
 			{
 				continue;
 			}
-			btDeformableNodeRigidContactConstraint constraint(contact);
+			btDeformableNodeRigidContactConstraint constraint(contact, infoGlobal);
 			btVector3 va = constraint.getVa();
 			btVector3 vb = constraint.getVb();
 			const btVector3 vr = vb - va;
@@ -173,7 +173,7 @@ void btDeformableContactProjection::setConstraints()
 			{
 				continue;
 			}
-			btDeformableFaceRigidContactConstraint constraint(contact);
+			btDeformableFaceRigidContactConstraint constraint(contact, infoGlobal);
 			btVector3 va = constraint.getVa();
 			btVector3 vb = constraint.getVb();
 			const btVector3 vr = vb - va;
@@ -190,7 +190,7 @@ void btDeformableContactProjection::setConstraints()
 		{
 			const btSoftBody::DeformableFaceNodeContact& contact = psb->m_faceNodeContacts[j];
 
-			btDeformableFaceNodeContactConstraint constraint(contact);
+			btDeformableFaceNodeContactConstraint constraint(contact, infoGlobal);
 			btVector3 va = constraint.getVa();
 			btVector3 vb = constraint.getVb();
 			const btVector3 vr = vb - va;

src/BulletSoftBody/btDeformableContactProjection.h

@@ -72,13 +72,13 @@ public:
     virtual void applyDynamicFriction(TVStack& f);
     
     // update and solve the constraints
-    virtual btScalar update(btCollisionObject** deformableBodies,int numDeformableBodies);
+    virtual btScalar update(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal);
     
     // solve the position error using split impulse
     virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal);
     
     // Add constraints to m_constraints. In addition, the constraints that each vertex own are recorded in m_constraintsDict.
-    virtual void setConstraints();
+    virtual void setConstraints(const btContactSolverInfo& infoGlobal);
     
     // Set up projections for each vertex by adding the projection direction to
     virtual void setProjection();

src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp

@@ -32,7 +32,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b
             m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
             // solver body velocity -> rigid body velocity
             solverBodyWriteBack(infoGlobal);
-            btScalar deformableResidual = m_deformableSolver->solveContactConstraints(deformableBodies,numDeformableBodies);
+            btScalar deformableResidual = m_deformableSolver->solveContactConstraints(deformableBodies,numDeformableBodies, infoGlobal);
             // update rigid body velocity in rigid/deformable contact
             m_leastSquaresResidual = btMax(m_leastSquaresResidual, deformableResidual);
             // solver body velocity <- rigid body velocity
@@ -112,7 +112,7 @@ void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseI
     if (infoGlobal.m_splitImpulse)
     {
         {
-            m_deformableSolver->splitImpulseSetup(infoGlobal);
+//            m_deformableSolver->splitImpulseSetup(infoGlobal);
             for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++)
             {
                 btScalar leastSquaresResidual = 0.f;
@@ -127,8 +127,8 @@ void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseI
                         leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
                     }
                     // solve the position correction between deformable and rigid/multibody
-                    btScalar residual = m_deformableSolver->solveSplitImpulse(infoGlobal);
+//                    btScalar residual = m_deformableSolver->solveSplitImpulse(infoGlobal);
-                    leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
+//                    leastSquaresResidual = btMax(leastSquaresResidual, residual * residual);
                 }
                 if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration >= (infoGlobal.m_numIterations - 1))
                 {

src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp

@@ -285,7 +285,7 @@ void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep)
 void btDeformableMultiBodyDynamicsWorld::setupConstraints()
 {
     // set up constraints between multibody and deformable bodies
-    m_deformableBodySolver->setConstraints();
+    m_deformableBodySolver->setConstraints(m_solverInfo);
     
     // set up constraints among multibodies
     {

src/BulletSoftBody/btSoftBody.cpp

@@ -53,6 +53,7 @@ btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btV
 		n.m_material = pm;
 	}
 	updateBounds();
+	setCollisionQuadrature(3);
 }
 
 btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo)
@@ -2403,10 +2404,9 @@ bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWr
     const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject();
     // use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect
     // but resolve contact at x_n
-//    btTransform wtr = (predict) ?
+    btTransform wtr = (predict) ?
-//    (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
+    (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
-//                 : colObjWrap->getWorldTransform();
+                 : colObjWrap->getWorldTransform();
-    const btTransform& wtr = colObjWrap->getWorldTransform();
 	btScalar dst =
 		m_worldInfo->m_sparsesdf.Evaluate(
 			wtr.invXform(x),
@@ -2457,10 +2457,9 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
     btTransform wtr = (predict) ?
     (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
     : colObjWrap->getWorldTransform();
-//    const btTransform& wtr = colObjWrap->getWorldTransform();
     btScalar dst;
     
-//#define USE_QUADRATURE 1
+#define USE_QUADRATURE 1
 //#define CACHE_PREV_COLLISION
     
     // use the contact position of the previous collision
@@ -2476,6 +2475,7 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
                                           nrm,
                                           margin);
         nrm = wtr.getBasis() * nrm;
+        cti.m_colObj = colObjWrap->getCollisionObject();
         // use cached contact point
     }
     else
@@ -2492,10 +2492,11 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
         contact_point = results.witnesses[0];
         getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
         nrm = results.normal;
+        cti.m_colObj = colObjWrap->getCollisionObject();
         for (int i = 0; i < 3; ++i)
             f.m_pcontact[i] = bary[i];
     }
-
+    return (dst < 0);
 #endif
 
     // use collision quadrature point
@@ -2505,7 +2506,11 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
         btVector3 local_nrm;
         for (int q = 0; q < m_quads.size(); ++q)
         {
-            btVector3 p = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, m_quads[q]);
+            btVector3 p;
+            if (predict)
+                p = BaryEval(f.m_n[0]->m_q, f.m_n[1]->m_q, f.m_n[2]->m_q, m_quads[q]);
+            else
+                p = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, m_quads[q]);
             btScalar local_dst = m_worldInfo->m_sparsesdf.Evaluate(
                                                     wtr.invXform(p),
                                                     shp,
@@ -2513,12 +2518,21 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
                                                     margin);
             if (local_dst < dst)
             {
+                if (local_dst < 0 && predict)
+                    return true;
                 dst = local_dst;
                 contact_point = p;
                 bary = m_quads[q];
-                nrm = wtr.getBasis() * local_nrm;
+                nrm = local_nrm;
+            }
+            if (!predict)
+            {
+                cti.m_colObj = colObjWrap->getCollisionObject();
+                cti.m_normal = wtr.getBasis() * nrm;
+                cti.m_offset = dst;
             }
         }
+        return (dst < 0);
     }
 #endif
     
@@ -2530,6 +2544,11 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
         triangle_transform.setOrigin(f.m_n[0]->m_x);
         btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_x-f.m_n[0]->m_x, f.m_n[2]->m_x-f.m_n[0]->m_x);
         btVector3 guess(0,0,0);
+        if (predict)
+        {
+            triangle_transform.setOrigin(f.m_n[0]->m_q);
+            triangle = btTriangleShape(btVector3(0,0,0), f.m_n[1]->m_q-f.m_n[0]->m_q, f.m_n[2]->m_q-f.m_n[0]->m_q);
+        }
         const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
         btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results);
         dst = results.distance - margin;
@@ -2547,9 +2566,7 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
         cti.m_offset = dst;
     }
     
-    if (dst < 0)
+    return (dst < 0);
-        return true;
-    return (false);
 }
 
 //
@@ -3681,8 +3698,8 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap
                     docollideFace.psb = this;
                     docollideFace.m_colObj1Wrap = pcoWrap;
                     docollideFace.m_rigidBody = prb1;
-                    docollideFace.dynmargin = basemargin + timemargin;
+					docollideFace.dynmargin = 0.05*(basemargin + timemargin);
-                    docollideFace.stamargin = basemargin;
+					docollideFace.stamargin = 0.05*basemargin;
                     m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace);
                 }
             }

src/BulletSoftBody/btSoftBodyInternals.h

@@ -1070,8 +1070,8 @@ struct btSoftColliders
 
 			if (!n.m_battach)
             {
-                // check for collision at x_{n+1}^* as well at x_n
+				// check for collision at x_{n+1}^*
-                if (psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ true) || psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predict = */ true))
+				if (psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predict = */ true))
                 {
                     const btScalar ima = n.m_im;
                     // todo: collision between multibody and fixed deformable node will be missed.
@@ -1159,7 +1159,6 @@ struct btSoftColliders
             btSoftBody::Node* n0 = f.m_n[0];
             btSoftBody::Node* n1 = f.m_n[1];
             btSoftBody::Node* n2 = f.m_n[2];
-            
             const btScalar m = (n0->m_im > 0 && n1->m_im > 0 && n2->m_im > 0 )? dynmargin : stamargin;
             btSoftBody::DeformableFaceRigidContact c;
             btVector3 contact_point;
@@ -1181,6 +1180,8 @@ struct btSoftColliders
                     // todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices
                     c.m_weights = btScalar(2)/(btScalar(1) + bary.length2()) * bary;
                     c.m_face = &f;
+					// friction is handled by the nodes to prevent sticking
+//                    const btScalar fc = 0;
                     const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
                     
                     // the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf