change collision detection between deformables to from continuous to discrete

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);

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

@@ -3702,12 +3702,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,
@@ -3723,6 +3731,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/btSoftBodyInternals.h

@@ -1243,6 +1243,7 @@ struct btSoftColliders
         btScalar dynmargin;
         btScalar stamargin;
     };
+    
 	//
 	// CollideVF_SS
 	//
@@ -1299,6 +1300,7 @@ struct btSoftColliders
 		btScalar mrg;
 	};
     
+    
     //
     // CollideVF_DD
     //
@@ -1309,37 +1311,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;
-            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)
+            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))
             {
-                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 +1356,7 @@ struct btSoftColliders
         }
         btSoftBody* psb[2];
         btScalar mrg;
+        bool useFaceNormal;
     };
     
     //
@@ -1371,37 +1372,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 +1426,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;
     };
 };