add naive implementation of self collision

src/BulletSoftBody/btDeformableBodySolver.cpp

@@ -22,7 +22,7 @@
 btDeformableBodySolver::btDeformableBodySolver()
 : m_numNodes(0)
 , m_cg(20)
-, m_maxNewtonIterations(3)
+, m_maxNewtonIterations(10)
 , m_newtonTolerance(1e-4)
 , m_lineSearch(true)
 {

src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp

@@ -23,7 +23,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlyIteration
         ///this is a special step to resolve penetrations (just for contacts)
         solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
         
-        m_maxOverrideNumSolverIterations = 150;
+        m_maxOverrideNumSolverIterations = 50;
         int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
         for (int iteration = 0; iteration < maxIterations; iteration++)
         {

src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp

@@ -51,6 +51,11 @@ void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar t
     ///perform collision detection
     btMultiBodyDynamicsWorld::performDiscreteCollisionDetection();
     
+    if (m_selfCollision)
+    {
+        softBodySelfCollision();
+    }
+    
     btMultiBodyDynamicsWorld::calculateSimulationIslands();
     
     beforeSolverCallbacks(timeStep);
@@ -70,6 +75,15 @@ void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar t
     // ///////////////////////////////
 }
 
+void btDeformableMultiBodyDynamicsWorld::softBodySelfCollision()
+{
+    for (int i = 0; i < m_softBodies.size(); i++)
+    {
+        btSoftBody* psb = (btSoftBody*)m_softBodies[i];
+        psb->defaultCollisionHandler(psb);
+    }
+}
+
 void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
 {
     BT_PROFILE("integrateTransforms");

src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h

@@ -48,6 +48,7 @@ class btDeformableMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld
     int m_contact_iterations;
     bool m_implicit;
     bool m_lineSearch;
+    bool m_selfCollision;
     
     typedef void (*btSolverCallback)(btScalar time, btDeformableMultiBodyDynamicsWorld* world);
     btSolverCallback m_solverCallback;
@@ -83,6 +84,7 @@ public:
         m_sbi.m_gravity.setValue(0, -10, 0);
         m_internalTime = 0.0;
         m_implicit = true;
+        m_selfCollision = true;
     }
 
     void setSolverCallback(btSolverCallback cb)
@@ -156,6 +158,8 @@ public:
     
     void sortConstraints();
     
+    void softBodySelfCollision();
+    
     void setImplicit(bool implicit)
     {
         m_implicit = implicit;

src/BulletSoftBody/btSoftBody.cpp

@@ -3515,6 +3515,18 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
                                                     docollide.psb[1]->m_fdbvt.m_root,
                                                     docollide);
             }
+            else
+            {
+                btSoftColliders::CollideVF_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,
+                                                    docollide.psb[1]->m_fdbvt.m_root,
+                                                    docollide);
+            }
         }
         break;
 		default:

src/BulletSoftBody/btSoftBodyInternals.h

@@ -1255,6 +1255,12 @@ struct btSoftColliders
 		{
 			btSoftBody::Node* node = (btSoftBody::Node*)lnode->data;
 			btSoftBody::Face* face = (btSoftBody::Face*)lface->data;
+            for (int i = 0; i < 3; ++i)
+            {
+                if (face->m_n[i] == node)
+                    continue;
+            }
+            
 			btVector3 o = node->m_x;
 			btVector3 p;
 			btScalar d = SIMD_INFINITY;
@@ -1318,7 +1324,8 @@ 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 = 2;
+            btScalar  scale = 1;
+            // 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;