added caching barycenters for static constraints, with lots of debugging lines

11 files changed, 222 insertions, 60 deletions

diff --git a/examples/DeformableDemo/GraspDeformable.cpp b/examples/DeformableDemo/GraspDeformable.cpp
index f52f67fe9..1d692a60b 100644
--- a/examples/DeformableDemo/GraspDeformable.cpp
+++ b/examples/DeformableDemo/GraspDeformable.cpp
@@ -35,6 +35,9 @@
 #include "../CommonInterfaces/CommonFileIOInterface.h"
 #include "Bullet3Common/b3FileUtils.h"
 
+#include "../ThirdPartyLibs/Wavefront/tiny_obj_loader.h"
+#include "../src/LinearMath/btScalar.h"
+
 ///The GraspDeformable shows grasping a volumetric deformable objects with multibody gripper with moter constraints.
 static btScalar sGripperVerticalVelocity = 0.f;
 static btScalar sGripperClosingTargetVelocity = 0.f;
@@ -73,7 +76,7 @@ public:
 
 	void resetCamera()
 	{
-        float dist = 0.3;
+        float dist = 0.2;
         float pitch = -45;
         float yaw = 100;
         float targetPos[3] = {0, -0.1, 0};
@@ -256,12 +259,12 @@ void GraspDeformable::initPhysics()
     //create a ground
     {
         btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(10.), btScalar(5.), btScalar(10.)));
-        groundShape->setMargin(0.001);
+        groundShape->setMargin(0.003);
         m_collisionShapes.push_back(groundShape);
 
         btTransform groundTransform;
         groundTransform.setIdentity();
-        groundTransform.setOrigin(btVector3(0, -5.1, 0));
+        groundTransform.setOrigin(btVector3(0, -5, 0));
         groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0));
         //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
         btScalar mass(0.);
@@ -338,12 +341,12 @@ void GraspDeformable::initPhysics()
     {
         bool onGround = false;
         const btScalar s = .05;
-        const btScalar h = -0.02;
+        const btScalar h = 0.01;
         btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s),
                                                          btVector3(+s, h, -s),
                                                          btVector3(-s, h, +s),
                                                          btVector3(+s, h, +s),
-                                                         10,10,
+                                                         2,2,
                                                                   0, true);
 
         if (onGround)
@@ -356,19 +359,109 @@ void GraspDeformable::initPhysics()
                                                  0, true);
 
         psb->getCollisionShape()->setMargin(0.001);
+
+        
         psb->generateBendingConstraints(2);
-        psb->setTotalMass(0.01);
-        psb->setSpringStiffness(10);
-        psb->setDampingCoefficient(0.05);
+        psb->setTotalMass(0.02);
+
         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.collisions = btSoftBody::fCollision::SDF_RD;
         psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_MDF;
         psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF;
+        
         getDeformableDynamicsWorld()->addSoftBody(psb);
-        getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce(0.05,0.005, true));
-        getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity*0.1));
+        
+        btScalar elastic_stiffness = 0.2;
+        btScalar damping_stiffness = 0.01;
+        bool not_damp_all_directions = true;
+        btScalar bending_stiffness = 0.2;
+        psb->setSpringStiffness(elastic_stiffness);
+        btDeformableLagrangianForce* springForce =
+        new btDeformableMassSpringForce(elastic_stiffness,
+                                        damping_stiffness, not_damp_all_directions,bending_stiffness);
+        getDeformableDynamicsWorld()->addForce(psb, springForce);
+        getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity));
+    }
+    
+    if(0){
+        
+              btSoftBody* psb = NULL;
+              
+              std::vector<tinyobj::shape_t> shapes;
+              tinyobj::attrib_t attribute;
+              char absolute_path[1024];
+              b3BulletDefaultFileIO fileio;
+              fileio.findResourcePath("dang_sticky_rice_chips_full_sim.obj", absolute_path, 1024);
+              std::string err = tinyobj::LoadObj(attribute, shapes, absolute_path, "", &fileio);
+              if (!shapes.empty())
+              {
+                  const tinyobj::shape_t& shape = shapes[0];
+                  btAlignedObjectArray<btScalar> vertices;
+                  btAlignedObjectArray<int> indices;
+                  for (int i = 0; i < attribute.vertices.size(); i++)
+                  {
+                      vertices.push_back(attribute.vertices[i]);
+                  }
+                  for (int i = 0; i < shape.mesh.indices.size(); i++)
+                  {
+                      indices.push_back(shape.mesh.indices[i].vertex_index);
+                  }
+                  int numTris = shape.mesh.indices.size() / 3;
+                  if (numTris > 0)
+                  {
+                      psb = btSoftBodyHelpers::CreateFromTriMesh(   getDeformableDynamicsWorld()->getWorldInfo(), &vertices[0], &indices[0], numTris);
+                      
+                      btAssert(psb!=nullptr);
+                  }
+                  btScalar elastic_stiffness = 0.2;
+                  btScalar damping_stiffness = 0.01;
+                  bool not_damp_all_directions = true;
+                  btScalar bending_stiffness = 0.2;
+                  btDeformableLagrangianForce* springForce =
+                  new btDeformableMassSpringForce(elastic_stiffness,
+                                                  damping_stiffness, not_damp_all_directions,bending_stiffness);
+                  psb->generateBendingConstraints(3);
+                  getDeformableDynamicsWorld()->addForce(psb, springForce);
+                  m_forces.push_back(springForce);
+                  
+                  btVector3 gravity =getDeformableDynamicsWorld()->getGravity();
+                  btDeformableLagrangianForce* gravityForce = new btDeformableGravityForce(gravity *0.2);
+                  getDeformableDynamicsWorld()->addForce(psb, gravityForce);
+                  m_forces.push_back(gravityForce);
+                  
+                  btScalar friction = 1;
+                  psb->m_cfg.kDF = friction;
+                  
+                  // turn on the collision flag for deformable
+                  // collision between deformable and rigid
+                  psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
+                  // turn on face contact for multibodies
+                  psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_MDF;
+                  /// turn on face contact for rigid body
+                  psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF;
+                  // collion between deformable and deformable and self-collision
+                  psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD;
+                  psb->setCollisionFlags(0);
+                  
+                  psb->setTotalMass(0.02);
+                  psb->setSelfCollision(false);
+                  btScalar repulsionStiffness = 0.2;
+                  psb->setSpringStiffness(repulsionStiffness);
+                  psb->initializeFaceTree();
+              }
+              //        psb->scale(btVector3(1, 1, 1));
+              psb->rotate(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.5));
+//              psb->translate(tr);
+              
+              psb->getCollisionShape()->setMargin(0.005);
+              psb->getCollisionShape()->setUserPointer(psb);
+              
+              psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
+              psb->m_cfg.kCHR = 1; // collision hardness with rigid body
+              getDeformableDynamicsWorld()->addSoftBody(psb);
+          
     }
     
 	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp b/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp
index 45d181713..856b5d411 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..4e01bb2f5 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)
 	{
 	}
 
@@ -39,6 +39,7 @@ public:
 	// return the number of iterations taken
 	int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false)
 	{
+        verbose = true;
 		BT_PROFILE("CRSolve");
 		btAssert(x.size() == b.size());
 		reinitialize(b);
diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
index 0c3e0b5eb..4a36e6617 100644
--- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
+++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
@@ -18,7 +18,7 @@
 #include "LinearMath/btQuickprof.h"
 
 btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody*>& softBodies, const TVStack& backup_v)
-	: m_softBodies(softBodies), m_projection(softBodies), m_backupVelocity(backup_v), m_implicit(false)
+	: m_softBodies(softBodies), m_projection(softBodies, backup_v), m_backupVelocity(backup_v), m_implicit(false)
 {
 	m_massPreconditioner = new MassPreconditioner(m_softBodies);
 	m_KKTPreconditioner = new KKTPreconditioner(m_softBodies, m_projection, m_lf, m_dt, m_implicit);
diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp
index 4916848fe..824e48331 100644
--- a/src/BulletSoftBody/btDeformableBodySolver.cpp
+++ b/src/BulletSoftBody/btDeformableBodySolver.cpp
@@ -32,6 +32,17 @@ btDeformableBodySolver::~btDeformableBodySolver()
 
 void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
 {
+//    std::cout <<"before solve deformable constraints "<<std::endl;
+    btSoftBody* psb = m_softBodies[0];
+    for(int i = 0; i<psb->m_faces.size(); i++){
+//        std::cout<<"face "<<i<<": ";
+        const btSoftBody::Face& face =psb->m_faces[i];
+        btVector3 vel = btVector3(0,0,0);
+        for(int j = 0; j<3; j++){
+            vel+= face.m_n[j]->m_v* face.m_pcontact[j];
+        }
+//        std::cout<<vel[0]<<" "<<vel[1]<<" "<<vel[2]<<std::endl;
+    }
 	BT_PROFILE("solveDeformableConstraints");
 	if (!m_implicit)
 	{
@@ -48,12 +59,25 @@ void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
 			m_objective->addLagrangeMultiplier(m_dv, x);
 			m_objective->m_preconditioner->reinitialize(true);
 			computeStep(x, rhs);
+            m_objective->m_projection.checkConstraints(x);
 			for (int i = 0; i < m_dv.size(); ++i)
 			{
 				m_dv[i] = x[i];
 			}
 		}
 		updateVelocity();
+        
+//        std::cout <<"after solve deformable constraints "<<std::endl;
+        btSoftBody* psb = m_softBodies[0];
+        for(int i = 0; i<psb->m_faces.size(); i++){
+//            std::cout<<"face "<<i<<": ";
+            const btSoftBody::Face& face =psb->m_faces[i];
+            btVector3 vel = btVector3(0,0,0);
+            for(int j = 0; j<3; j++){
+                vel+=face.m_n[j]->m_v* face.m_pcontact[j];
+            }
+//            std::cout<<vel[0]<<" "<<vel[1]<<" "<<vel[2]<<std::endl;
+        }
 	}
 	else
 	{
@@ -283,6 +307,10 @@ void btDeformableBodySolver::updateVelocity()
 			else
 			{
 				psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter] - psb->m_nodes[j].m_splitv;
+//                std::cout<<"node "<<j<<", back up vel "<< m_backupVelocity[counter][0] <<" "<< m_backupVelocity[counter][1]<<" "<<m_backupVelocity[counter][2]<<std::endl;
+//                std::cout<<" , dv: "<< m_dv[counter][0]<<" "<< m_dv[counter][1]<<" "<< m_dv[counter][2]<<" "<<std::endl;
+//                std::cout<<", split v: "<<psb->m_nodes[j].m_splitv[0]<<" "<<psb->m_nodes[j].m_splitv[1]<<" "<<psb->m_nodes[j].m_splitv[2]<<" "<<std::endl;
+                
 			}
 			psb->m_maxSpeedSquared = btMax(psb->m_maxSpeedSquared, psb->m_nodes[j].m_v.length2());
 			++counter;
@@ -349,7 +377,7 @@ void btDeformableBodySolver::setupDeformableSolve(bool implicit)
 			{
 				m_dv[counter] = psb->m_nodes[j].m_v + psb->m_nodes[j].m_splitv - m_backupVelocity[counter];
 			}
-			psb->m_nodes[j].m_v = m_backupVelocity[counter];
+//			psb->m_nodes[j].m_v = m_backupVelocity[counter];
 			++counter;
 		}
 	}
diff --git a/src/BulletSoftBody/btDeformableContactConstraint.h b/src/BulletSoftBody/btDeformableContactConstraint.h
index dc7077377..3b559bbf7 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..1fd67daf5 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]);
 	}
 }
 
@@ -525,11 +526,22 @@ void btDeformableContactProjection::setLagrangeMultiplier()
 			{
 				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;
+            lm.m_vals[0] = lm.m_vals[1] = lm.m_vals[2] = 0;
+            
+            btVector3 face_vel = btVector3(0,0,0);
+            btVector3 backup_face_vel = btVector3(0,0,0);
+            for(int kk = 0; kk<3; kk++){
+                face_vel += (face->m_n[kk]->m_v + face->m_n[kk]->m_splitv)* bary[kk];
+                backup_face_vel += m_backupVelocity[face->m_n[kk]->index] * bary[kk];
+            }
+            
+            btVector3 dvel = face_vel - backup_face_vel;
+            
 			for (int k = 0; k < 3; ++k)
 			{
 				face->m_n[k]->m_constrained = true;
@@ -538,15 +550,21 @@ 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);
 				lm.m_dirs[2] = btVector3(0, 0, 1);
+                lm.m_vals[0] = dvel[0];
+                lm.m_vals[1] = dvel[1];
+                lm.m_vals[2] = dvel[2];
 			}
 			else
 			{
+                face->m_pcontact[3] = 0;
 				lm.m_num_constraints = 1;
 				lm.m_dirs[0] = m_faceRigidConstraints[i][j].m_normal;
+                lm.m_vals[0] = dvel.dot(m_faceRigidConstraints[i][j].m_normal);
 			}
 			m_lagrangeMultipliers.push_back(lm);
 		}
diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h
index 4964eaf99..82ae19ae6 100644
--- a/src/BulletSoftBody/btDeformableContactProjection.h
+++ b/src/BulletSoftBody/btDeformableContactProjection.h
@@ -32,6 +32,7 @@ struct LagrangeMultiplier
 	btScalar m_weights[3];  // weights of the nodes involved, same size as m_num_nodes
 	btVector3 m_dirs[3];    // Constraint directions, same size of m_num_constraints;
 	int m_indices[3];       // indices of the nodes involved, same size as m_num_nodes;
+    btScalar m_vals[3];
 };
 
 class btDeformableContactProjection
@@ -39,6 +40,7 @@ class btDeformableContactProjection
 public:
 	typedef btAlignedObjectArray<btVector3> TVStack;
 	btAlignedObjectArray<btSoftBody*>& m_softBodies;
+    const TVStack& m_backupVelocity;
 
 	// all constraints involving face
 	btAlignedObjectArray<btDeformableContactConstraint*> m_allFaceConstraints;
@@ -64,8 +66,8 @@ public:
 
 	bool m_useStrainLimiting;
 
-	btDeformableContactProjection(btAlignedObjectArray<btSoftBody*>& softBodies)
-		: m_softBodies(softBodies)
+	btDeformableContactProjection(btAlignedObjectArray<btSoftBody*>& softBodies, const TVStack& backupVelocity)
+		: m_softBodies(softBodies), m_backupVelocity(backupVelocity)
 	{
 	}
 
diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
index dbde78944..257483d64 100644
--- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
+++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
@@ -401,6 +401,7 @@ void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep)
 
 	// At this point, dv should be golden for nodes in contact
 	// proceed to solve deformable momentum equation
+    
 	m_deformableBodySolver->solveDeformableConstraints(timeStep);
 }
 
diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp
index 427c14954..90f39db20 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,7 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
 		return (dst < 0);
 	}
 #endif
-	btGjkEpaSolver2::sResults results;
+
 	btTransform triangle_transform;
 	triangle_transform.setIdentity();
 	triangle_transform.setOrigin(f.m_n[0]->m_q);
@@ -2906,13 +2870,65 @@ 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 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);
+            const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
+            btGjkEpaSolver2::SignedDistance(contact_point, margin, csh, wtr,results);
+            cti.m_colObj = colObjWrap->getCollisionObject();
+//            std::cout<<"****** distance1 "<<results.distance<<std::endl;
+//            std::cout<<"normal "<<results.normal[0]<<" "<<results.normal[1]<<" "<<results.normal[2]<<std::endl;
+//            std::cout<<"point is at "<<contact_point[0]<<" "<<contact_point[1]<<" "<<contact_point[2]<<std::endl;
+//            std::cout<<"three vertices: "<<std::endl;
+//            std::cout<<f.m_n[0]->m_x[0]<<" "<<f.m_n[0]->m_x[1]<<" "<<f.m_n[0]->m_x[2]<<" "<<std::endl;
+//            std::cout<<f.m_n[1]->m_x[0]<<" "<<f.m_n[1]->m_x[1]<<" "<<f.m_n[1]->m_x[2]<<" "<<std::endl;
+//            std::cout<<f.m_n[2]->m_x[0]<<" "<<f.m_n[2]->m_x[1]<<" "<<f.m_n[2]->m_x[2]<<" "<<std::endl;
+            dst = results.distance;
+            cti.m_normal = results.normal;
+            cti.m_offset = dst;
+            
+            
+            // use face contact to redo collision detection
+            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;
+            
+//            std::cout<<"contact point 1 "<<results.witnesses[0][0]<< " "<<results.witnesses[0][1]<< " "<<results.witnesses[0][2]<< " "<<std::endl;
+//            std::cout<<"contact point 2 "<<results.witnesses[1][0]<< " "<<results.witnesses[1][1]<< " "<<results.witnesses[1][2]<< " "<<std::endl;
+//            std::cout<<"result distance  "<<results.distance<<std::endl;
+//            std::cout<< csh->getMargin() <<" "<<margin<<std::endl;
+//            std::cout<<"****** distance 2"<<dst<<std::endl;
+//            std::cout<<"normal "<< results.normal[0]<< " "<<results.normal[1]<< " "<<results.normal[2]<<std::endl;
+            return true;
+        }
+    #endif
+    
+    //
 	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;
+    
+//    std::cout<<"contact point 1 "<<contact_point[0]<< " "<<contact_point[1]<< " "<<contact_point[2]<< " "<<std::endl;
+//    std::cout<<"contact point 2 "<<results.witnesses[1][0]<< " "<<results.witnesses[1][1]<< " "<<results.witnesses[1][2]<< " "<<std::endl;
+//    std::cout<<"result distance  "<<results.distance<<std::endl;
+//    std::cout<< csh->getMargin() <<" "<<margin<<std::endl;
+//    std::cout<<"distance "<<dst<<std::endl;
 	cti.m_colObj = colObjWrap->getCollisionObject();
 	cti.m_normal = results.normal;
 	cti.m_offset = dst;
diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h
index 8d13b39bd..e626a8b75 100644
--- a/src/BulletSoftBody/btSoftBodyInternals.h
+++ b/src/BulletSoftBody/btSoftBodyInternals.h
@@ -1766,7 +1766,7 @@ struct btSoftColliders
 			btVector3 bary;
 			if (psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, true))
 			{
-				f.m_pcontact[3] = 1;
+//				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.
@@ -1843,6 +1843,8 @@ struct btSoftColliders
 			{
 				f.m_pcontact[3] = 0;
 			}
+            //always mark caching contact as false
+            f.m_pcontact[3] = 0;
 		}
 		btSoftBody* psb;
 		const btCollisionObjectWrapper* m_colObj1Wrap;