add normal cone optimization for self-collision

examples/DeformableDemo/DeformableSelfCollision.cpp

@@ -153,6 +153,7 @@ void DeformableSelfCollision::initPhysics()
                                                          btVector3(-s, h, +4*s),
                                                          btVector3(+s, h, +4*s),
                                                          10,40,
+//                                                         2,2,
                                                          0, true, 0.01);
 
         

examples/DeformableDemo/PinchFriction.cpp

@@ -274,8 +274,8 @@ void PinchFriction::initPhysics()
         
         psb->scale(btVector3(2, 2, 1));
         psb->translate(btVector3(0, 2.1, 2.2));
-        psb->getCollisionShape()->setMargin(0.03);
+        psb->getCollisionShape()->setMargin(0.1);
-        psb->setTotalMass(1);
+        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;
@@ -303,8 +303,8 @@ void PinchFriction::initPhysics()
         
         psb2->scale(btVector3(2, 2, 1));
         psb2->translate(btVector3(0, 2.1, -2.2));
-        psb2->getCollisionShape()->setMargin(0.03);
+        psb2->getCollisionShape()->setMargin(0.1);
-        psb2->setTotalMass(1);
+        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;
@@ -332,8 +332,8 @@ void PinchFriction::initPhysics()
         
         psb3->scale(btVector3(2, 2, 1));
         psb3->translate(btVector3(0, 2.1, 0));
-        psb3->getCollisionShape()->setMargin(0.03);
+        psb3->getCollisionShape()->setMargin(0.1);
-        psb3->setTotalMass(1);
+        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;

src/BulletCollision/BroadphaseCollision/btDbvt.h

@@ -21,7 +21,7 @@ subject to the following restrictions:
 #include "LinearMath/btVector3.h"
 #include "LinearMath/btTransform.h"
 #include "LinearMath/btAabbUtil2.h"
-
+#include <iostream>
 //
 // Compile time configuration
 //
@@ -131,6 +131,8 @@ subject to the following restrictions:
 /* btDbvtAabbMm			*/
 struct btDbvtAabbMm
 {
+    DBVT_INLINE btDbvtAabbMm(){}
+    DBVT_INLINE btDbvtAabbMm(const btDbvtAabbMm& other): mi(other.mi), mx(other.mx){}
 	DBVT_INLINE btVector3 Center() const { return ((mi + mx) / 2); }
 	DBVT_INLINE btVector3 Lengths() const { return (mx - mi); }
 	DBVT_INLINE btVector3 Extents() const { return ((mx - mi) / 2); }
@@ -190,6 +192,36 @@ struct btDbvtNode
 	};
 };
 
+/* btDbv(normal)tNode                */
+struct btDbvntNode
+{
+    btDbvtVolume volume;
+    btVector3 normal;
+    btScalar angle;
+    DBVT_INLINE bool isleaf() const { return (childs[1] == 0); }
+    DBVT_INLINE bool isinternal() const { return (!isleaf()); }
+    btDbvntNode* childs[2];
+    void* data;
+
+    btDbvntNode(const btDbvtNode* n)
+    : volume(n->volume)
+    , angle(0)
+    , normal(0,0,0)
+    , data(n->data)
+    {
+        childs[0] = 0;
+        childs[1] = 0;
+    }
+    
+    ~btDbvntNode()
+    {
+        if (childs[0])
+            delete childs[0];
+        if (childs[1])
+            delete childs[1];
+    }
+};
+
 typedef btAlignedObjectArray<const btDbvtNode*> btNodeStack;
 
 ///The btDbvt class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree).
@@ -225,6 +257,14 @@ struct btDbvt
 		btDbvtNode* parent;
 		sStkCLN(const btDbvtNode* n, btDbvtNode* p) : node(n), parent(p) {}
 	};
+    
+    struct sStknNN
+    {
+        const btDbvntNode* a;
+        const btDbvntNode* b;
+        sStknNN() {}
+        sStknNN(const btDbvntNode* na, const btDbvntNode* nb) : a(na), b(nb) {}
+    };
 	// Policies/Interfaces
 
 	/* ICollide	*/
@@ -234,6 +274,7 @@ struct btDbvt
 		DBVT_VIRTUAL void Process(const btDbvtNode*, const btDbvtNode*) {}
 		DBVT_VIRTUAL void Process(const btDbvtNode*) {}
 		DBVT_VIRTUAL void Process(const btDbvtNode* n, btScalar) { Process(n); }
+        DBVT_VIRTUAL void Process(const btDbvntNode*, const btDbvntNode*) {}
 		DBVT_VIRTUAL bool Descent(const btDbvtNode*) { return (true); }
 		DBVT_VIRTUAL bool AllLeaves(const btDbvtNode*) { return (true); }
 	};
@@ -306,6 +347,9 @@ struct btDbvt
 	void collideTT(const btDbvtNode* root0,
 				   const btDbvtNode* root1,
 				   DBVT_IPOLICY);
+    DBVT_PREFIX
+    void selfCollideT(const btDbvntNode* root,
+                   DBVT_IPOLICY);
 
 	DBVT_PREFIX
 	void collideTTpersistentStack(const btDbvtNode* root0,
@@ -837,6 +881,71 @@ inline void btDbvt::collideTT(const btDbvtNode* root0,
 	}
 }
 
+//
+DBVT_PREFIX
+inline void btDbvt::selfCollideT(const btDbvntNode* root,
+                              DBVT_IPOLICY)
+{
+    DBVT_CHECKTYPE
+    if (root)
+    {
+        int depth = 1;
+        int treshold = DOUBLE_STACKSIZE - 4;
+        btAlignedObjectArray<sStknNN> stkStack;
+        stkStack.resize(DOUBLE_STACKSIZE);
+        stkStack[0] = sStknNN(root, root);
+        do
+        {
+            sStknNN p = stkStack[--depth];
+            if (depth > treshold)
+            {
+                stkStack.resize(stkStack.size() * 2);
+                treshold = stkStack.size() - 4;
+            }
+            if (p.a == p.b)
+            {
+                if (p.a->isinternal() && p.a->angle > SIMD_PI)
+                {
+                    stkStack[depth++] = sStknNN(p.a->childs[0], p.a->childs[0]);
+                    stkStack[depth++] = sStknNN(p.a->childs[1], p.a->childs[1]);
+                    stkStack[depth++] = sStknNN(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++] = sStknNN(p.a->childs[0], p.b->childs[0]);
+                        stkStack[depth++] = sStknNN(p.a->childs[1], p.b->childs[0]);
+                        stkStack[depth++] = sStknNN(p.a->childs[0], p.b->childs[1]);
+                        stkStack[depth++] = sStknNN(p.a->childs[1], p.b->childs[1]);
+                    }
+                    else
+                    {
+                        stkStack[depth++] = sStknNN(p.a->childs[0], p.b);
+                        stkStack[depth++] = sStknNN(p.a->childs[1], p.b);
+                    }
+                }
+                else
+                {
+                    if (p.b->isinternal())
+                    {
+                        stkStack[depth++] = sStknNN(p.a, p.b->childs[0]);
+                        stkStack[depth++] = sStknNN(p.a, p.b->childs[1]);
+                    }
+                    else
+                    {
+                        policy.Process(p.a, p.b);
+                    }
+                }
+            }
+        } while (depth);
+    }
+}
+
+
 DBVT_PREFIX
 inline void btDbvt::collideTTpersistentStack(const btDbvtNode* root0,
 											 const btDbvtNode* root1,

src/BulletSoftBody/btDeformableBodySolver.cpp

@@ -415,6 +415,7 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d
 
 void btDeformableBodySolver::updateSoftBodies()
 {
+    BT_PROFILE("updateSoftBodies");
     for (int i = 0; i < m_softBodies.size(); i++)
     {
         btSoftBody *psb = (btSoftBody *)m_softBodies[i];

src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp

@@ -77,6 +77,7 @@ void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar t
 
 void btDeformableMultiBodyDynamicsWorld::softBodySelfCollision()
 {
+    m_deformableBodySolver->updateSoftBodies();
     for (int i = 0; i < m_softBodies.size(); i++)
     {
         btSoftBody* psb = (btSoftBody*)m_softBodies[i];

src/BulletSoftBody/btSoftBody.cpp

@@ -18,6 +18,7 @@ subject to the following restrictions:
 #include "BulletSoftBody/btSoftBodySolvers.h"
 #include "btSoftBodyData.h"
 #include "LinearMath/btSerializer.h"
+#include "LinearMath/btAlignedAllocator.h"
 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
 #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
@@ -3453,9 +3454,55 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap
 	}
 }
 
+static inline btDbvntNode* copyToDbvnt(const btDbvtNode* n)
+{
+    if (n == 0)
+        return 0;
+    btDbvntNode* root = new btDbvntNode(n);
+    if (n->isinternal())
+    {
+        btDbvntNode* c0 = copyToDbvnt(n->childs[0]);
+        root->childs[0] = c0;
+        btDbvntNode* c1 = copyToDbvnt(n->childs[1]);
+        root->childs[1] = c1;
+    }
+    return root;
+}
+
+static inline void calculateNormalCone(btDbvntNode* root)
+{
+    if (!root)
+        return;
+    if (root->isleaf())
+    {
+        const btSoftBody::Face* face = (btSoftBody::Face*)root->data;
+        root->normal = face->m_normal;
+        root->angle = 0;
+    }
+    else
+    {
+        btVector3 n0(0,0,0), n1(0,0,0);
+        btScalar a0 = 0, a1 = 0;
+        if (root->childs[0])
+        {
+            calculateNormalCone(root->childs[0]);
+            n0 = root->childs[0]->normal;
+            a0 = root->childs[0]->angle;
+        }
+        if (root->childs[1])
+        {
+            calculateNormalCone(root->childs[1]);
+            n1 = root->childs[1]->normal;
+            a1 = root->childs[1]->angle;
+        }
+        root->normal = (n0+n1).safeNormalize();
+        root->angle = btMax(a0,a1) + btAngle(n0, n1)*0.5;
+    }
+}
 //
 void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
 {
+    BT_PROFILE("Deformable Collision");
 	const int cf = m_cfg.collisions & psb->m_cfg.collisions;
 	switch (cf & fCollision::SVSmask)
 	{
@@ -3495,7 +3542,6 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
 		break;
         case fCollision::VF_DD:
         {
-            // self-collision not supported yet
             if (this != psb)
             {
                 btSoftColliders::CollideVF_DD docollide;
@@ -3517,15 +3563,27 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
             }
             else
             {
-                btSoftColliders::CollideVF_DD docollide;
+                btSoftColliders::CollideFF_DD docollide;
                 docollide.mrg = getCollisionShape()->getMargin() +
                 psb->getCollisionShape()->getMargin();
-                /* psb0 nodes vs psb0 faces    */
                 docollide.psb[0] = this;
                 docollide.psb[1] = psb;
-                docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
+                /* psb0 faces vs psb0 faces    */
-                                                    docollide.psb[1]->m_fdbvt.m_root,
+                btDbvntNode* root = copyToDbvnt(this->m_fdbvt.m_root);
-                                                    docollide);
+                calculateNormalCone(root);
+                this->m_fdbvt.selfCollideT(root,docollide);
+                delete root;
+                
+//                btSoftColliders::CollideFF_DD docollide;
+//                /* common                    */
+//                docollide.mrg = getCollisionShape()->getMargin() +
+//                psb->getCollisionShape()->getMargin();
+//                /* psb0 nodes vs psb1 faces    */
+//                docollide.psb[0] = this;
+//                docollide.psb[1] = psb;
+//                docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_fdbvt.m_root,
+//                                                    docollide.psb[1]->m_fdbvt.m_root,
+//                                                    docollide);
             }
         }
         break;

src/BulletSoftBody/btSoftBodyInternals.h

@@ -1306,8 +1306,8 @@ struct btSoftColliders
     //
     struct CollideVF_DD : btDbvt::ICollide
     {
-        void Process(const btDbvtNode* lnode,
+        void Process(const btDbvntNode* lnode,
-                     const btDbvtNode* lface)
+                     const btDbvntNode* lface)
         {
             btSoftBody::Node* node = (btSoftBody::Node*)lnode->data;
             btSoftBody::Face* face = (btSoftBody::Face*)lface->data;
@@ -1324,7 +1324,7 @@ struct btSoftColliders
             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;
+            btScalar  scale = 2;
             // enlarge the triangle to catch collision on the edge
             btVector3 u0 = vc + (v0-vc)*scale;
             btVector3 u1 = vc + (v1-vc)*scale;
@@ -1359,6 +1359,123 @@ struct btSoftColliders
         btSoftBody* psb[2];
         btScalar mrg;
     };
+    
+    //
+    // CollideFF_DD
+    //
+    struct CollideFF_DD : btDbvt::ICollide
+    {
+        void Process(const btDbvntNode* lface1,
+                     const btDbvntNode* 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 = btMax(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);
+                    }
+                }
+            }
+        }
+        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 = btMax(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;
+    };
 };
 
 #endif  //_BT_SOFT_BODY_INTERNALS_H