Merge pull request #2506 from xhan0619/master

Deformable vs. Rigid face contact improvement and Deformable vs. Deformable contact improvement

examples/DeformableDemo/PinchFriction.cpp

@@ -93,16 +93,6 @@ public:
     virtual void renderScene()
     {
         CommonRigidBodyBase::renderScene();
-//        btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
-//
-//        for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
-//        {
-//            btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i];
-//            {
-//                btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer());
-//                btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
-//            }
-//        }
     }
 };
 
@@ -115,8 +105,8 @@ void dynamics2(btScalar time, btDeformableMultiBodyDynamicsWorld* world)
     btScalar pressTime = 0.45;
     btScalar liftTime = 5;
     btScalar shiftTime = 1.75;
-    btScalar holdTime = 4.5*1000;
+    btScalar holdTime = 7.5;
-    btScalar dropTime = 5.3*1000;
+    btScalar dropTime = 8.3;
     btTransform rbTransform;
     rbTransform.setIdentity();
     btVector3 translation;
@@ -259,7 +249,7 @@ void PinchFriction::initPhysics()
         btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
         btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia);
         btRigidBody* body = new btRigidBody(rbInfo);
-        body->setFriction(0);
+        body->setFriction(0.5);
         
         //add the ground to the dynamics world
         m_dynamicsWorld->addRigidBody(body);
@@ -275,11 +265,11 @@ void PinchFriction::initPhysics()
         
         psb->scale(btVector3(2, 2, 1));
         psb->translate(btVector3(0, 2.1, 2.2));
-        psb->getCollisionShape()->setMargin(0.1);
+        psb->getCollisionShape()->setMargin(0.05);
         psb->setTotalMass(.6);
         psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
         psb->m_cfg.kCHR = 1; // collision hardness with rigid body
-        psb->m_cfg.kDF = 20;
+        psb->m_cfg.kDF = 2;
         btSoftBodyHelpers::generateBoundaryFaces(psb);
         psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
         psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD;
@@ -304,11 +294,11 @@ void PinchFriction::initPhysics()
         
         psb2->scale(btVector3(2, 2, 1));
         psb2->translate(btVector3(0, 2.1, -2.2));
-        psb2->getCollisionShape()->setMargin(0.1);
+        psb2->getCollisionShape()->setMargin(0.05);
         psb2->setTotalMass(.6);
         psb2->m_cfg.kKHR = 1; // collision hardness with kinematic objects
         psb2->m_cfg.kCHR = 1; // collision hardness with rigid body
-        psb2->m_cfg.kDF = 20;
+        psb2->m_cfg.kDF = 2;
         psb2->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
         psb2->m_cfg.collisions |= btSoftBody::fCollision::VF_DD;
         btSoftBodyHelpers::generateBoundaryFaces(psb2);
@@ -333,11 +323,11 @@ void PinchFriction::initPhysics()
         
         psb3->scale(btVector3(2, 2, 1));
         psb3->translate(btVector3(0, 2.1, 0));
-        psb3->getCollisionShape()->setMargin(0.1);
+        psb3->getCollisionShape()->setMargin(0.05);
         psb3->setTotalMass(.6);
         psb3->m_cfg.kKHR = 1; // collision hardness with kinematic objects
         psb3->m_cfg.kCHR = 1; // collision hardness with rigid body
-        psb3->m_cfg.kDF = 20;
+        psb3->m_cfg.kDF = 2;
         psb3->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
         psb3->m_cfg.collisions |= btSoftBody::fCollision::VF_DD;
         btSoftBodyHelpers::generateBoundaryFaces(psb3);

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -1642,7 +1642,6 @@ struct PhysicsServerCommandProcessorInternalData
     btAlignedObjectArray<btDeformableLagrangianForce*> m_lf;
 #endif
 	
-	
 	btMultiBodyDynamicsWorld* m_dynamicsWorld;
 
 	int m_constraintSolverType;

examples/pybullet/examples/deformable_ball.py

@@ -13,6 +13,7 @@ planeId = p.loadURDF("plane.urdf", [0,0,-2],planeOrn)
 boxId = p.loadURDF("cube.urdf", [0,3,2],useMaximalCoordinates = True)
 
 ballId = p.loadSoftBody("ball.vtk", basePosition = [0,0,-1], scale = 0.5, mass = 0.1, useNeoHookean = 1, NeoHookeanMu = 20, NeoHookeanLambda = 20, NeoHookeanDamping = 0.001, useSelfCollision = 1, frictionCoeff = .5)
+p.setTimeStep(0.001)
 p.setPhysicsEngineParameter(sparseSdfVoxelSize=0.25)
 p.setRealTimeSimulation(1)
 

src/BulletCollision/BroadphaseCollision/btDbvt.h

@@ -349,6 +349,9 @@ struct btDbvt
     DBVT_PREFIX
     void selfCollideT(const btDbvntNode* root,
                    DBVT_IPOLICY);
+    DBVT_PREFIX
+    void selfCollideTT(const btDbvtNode* root,
+                      DBVT_IPOLICY);
 
 	DBVT_PREFIX
 	void collideTTpersistentStack(const btDbvtNode* root0,
@@ -944,6 +947,70 @@ inline void btDbvt::selfCollideT(const btDbvntNode* root,
     }
 }
 
+//
+DBVT_PREFIX
+inline void btDbvt::selfCollideTT(const btDbvtNode* root,
+                                 DBVT_IPOLICY)
+{
+    DBVT_CHECKTYPE
+    if (root)
+    {
+        int depth = 1;
+        int treshold = DOUBLE_STACKSIZE - 4;
+        btAlignedObjectArray<sStkNN> stkStack;
+        stkStack.resize(DOUBLE_STACKSIZE);
+        stkStack[0] = sStkNN(root, root);
+        do
+        {
+            sStkNN p = stkStack[--depth];
+            if (depth > treshold)
+            {
+                stkStack.resize(stkStack.size() * 2);
+                treshold = stkStack.size() - 4;
+            }
+            if (p.a == p.b)
+            {
+                if (p.a->isinternal())
+                {
+                    stkStack[depth++] = sStkNN(p.a->childs[0], p.a->childs[0]);
+                    stkStack[depth++] = sStkNN(p.a->childs[1], p.a->childs[1]);
+                    stkStack[depth++] = sStkNN(p.a->childs[0], p.a->childs[1]);
+                }
+            }
+            else if (Intersect(p.a->volume, p.b->volume))
+            {
+                if (p.a->isinternal())
+                {
+                    if (p.b->isinternal())
+                    {
+                        stkStack[depth++] = sStkNN(p.a->childs[0], p.b->childs[0]);
+                        stkStack[depth++] = sStkNN(p.a->childs[1], p.b->childs[0]);
+                        stkStack[depth++] = sStkNN(p.a->childs[0], p.b->childs[1]);
+                        stkStack[depth++] = sStkNN(p.a->childs[1], p.b->childs[1]);
+                    }
+                    else
+                    {
+                        stkStack[depth++] = sStkNN(p.a->childs[0], p.b);
+                        stkStack[depth++] = sStkNN(p.a->childs[1], p.b);
+                    }
+                }
+                else
+                {
+                    if (p.b->isinternal())
+                    {
+                        stkStack[depth++] = sStkNN(p.a, p.b->childs[0]);
+                        stkStack[depth++] = sStkNN(p.a, p.b->childs[1]);
+                    }
+                    else
+                    {
+                        policy.Process(p.a, p.b);
+                    }
+                }
+            }
+        } while (depth);
+    }
+}
+
 
 DBVT_PREFIX
 inline void btDbvt::collideTTpersistentStack(const btDbvtNode* root0,

src/BulletSoftBody/btDeformableBodySolver.cpp

@@ -431,11 +431,10 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d
     for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i)
     {
         btSoftBody::Node& n = psb->m_nodes[i];
-        vol = btDbvtVolume::FromCR(n.m_q, psb->m_sst.radmrg);
+        btVector3 points[2] = {n.m_x, n.m_q};
-        psb->m_ndbvt.update(n.m_leaf,
+        vol = btDbvtVolume::FromPoints(points, 2);
-                       vol,
+        vol.Expand(btVector3(psb->m_sst.radmrg, psb->m_sst.radmrg, psb->m_sst.radmrg));
-                       n.m_v * psb->m_sst.velmrg,
+        psb->m_ndbvt.update(n.m_leaf, vol);
-                       psb->m_sst.updmrg);
     }
 
     if (!psb->m_fdbvt.empty())
@@ -443,15 +442,12 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d
         for (int i = 0; i < psb->m_faces.size(); ++i)
         {
             btSoftBody::Face& f = psb->m_faces[i];
-            const btVector3 v = (f.m_n[0]->m_v +
+            btVector3 points[6] = {f.m_n[0]->m_x, f.m_n[0]->m_q,
-                                 f.m_n[1]->m_v +
+                                   f.m_n[1]->m_x, f.m_n[1]->m_q,
-                                 f.m_n[2]->m_v) /
+                                   f.m_n[2]->m_x, f.m_n[2]->m_q};
-            3;
+            vol = btDbvtVolume::FromPoints(points, 6);
-            vol = VolumeOf(f, psb->m_sst.radmrg);
+            vol.Expand(btVector3(psb->m_sst.radmrg, psb->m_sst.radmrg, psb->m_sst.radmrg));
-            psb->m_fdbvt.update(f.m_leaf,
+            psb->m_fdbvt.update(f.m_leaf, vol);
-                           vol,
-                           v * psb->m_sst.velmrg,
-                           psb->m_sst.updmrg);
         }
     }
     /* Clear contacts        */

src/BulletSoftBody/btSoftBody.cpp

@@ -117,7 +117,7 @@ void btSoftBody::initDefaults()
 	m_restLengthScale = btScalar(1.0);
     m_dampingCoefficient = 1;
     m_sleepingThreshold = 0.1;
-    m_useFaceContact = false;
+    m_useFaceContact = true;
 	m_useSelfCollision = false;
     m_collisionFlags = 0;
 }
@@ -325,7 +325,7 @@ void btSoftBody::appendFace(int model, Material* mat)
 		ZeroInitialize(f);
 		f.m_material = mat ? mat : m_materials[0];
 	}
-	m_faces.push_back(f);
+    m_faces.push_back(f);
 }
 
 //
@@ -2443,7 +2443,7 @@ static void getBarycentric(const btVector3& p, btVector3& a, btVector3& b, btVec
 
 //
 bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap,
-                                        const Face& f,
+                                        Face& f,
                                         btVector3& contact_point,
                                         btVector3& bary,
                                         btScalar margin,
@@ -2457,25 +2457,96 @@ 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 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;
+        // 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;
+        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];
+    }
     
-    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);
-    btScalar 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);
     if (!predict)
     {
         cti.m_colObj = colObjWrap->getCollisionObject();
-//        cti.m_normal = wtr.getBasis() * results.normal;
+        cti.m_normal = nrm;
-        cti.m_normal = results.normal;
         cti.m_offset = dst;
     }
+    
     if (dst < 0)
         return true;
     return (false);
@@ -3315,6 +3386,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)
 {
@@ -3706,12 +3788,20 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
                     docollide.mrg = getCollisionShape()->getMargin() +
                     psb->getCollisionShape()->getMargin();
                     /* psb0 nodes vs psb1 faces    */
+                    if (psb->m_tetras.size() > 0)
+                        docollide.useFaceNormal = true;
+                    else
+                        docollide.useFaceNormal = false;
                     docollide.psb[0] = this;
                     docollide.psb[1] = psb;
                     docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
                                                         docollide.psb[1]->m_fdbvt.m_root,
                                                         docollide);
                     /* psb1 nodes vs psb0 faces    */
+                    if (this->m_tetras.size() > 0)
+                        docollide.useFaceNormal = true;
+                    else
+                        docollide.useFaceNormal = false;
                     docollide.psb[0] = psb;
                     docollide.psb[1] = this;
                     docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
@@ -3727,6 +3817,10 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
                       psb->getCollisionShape()->getMargin();
                       docollide.psb[0] = this;
                       docollide.psb[1] = psb;
+                      if (this->m_tetras.size() > 0)
+                          docollide.useFaceNormal = true;
+                      else
+                          docollide.useFaceNormal = false;
                       /* psb0 faces vs psb0 faces    */
                       btDbvntNode* root = copyToDbvnt(this->m_fdbvt.m_root);
                       calculateNormalCone(root);

src/BulletSoftBody/btSoftBody.h

@@ -288,6 +288,7 @@ public:
 		btVector3 m_normal;  // Normal
 		btScalar m_ra;       // Rest area
 		btDbvtNode* m_leaf;  // Leaf data
+        btVector4 m_pcontact; // barycentric weights of the persistent contact
         int m_index;
 	};
 	/* Tetra		*/
@@ -801,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;
@@ -1120,7 +1122,7 @@ public:
 	void initializeFaceTree();
 	btVector3 evaluateCom() const;
 	bool checkDeformableContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const;
-    bool checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap, const Face& x, btVector3& contact_point, btVector3& bary, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const;
+    bool checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap, Face& x, btVector3& contact_point, btVector3& bary, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const;
     bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti) const;
 	void updateNormals();
 	void updateBounds();
@@ -1142,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);

src/BulletSoftBody/btSoftBodyHelpers.cpp

@@ -1500,7 +1500,6 @@ void btSoftBodyHelpers::getBarycentricWeights(const btVector3& a, const btVector
     bary = btVector4(va6*v6, vb6*v6, vc6*v6, vd6*v6);
 }
 
-
 // Iterate through all render nodes to find the simulation tetrahedron that contains the render node and record the barycentric weights
 // If the node is not inside any tetrahedron, assign it to the tetrahedron in which the node has the least negative barycentric weight
 void btSoftBodyHelpers::interpolateBarycentricWeights(btSoftBody* psb)

src/BulletSoftBody/btSoftBodyInternals.h

@@ -1166,6 +1166,7 @@ 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.
@@ -1236,6 +1237,10 @@ struct btSoftColliders
                     psb->m_faceRigidContacts.push_back(c);
                 }
             }
+            else
+            {
+                f.m_pcontact[3] = 0;
+            }
         }
         btSoftBody* psb;
         const btCollisionObjectWrapper* m_colObj1Wrap;
@@ -1243,6 +1248,7 @@ struct btSoftColliders
         btScalar dynmargin;
         btScalar stamargin;
     };
+    
 	//
 	// CollideVF_SS
 	//
@@ -1299,6 +1305,7 @@ struct btSoftColliders
 		btScalar mrg;
 	};
     
+    
     //
     // CollideVF_DD
     //
@@ -1309,37 +1316,35 @@ struct btSoftColliders
         {
             btSoftBody::Node* node = (btSoftBody::Node*)lnode->data;
             btSoftBody::Face* face = (btSoftBody::Face*)lface->data;
+            
             btVector3 o = node->m_x;
-            btVector3 p, normal;
+            btVector3 p;
-            const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]};
+            btScalar d = SIMD_INFINITY;
-            btVector3 dir = node->m_q - o;
+            ProjectOrigin(face->m_n[0]->m_x - o,
-            btScalar l = dir.length();
+                          face->m_n[1]->m_x - o,
-            if (l < SIMD_EPSILON)
+                          face->m_n[2]->m_x - o,
-                return;
+                          p, d);
-            btVector3 rayEnd = dir.normalized() * (l + 2*mrg);
+            const btScalar m = mrg + (o - node->m_q).safeNorm() * 2;
-            // register an intersection if the line segment formed by the trajectory of the node in the timestep intersects the face
+            if (d < (m * m))
-            btVector3 v0 = face->m_n[0]->m_x;
-            btVector3 v1 = face->m_n[1]->m_x;
-            btVector3 v2 = face->m_n[2]->m_x;
-            btVector3 vc = (v0+v1+v2)/3.;
-            btScalar  scale = 1.5;
-            // enlarge the triangle to catch collision on the edge
-            btVector3 u0 = vc + (v0-vc)*scale;
-            btVector3 u1 = vc + (v1-vc)*scale;
-            btVector3 u2 = vc + (v2-vc)*scale;
-            bool intersect = lineIntersectsTriangle(btVector3(0,0,0), rayEnd, u0-o, u1-o, u2-o, p, normal);
-
-            if (intersect)
             {
-                p += o;
+                const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]};
-                const btVector3 w = BaryCoord(n[0]->m_x, n[1]->m_x, n[2]->m_x, p);
+                const btVector3 w = BaryCoord(n[0]->m_x, n[1]->m_x, n[2]->m_x, p + o);
                 const btScalar ma = node->m_im;
                 btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w);
+                if ((n[0]->m_im <= 0) ||
+                    (n[1]->m_im <= 0) ||
+                    (n[2]->m_im <= 0))
+                {
+                    mb = 0;
+                }
                 const btScalar ms = ma + mb;
                 if (ms > 0)
                 {
                     btSoftBody::DeformableFaceNodeContact c;
-                    c.m_normal = normal;
+                    if (useFaceNormal)
+                        c.m_normal = face->m_normal;
+                    else
+                        c.m_normal = p / -btSqrt(d);
                     c.m_margin = mrg;
                     c.m_node = node;
                     c.m_face = face;
@@ -1356,6 +1361,7 @@ struct btSoftColliders
         }
         btSoftBody* psb[2];
         btScalar mrg;
+        bool useFaceNormal;
     };
     
     //
@@ -1371,37 +1377,45 @@ struct btSoftColliders
             for (int node_id = 0; node_id < 3; ++node_id)
             {
                 btSoftBody::Node* node = f->m_n[node_id];
-                btVector3 o = node->m_x;
+                bool skip = false;
-                btVector3 p, normal;
+                for (int i = 0; i < 3; ++i)
-                const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]};
-                btVector3 dir = node->m_q - o;
-                btScalar l = dir.length();
-                if (l < SIMD_EPSILON)
-                    return;
-                btVector3 rayEnd = dir.normalized() * (l + 2*mrg);
-                // register an intersection if the line segment formed by the trajectory of the node in the timestep intersects the face
-                btVector3 v0 = face->m_n[0]->m_x;
-                btVector3 v1 = face->m_n[1]->m_x;
-                btVector3 v2 = face->m_n[2]->m_x;
-                btVector3 vc = (v0+v1+v2)/3.;
-                btScalar  scale = 1.5;
-                // enlarge the triangle to catch collision on the edge
-                btVector3 u0 = vc + (v0-vc)*scale;
-                btVector3 u1 = vc + (v1-vc)*scale;
-                btVector3 u2 = vc + (v2-vc)*scale;
-                bool intersect = lineIntersectsTriangle(btVector3(0,0,0), rayEnd, u0-o, u1-o, u2-o, p, normal);
-                
-                if (intersect)
                 {
-                    p += o;
+                    if (face->m_n[i] == node)
-                    const btVector3 w = BaryCoord(n[0]->m_x, n[1]->m_x, n[2]->m_x, p);
+                    {
+                        skip = true;
+                        break;
+                    }
+                }
+                if (skip)
+                    continue;
+                btVector3 o = node->m_x;
+                btVector3 p;
+                btScalar d = SIMD_INFINITY;
+                ProjectOrigin(face->m_n[0]->m_x - o,
+                              face->m_n[1]->m_x - o,
+                              face->m_n[2]->m_x - o,
+                              p, d);
+                const btScalar m = mrg + (o - node->m_q).safeNorm() * 2;
+                if (d < (m * m))
+                {
+                    const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]};
+                    const btVector3 w = BaryCoord(n[0]->m_x, n[1]->m_x, n[2]->m_x, p + o);
                     const btScalar ma = node->m_im;
                     btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w);
+                    if ((n[0]->m_im <= 0) ||
+                        (n[1]->m_im <= 0) ||
+                        (n[2]->m_im <= 0))
+                    {
+                        mb = 0;
+                    }
                     const btScalar ms = ma + mb;
                     if (ms > 0)
                     {
                         btSoftBody::DeformableFaceNodeContact c;
-                        c.m_normal = normal;
+                        if (useFaceNormal)
+                            c.m_normal = face->m_normal;
+                        else
+                            c.m_normal = p / -btSqrt(d);
                         c.m_margin = mrg;
                         c.m_node = node;
                         c.m_face = face;
@@ -1417,62 +1431,9 @@ struct btSoftColliders
                 }
             }
         }
-        void Process(const btDbvtNode* lface1,
-                     const btDbvtNode* lface2)
-        {
-            btSoftBody::Face* f = (btSoftBody::Face*)lface1->data;
-            btSoftBody::Face* face = (btSoftBody::Face*)lface2->data;
-            for (int node_id = 0; node_id < 3; ++node_id)
-            {
-                btSoftBody::Node* node = f->m_n[node_id];
-                btVector3 o = node->m_x;
-                btVector3 p, normal;
-                const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]};
-                btVector3 dir = node->m_q - o;
-                btScalar l = dir.length();
-                if (l < SIMD_EPSILON)
-                    return;
-                btVector3 rayEnd = dir.normalized() * (l + 2*mrg);
-                // register an intersection if the line segment formed by the trajectory of the node in the timestep intersects the face
-                btVector3 v0 = face->m_n[0]->m_x;
-                btVector3 v1 = face->m_n[1]->m_x;
-                btVector3 v2 = face->m_n[2]->m_x;
-                btVector3 vc = (v0+v1+v2)/3.;
-                btScalar  scale = 1.5;
-                // enlarge the triangle to catch collision on the edge
-                btVector3 u0 = vc + (v0-vc)*scale;
-                btVector3 u1 = vc + (v1-vc)*scale;
-                btVector3 u2 = vc + (v2-vc)*scale;
-                bool intersect = lineIntersectsTriangle(btVector3(0,0,0), rayEnd, u0-o, u1-o, u2-o, p, normal);
-                
-                if (intersect)
-                {
-                    p += o;
-                    const btVector3 w = BaryCoord(n[0]->m_x, n[1]->m_x, n[2]->m_x, p);
-                    const btScalar ma = node->m_im;
-                    btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w);
-                    const btScalar ms = ma + mb;
-                    if (ms > 0)
-                    {
-                        btSoftBody::DeformableFaceNodeContact c;
-                        c.m_normal = normal;
-                        c.m_margin = mrg;
-                        c.m_node = node;
-                        c.m_face = face;
-                        c.m_bary = w;
-                        // 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) + w.length2()) * w;
-                        c.m_friction = psb[0]->m_cfg.kDF *  psb[1]->m_cfg.kDF;
-                        // the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf
-                        c.m_imf = c.m_bary[0]*c.m_weights[0] * n[0]->m_im + c.m_bary[1]*c.m_weights[1] * n[1]->m_im + c.m_bary[2]*c.m_weights[2] * n[2]->m_im;
-                        c.m_c0 = btScalar(1)/(ma + c.m_imf);
-                        psb[0]->m_faceNodeContacts.push_back(c);
-                    }
-                }
-            }
-        }
         btSoftBody* psb[2];
         btScalar mrg;
+        bool useFaceNormal;
     };
 };
 

src/BulletSoftBody/btSparseSDF.h

@@ -348,7 +348,7 @@ struct btSparseSdf
 	{
 		struct btS
 		{
-			int x, y, z,w;
+			int x, y, z, w;
 			void* p;
 		};