Merge pull request #2984 from fuchuyuan/cachebarycenter

Cachebarycenter

6 files changed, 53 insertions, 52 deletions

diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp b/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp
index 45d181713..7d53f8624 100644
--- a/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp
+++ b/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp
@@ -1049,7 +1049,8 @@ btScalar btGjkEpaSolver2::SignedDistance(const btVector3& position,
 		const btScalar length = delta.length();
 		results.normal = delta / length;
 		results.witnesses[0] += results.normal * margin;
-		return (length - margin);
+		results.distance = length - margin;
+		return results.distance;
 	}
 	else
 	{
diff --git a/src/BulletSoftBody/btConjugateResidual.h b/src/BulletSoftBody/btConjugateResidual.h
index e3bca6e12..614612036 100644
--- a/src/BulletSoftBody/btConjugateResidual.h
+++ b/src/BulletSoftBody/btConjugateResidual.h
@@ -30,7 +30,7 @@ class btConjugateResidual : public btKrylovSolver<MatrixX>
 
 public:
 	btConjugateResidual(const int max_it_in)
-		: Base(max_it_in, 1e-4)
+		: Base(max_it_in, 1e-8)
 	{
 	}
 
diff --git a/src/BulletSoftBody/btDeformableContactConstraint.h b/src/BulletSoftBody/btDeformableContactConstraint.h
index dc7077377..1e2c9f5bc 100644
--- a/src/BulletSoftBody/btDeformableContactConstraint.h
+++ b/src/BulletSoftBody/btDeformableContactConstraint.h
@@ -216,7 +216,7 @@ public:
 class btDeformableFaceRigidContactConstraint : public btDeformableRigidContactConstraint
 {
 public:
-	const btSoftBody::Face* m_face;
+	btSoftBody::Face* m_face;
 	bool m_useStrainLimiting;
 	btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal, bool useStrainLimiting);
 	btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other);
diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp
index 584b9d6ec..7f67260ce 100644
--- a/src/BulletSoftBody/btDeformableContactProjection.cpp
+++ b/src/BulletSoftBody/btDeformableContactProjection.cpp
@@ -452,7 +452,8 @@ void btDeformableContactProjection::checkConstraints(const TVStack& x)
 				d[j] += lm.m_weights[k] * x[lm.m_indices[k]].dot(lm.m_dirs[j]);
 			}
 		}
-		printf("d = %f, %f, %f\n", d[0], d[1], d[2]);
+		//		printf("d = %f, %f, %f\n", d[0], d[1], d[2]);
+		//        printf("val = %f, %f, %f\n", lm.m_vals[0], lm.m_vals[1], lm.m_vals[2]);
 	}
 }
 
@@ -493,6 +494,7 @@ void btDeformableContactProjection::setLagrangeMultiplier()
 			lm.m_dirs[2] = btVector3(0, 0, 1);
 			m_lagrangeMultipliers.push_back(lm);
 		}
+
 		for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j)
 		{
 			if (!m_nodeRigidConstraints[i][j].m_binding)
@@ -519,17 +521,19 @@ void btDeformableContactProjection::setLagrangeMultiplier()
 			}
 			m_lagrangeMultipliers.push_back(lm);
 		}
+
 		for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j)
 		{
 			if (!m_faceRigidConstraints[i][j].m_binding)
 			{
 				continue;
 			}
-			const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
+			btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face;
 
 			btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary;
 			LagrangeMultiplier lm;
 			lm.m_num_nodes = 3;
+
 			for (int k = 0; k < 3; ++k)
 			{
 				face->m_n[k]->m_constrained = true;
@@ -538,6 +542,7 @@ void btDeformableContactProjection::setLagrangeMultiplier()
 			}
 			if (m_faceRigidConstraints[i][j].m_static)
 			{
+				face->m_pcontact[3] = 1;
 				lm.m_num_constraints = 3;
 				lm.m_dirs[0] = btVector3(1, 0, 0);
 				lm.m_dirs[1] = btVector3(0, 1, 0);
@@ -545,6 +550,7 @@ void btDeformableContactProjection::setLagrangeMultiplier()
 			}
 			else
 			{
+				face->m_pcontact[3] = 0;
 				lm.m_num_constraints = 1;
 				lm.m_dirs[0] = m_faceRigidConstraints[i][j].m_normal;
 			}
diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp
index 427c14954..98c6f5759 100644
--- a/src/BulletSoftBody/btSoftBody.cpp
+++ b/src/BulletSoftBody/btSoftBody.cpp
@@ -2818,46 +2818,10 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
 	btTransform wtr = (predict) ? (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform() * (*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
 								: colObjWrap->getWorldTransform();
 	btScalar dst;
+	btGjkEpaSolver2::sResults results;
 
-	//#define USE_QUADRATURE 1
-	//#define CACHE_PREV_COLLISION
-
-	// use the contact position of the previous collision
-#ifdef CACHE_PREV_COLLISION
-	if (f.m_pcontact[3] != 0)
-	{
-		for (int i = 0; i < 3; ++i)
-			bary[i] = f.m_pcontact[i];
-		contact_point = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
-		dst = m_worldInfo->m_sparsesdf.Evaluate(
-			wtr.invXform(contact_point),
-			shp,
-			nrm,
-			margin);
-		nrm = wtr.getBasis() * nrm;
-		cti.m_colObj = colObjWrap->getCollisionObject();
-		// use cached contact point
-	}
-	else
-	{
-		btGjkEpaSolver2::sResults results;
-		btTransform triangle_transform;
-		triangle_transform.setIdentity();
-		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);
-		const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
-		btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results);
-		dst = results.distance - margin;
-		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
+//	#define USE_QUADRATURE 1
+//#define CACHE_PREV_COLLISION
 
 	// use collision quadrature point
 #ifdef USE_QUADRATURE
@@ -2895,7 +2859,8 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
 		return (dst < 0);
 	}
 #endif
-	btGjkEpaSolver2::sResults results;
+
+	// collision detection using x*
 	btTransform triangle_transform;
 	triangle_transform.setIdentity();
 	triangle_transform.setOrigin(f.m_n[0]->m_q);
@@ -2906,19 +2871,50 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
 	dst = results.distance - 2.0 * csh->getMargin() - margin;  // margin padding so that the distance = the actual distance between face and rigid - margin of rigid - margin of deformable
 	if (dst >= 0)
 		return false;
+
+// Use consistent barycenter to recalculate distance.
+#ifdef CACHE_PREV_COLLISION
+	if (f.m_pcontact[3] != 0)
+	{
+		for (int i = 0; i < 3; ++i)
+			bary[i] = f.m_pcontact[i];
+		contact_point = BaryEval(f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
+		const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
+		btGjkEpaSolver2::SignedDistance(contact_point, margin, csh, wtr, results);
+		cti.m_colObj = colObjWrap->getCollisionObject();
+		dst = results.distance;
+		cti.m_normal = results.normal;
+		cti.m_offset = dst;
+
+		//point-convex CD
+		wtr = colObjWrap->getWorldTransform();
+		btTriangleShape triangle2(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);
+		triangle_transform.setOrigin(f.m_n[0]->m_x);
+		btGjkEpaSolver2::SignedDistance(&triangle2, triangle_transform, csh, wtr, guess, results);
+
+		dst = results.distance - csh->getMargin() - margin;
+		return true;
+	}
+#endif
+
+	// Use triangle-convex CD.
 	wtr = colObjWrap->getWorldTransform();
 	btTriangleShape triangle2(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);
 	triangle_transform.setOrigin(f.m_n[0]->m_x);
 	btGjkEpaSolver2::SignedDistance(&triangle2, triangle_transform, csh, wtr, guess, results);
 	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);
+
+	for (int i = 0; i < 3; ++i)
+		f.m_pcontact[i] = bary[i];
+
 	dst = results.distance - csh->getMargin() - margin;
 	cti.m_colObj = colObjWrap->getCollisionObject();
 	cti.m_normal = results.normal;
 	cti.m_offset = dst;
 	return true;
 }
-//
+
 void btSoftBody::updateNormals()
 {
 	const btVector3 zv(0, 0, 0);
diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h
index 8d13b39bd..c17bbb5cd 100644
--- a/src/BulletSoftBody/btSoftBodyInternals.h
+++ b/src/BulletSoftBody/btSoftBodyInternals.h
@@ -1686,6 +1686,7 @@ struct btSoftColliders
 						c.m_c2 = ima;
 						c.m_c3 = fc;
 						c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
+						c.m_c5 = n.m_effectiveMass_inv;
 
 						if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
 						{
@@ -1695,7 +1696,6 @@ struct btSoftColliders
 							const btVector3 ra = n.m_x - wtr.getOrigin();
 
 							c.m_c0 = ImpulseMatrix(1, n.m_effectiveMass_inv, imb, iwi, ra);
-							c.m_c5 = n.m_effectiveMass_inv;
 							//                            c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
 							c.m_c1 = ra;
 						}
@@ -1766,7 +1766,6 @@ struct btSoftColliders
 			btVector3 bary;
 			if (psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, true))
 			{
-				f.m_pcontact[3] = 1;
 				btScalar ima = n0->m_im + n1->m_im + n2->m_im;
 				const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
 				// todo: collision between multibody and fixed deformable face will be missed.
@@ -1839,10 +1838,9 @@ struct btSoftColliders
 					psb->m_faceRigidContacts.push_back(c);
 				}
 			}
-			else
-			{
-				f.m_pcontact[3] = 0;
-			}
+			// Set caching barycenters to be false after collision detection.
+			// Only turn on when contact is static.
+			f.m_pcontact[3] = 0;
 		}
 		btSoftBody* psb;
 		const btCollisionObjectWrapper* m_colObj1Wrap;