add option to approximate face contact with collision quadrature points

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -8129,6 +8129,8 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
             {
                 psb->m_renderNodes.resize(0);
             }
+            // This value should really be a exposed parameter 
+            psb->setCollisionQuadrature(5);
             btVector3 gravity = m_data->m_dynamicsWorld->getGravity();
             btDeformableLagrangianForce* gravityForce = new btDeformableGravityForce(gravity);
             m_data->m_dynamicsWorld->addForce(psb, gravityForce);

src/BulletSoftBody/btSoftBody.cpp

@@ -2454,13 +2454,18 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
     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) ?
-//    (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
-//    : colObjWrap->getWorldTransform();
-    const btTransform& wtr = colObjWrap->getWorldTransform();
+    btTransform wtr = (predict) ?
+    (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
+    : colObjWrap->getWorldTransform();
+//    const btTransform& wtr = colObjWrap->getWorldTransform();
     btScalar dst;
-//    if (f.m_pcontact[3] != 0)
-    if (0)
+    
+#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];
@@ -2490,12 +2495,58 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
         for (int i = 0; i < 3; ++i)
             f.m_pcontact[i] = bary[i];
     }
+
+#endif
+
+    // use collision quadrature point
+#ifdef USE_QUADRATURE
+    {
+        dst = SIMD_INFINITY;
+        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]);
+            btScalar local_dst = m_worldInfo->m_sparsesdf.Evaluate(
+                                                    wtr.invXform(p),
+                                                    shp,
+                                                    local_nrm,
+                                                    margin);
+            if (local_dst < dst)
+            {
+                dst = local_dst;
+                contact_point = p;
+                bary = m_quads[q];
+                nrm = wtr.getBasis() * local_nrm;
+            }
+        }
+    }
+#endif
+    
+    // regular face contact
+    {
+        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;
+        for (int i = 0; i < 3; ++i)
+            f.m_pcontact[i] = bary[i];
+    }
+    
     if (!predict)
     {
         cti.m_colObj = colObjWrap->getCollisionObject();
         cti.m_normal = nrm;
         cti.m_offset = dst;
     }
+    
     if (dst < 0)
         return true;
     return (false);
@@ -3331,6 +3382,17 @@ void btSoftBody::interpolateRenderMesh()
     }
 }
 
+void btSoftBody::setCollisionQuadrature(int N)
+{
+    for (int i = 0; i <= N; ++i)
+    {
+        for (int j = 0; i+j <= N; ++j)
+        {
+            m_quads.push_back(btVector3(btScalar(i)/btScalar(N), btScalar(j)/btScalar(N), btScalar(N-i-j)/btScalar(N)));
+        }
+    }
+}
+
 //
 void btSoftBody::PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti)
 {

src/BulletSoftBody/btSoftBody.h

@@ -802,6 +802,7 @@ public:
     btScalar m_sleepingThreshold;
     btScalar m_maxSpeedSquared;
     bool m_useFaceContact;
+    btAlignedObjectArray<btVector3> m_quads; // quadrature points for collision detection
     
     btAlignedObjectArray<btVector4> m_renderNodesInterpolationWeights;
     btAlignedObjectArray<btAlignedObjectArray<const btSoftBody::Node*> > m_renderNodesParents;
@@ -1143,6 +1144,7 @@ public:
 	void applyForces();
     void setMaxStress(btScalar maxStress);
     void interpolateRenderMesh();
+    void setCollisionQuadrature(int N);
 	static void PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti);
 	static void PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti);
 	static void PSolve_SContacts(btSoftBody* psb, btScalar, btScalar ti);