switched to deformable rigid contact from Jacobi to Gauss Seidel

src/BulletSoftBody/btDeformableBackwardEulerObjective.h

@@ -71,7 +71,7 @@ public:
     
     void setDt(btScalar dt);
     
-    // enforce constraints in CG solve
+    // enforce constraints in contact solve
     void enforceConstraint(TVStack& x)
     {
         BT_PROFILE("enforceConstraint");

src/BulletSoftBody/btDeformableBodySolver.cpp

@@ -24,7 +24,7 @@ btDeformableBodySolver::btDeformableBodySolver()
 , m_cg(20)
 , m_maxNewtonIterations(5)
 , m_newtonTolerance(1e-4)
-, m_lineSearch(false)
+, m_lineSearch(true)
 {
     m_objective = new btDeformableBackwardEulerObjective(m_softBodySet, m_backupVelocity);
 }
@@ -69,12 +69,14 @@ void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
             {
                 break;
             }
+            // todo xuchenhan@: this really only needs to be calculated once
             m_objective->applyDynamicFriction(m_residual);
             if (m_lineSearch)
             {
                 btScalar inner_product = computeDescentStep(m_ddv,m_residual);
                 btScalar alpha = 0.01, beta = 0.5; // Boyd & Vandenberghe suggested alpha between 0.01 and 0.3, beta between 0.1 to 0.8
                 btScalar scale = 2;
+                // todo xuchenhan@: add damping energy to f0 and f1
                 btScalar f0 = m_objective->totalEnergy()+kineticEnergy(), f1, f2;
                 backupDv();
                 do {
@@ -239,14 +241,18 @@ btScalar btDeformableBodySolver::solveContactConstraints()
         m_dv[i].setZero();
     }
     btScalar maxSquaredResidual = m_objective->projection.update();
-    m_objective->enforceConstraint(m_dv);
+//    m_objective->enforceConstraint(m_dv);
-//    std::cout << "=================" << std::endl;
+//    m_objective->updateVelocity(m_dv);
-//    for (int i = 0; i < m_dv.size(); ++i)
+    int counter = 0;
-//    {
+    for (int i = 0; i < m_softBodySet.size(); ++i)
-//        std::cout << m_dv[i].getX() << " " << m_dv[i].getY() << " " << m_dv[i].getZ() << std::endl;
+    {
-//    }
+        btSoftBody* psb = m_softBodySet[i];
-    
+        for (int j = 0; j < psb->m_nodes.size(); ++j)
-    m_objective->updateVelocity(m_dv);
+        {
+            m_dv[counter] = psb->m_nodes[j].m_v - m_backupVelocity[counter];
+            ++counter;
+        }
+    }
     return maxSquaredResidual;
 }
 
@@ -374,7 +380,7 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d
         psb->m_bUpdateRtCst = false;
         psb->updateConstants();
         psb->m_fdbvt.clear();
-        if (psb->m_cfg.collisions & btSoftBody::fCollision::SDF_RDF)
+        if (psb->m_cfg.collisions & btSoftBody::fCollision::SDF_RD)
         {
             psb->initializeFaceTree();
         }

src/BulletSoftBody/btDeformableContactConstraint.cpp

@@ -15,6 +15,7 @@
 
 #include "btDeformableContactConstraint.h"
 
+/* ================   Deformable vs. Rigid   =================== */
 btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c)
 : m_contact(&c)
 , btDeformableContactConstraint(c.m_cti.m_normal)
@@ -91,6 +92,8 @@ btScalar btDeformableRigidContactConstraint::solveConstraint()
     btVector3 vb = getVb();
     btVector3 vr = vb - va;
     const btScalar dn = btDot(vr, cti.m_normal);
+//    if (dn > 0)
+//        return 0;
     // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
     btScalar residualSquare = dn*dn;
     btVector3 impulse = m_contact->m_c0 * vr;
@@ -133,7 +136,7 @@ btScalar btDeformableRigidContactConstraint::solveConstraint()
         }
     }
     impulse = impulse_normal + impulse_tangent;
-    
+    applyImpulse(impulse);
     
     if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
     {
@@ -166,6 +169,7 @@ btScalar btDeformableRigidContactConstraint::solveConstraint()
     return residualSquare;
 }
 
+/* ================   Node vs. Rigid   =================== */
 btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact)
     : m_node(contact.m_node)
     , btDeformableRigidContactConstraint(contact)
@@ -184,7 +188,46 @@ btVector3 btDeformableNodeRigidContactConstraint::getVb() const
 }
 
 
-btVector3 btDeformableNodeRigidContactConstraint::getDv(btSoftBody::Node* node) const
+btVector3 btDeformableNodeRigidContactConstraint::getDv(const btSoftBody::Node* node) const
 {
     return m_total_normal_dv + m_total_tangent_dv;
 }
+
+/* ================   Face vs. Rigid   =================== */
+btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact)
+: m_face(contact.m_face)
+, m_solved(false)
+, btDeformableRigidContactConstraint(contact)
+{
+}
+
+btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other)
+: m_face(other.m_face)
+, m_solved(false)
+, btDeformableRigidContactConstraint(other)
+{
+}
+
+btVector3 btDeformableFaceRigidContactConstraint::getVb() const
+{
+    const btSoftBody::DeformableFaceRigidContact* contact = getContact();
+    btVector3 vb = m_face->m_n[0]->m_v * contact->m_bary[0] + m_face->m_n[1]->m_v * contact->m_bary[1] + m_face->m_n[2]->m_v * contact->m_bary[2];
+    return vb;
+}
+
+
+btVector3 btDeformableFaceRigidContactConstraint::getDv(const btSoftBody::Node* node) const
+{
+    btVector3 face_dv = m_total_normal_dv + m_total_tangent_dv;
+    const btSoftBody::DeformableFaceRigidContact* contact = getContact();
+    if (m_face->m_n[0] == node)
+    {
+        return face_dv * contact->m_weights[0];
+    }
+    if (m_face->m_n[1] == node)
+    {
+        return face_dv * contact->m_weights[1];
+    }
+    btAssert(node == m_face->m_n[2]);
+    return face_dv * contact->m_weights[2];
+}

src/BulletSoftBody/btDeformableContactConstraint.h

@@ -58,7 +58,7 @@ public:
     virtual btVector3 getVb() const = 0;
     
     // get the velocity change of the soft body node in the constraint
-    virtual btVector3 getDv(btSoftBody::Node*) const = 0;
+    virtual btVector3 getDv(const btSoftBody::Node*) const = 0;
 };
 
 class btDeformableStaticConstraint : public btDeformableContactConstraint
@@ -96,7 +96,7 @@ public:
         return btVector3(0,0,0);
     }
     
-    virtual btVector3 getDv(btSoftBody::Node* n) const
+    virtual btVector3 getDv(const btSoftBody::Node* n) const
     {
         return btVector3(0,0,0);
     }
@@ -120,6 +120,8 @@ public:
     virtual btVector3 getVa() const;
     
     virtual btScalar solveConstraint();
+    
+    virtual void applyImpulse(const btVector3& impulse) = 0;
 };
 
 
@@ -135,16 +137,59 @@ public:
     
     virtual ~btDeformableNodeRigidContactConstraint()
     {
-        
     }
     
     virtual btVector3 getVb() const;
     
-    virtual btVector3 getDv(btSoftBody::Node*) const;
+    virtual btVector3 getDv(const btSoftBody::Node*) const;
     
     const btSoftBody::DeformableNodeRigidContact* getContact() const
     {
         return static_cast<const btSoftBody::DeformableNodeRigidContact*>(m_contact);
     }
+    
+    virtual void applyImpulse(const btVector3& impulse)
+    {
+        const btSoftBody::DeformableNodeRigidContact* contact = getContact();
+        btVector3 dv = impulse * contact->m_c2;
+        contact->m_node->m_v -= dv;
+    }
+};
+
+
+class btDeformableFaceRigidContactConstraint : public btDeformableRigidContactConstraint
+{
+public:
+    const btSoftBody::Face* m_face;
+    bool m_solved;
+    btDeformableFaceRigidContactConstraint(){}
+    btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact);
+    btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other);
+    
+    virtual ~btDeformableFaceRigidContactConstraint()
+    {
+    }
+    
+    virtual btVector3 getVb() const;
+    
+    virtual btVector3 getDv(const btSoftBody::Node*) const;
+    
+    const btSoftBody::DeformableFaceRigidContact* getContact() const
+    {
+        return static_cast<const btSoftBody::DeformableFaceRigidContact*>(m_contact);
+    }
+    
+    virtual void applyImpulse(const btVector3& impulse)
+    {
+        const btSoftBody::DeformableFaceRigidContact* contact = getContact();
+        btVector3 dv = impulse * contact->m_c2;
+        btSoftBody::Face* face = contact->m_face;
+        if (face->m_n[0]->m_im > 0)
+            face->m_n[0]->m_v -= dv * contact->m_weights[0];
+        if (face->m_n[1]->m_im > 0)
+            face->m_n[1]->m_v -= dv * contact->m_weights[1];
+        if (face->m_n[2]->m_im > 0)
+            face->m_n[2]->m_v -= dv * contact->m_weights[2];
+    }
 };
 #endif /* BT_DEFORMABLE_CONTACT_CONSTRAINT_H */

src/BulletSoftBody/btDeformableContactProjection.cpp

@@ -20,8 +20,7 @@
 btScalar btDeformableContactProjection::update()
 {
     btScalar residualSquare = 0;
-    // loop through constraints to set constrained values
+
-    
     // node constraints
     for (int index = 0; index < m_nodeRigidConstraints.size(); ++index)
     {
@@ -34,15 +33,14 @@ btScalar btDeformableContactProjection::update()
     }
     
     // face constraints
-//    for (int index = 0; index < m_faceRigidConstraints.size(); ++index)
+    for (int index = 0; index < m_allFaceConstraints.size(); ++index)
-//    {
+    {
-//        btAlignedObjectArray<btDeformableFaceRigidContactConstraint>& constraints = *m_faceRigidConstraints.getAtIndex(index);
+        btDeformableFaceRigidContactConstraint* constraint = m_allFaceConstraints[index];
-//        for (int i = 0; i < constraints.size(); ++i)
+        btScalar localResidualSquare = constraint->solveConstraint();
-//        {
+        residualSquare = btMax(residualSquare, localResidualSquare);
-//            btScalar localResidualSquare = constraints[i].solveConstraint();
+    }
-//            residualSquare = btMax(residualSquare, localResidualSquare);
+    
-//        }
+    // todo xuchenhan@: deformable/deformable constraints
-//    }
     return residualSquare;
 }
 
@@ -68,8 +66,6 @@ void btDeformableContactProjection::setConstraints()
     for (int i = 0; i < m_softBodies.size(); ++i)
     {
         btSoftBody* psb = m_softBodies[i];
-        btMultiBodyJacobianData jacobianData_normal;
-        btMultiBodyJacobianData jacobianData_complementary;
         for (int j = 0; j < psb->m_nodeRigidContacts.size(); ++j)
         {
             const btSoftBody::DeformableNodeRigidContact& contact = psb->m_nodeRigidContacts[j];
@@ -99,38 +95,142 @@ void btDeformableContactProjection::setConstraints()
                 }
             }
         }
+        
+         // set Deformable Face vs. Rigid constraint
+        for (int j = 0; j < psb->m_faceRigidContacts.size(); ++j)
+        {
+            const btSoftBody::DeformableFaceRigidContact& contact = psb->m_faceRigidContacts[j];
+            // skip fixed faces
+            if (contact.m_c2 == 0)
+            {
+                continue;
+            }
+            btDeformableFaceRigidContactConstraint* constraint = new btDeformableFaceRigidContactConstraint(contact);
+            btVector3 va = constraint->getVa();
+            btVector3 vb = constraint->getVb();
+            const btVector3 vr = vb - va;
+            const btSoftBody::sCti& cti = contact.m_cti;
+            const btScalar dn = btDot(vr, cti.m_normal);
+            if (dn < SIMD_EPSILON)
+            {
+                m_allFaceConstraints.push_back(constraint);
+                // add face constraints to each of the nodes
+                for (int k = 0; k < 3; ++k)
+                {
+                    btSoftBody::Node* node = contact.m_face->m_n[k];
+                    // static node does not need to own face/rigid constraint
+                    if (node->m_im != 0)
+                    {
+                        if (m_faceRigidConstraints.find(node->index) == NULL)
+                        {
+                            btAlignedObjectArray<btDeformableFaceRigidContactConstraint*> constraintsList;
+                            constraintsList.push_back(constraint);
+                            m_faceRigidConstraints.insert(node->index, constraintsList);
+                        }
+                        else
+                        {
+                            btAlignedObjectArray<btDeformableFaceRigidContactConstraint*>& constraintsList = *m_faceRigidConstraints[node->index];
+                            constraintsList.push_back(constraint);
+                        }
+                    }
+                }
+            }
+            else
+            {
+                delete constraint;
+            }
+        }
     }
-    
-    // todo xuchenhan@: set Deformable Face vs. Rigid constraint
-    
     // todo xuchenhan@: set Deformable Face vs. Deformable Node
 }
 
 void btDeformableContactProjection::enforceConstraint(TVStack& x)
 {
-    for (int i = 0; i < x.size(); ++i)
+    // x is set to zero when passed in
+    
+    // loop through node constraints to add in contributions to dv
+    for (int index = 0; index < m_nodeRigidConstraints.size(); ++index)
     {
-        x[i].setZero();
+        btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraintsList = *m_nodeRigidConstraints.getAtIndex(index);
-        if (m_staticConstraints.find(i) != NULL)
+        // i is node index
+        int i = m_nodeRigidConstraints.getKeyAtIndex(index).getUid1();
+        int numConstraints = 1;
+//        int numConstraints = constraintsList.size();
+//        if (m_faceRigidConstraints.find(i) != NULL)
+//        {
+//            numConstraints += m_faceRigidConstraints[i]->size();
+//        }
+        for (int j = 0; j < constraintsList.size(); ++j)
         {
-            // if a node is fixed, dv = 0
+            const btDeformableNodeRigidContactConstraint& constraint = constraintsList[j];
-            continue;
+            x[i] += constraint.getDv(constraint.getContact()->m_node)/btScalar(numConstraints);
         }
-        if (m_nodeRigidConstraints.find(i) != NULL)
+    }
+    
+    // loop through face constraints to add in contributions to dv
+    // note that for each face constraint is owned by three nodes. Be careful here to only add the dv to the node that owns the constraint
+    for (int index = 0; index < m_faceRigidConstraints.size(); ++index)
+    {
+        btAlignedObjectArray<btDeformableFaceRigidContactConstraint*>& constraintsList = *m_faceRigidConstraints.getAtIndex(index);
+        // i is node index
+        int i = m_faceRigidConstraints.getKeyAtIndex(index).getUid1();
+        int numConstraints = 1;
+//        int numConstraints = constraintsList.size();
+//        if (m_nodeRigidConstraints.find(i) != NULL)
+//        {
+//            numConstraints += m_nodeRigidConstraints[i]->size();
+//        }
+        for (int j = 0; j < constraintsList.size(); ++j)
         {
-            btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraintsList = *m_nodeRigidConstraints[i];
+            const btDeformableFaceRigidContactConstraint* constraint = constraintsList[j];
-            for (int j = 0; j < constraintsList.size(); ++j)
+            const btSoftBody::Face* face = constraint->m_face;
+            btSoftBody::Node* node;
+            // find the node that owns the constraint
+            for (int k = 0; k < 3; ++k)
             {
-                const btDeformableNodeRigidContactConstraint& constraint = constraintsList[j];
+                if (face->m_n[k]->index == i)
-                x[i] += constraint.getDv(m_nodes->at(i));
+                {
+                    node = face->m_n[k];
+                    break;
+                }
             }
+            x[i] += constraint->getDv(node) / btScalar(numConstraints);
         }
+    }
+    // todo xuchenhan@: add deformable deformable constraints' contribution to dv
+    
+    // Finally, loop through static constraints to set dv of static nodes to zero
+    for (int index = 0; index < m_staticConstraints.size(); ++index)
+    {
+        const btDeformableStaticConstraint& constraint = *m_staticConstraints.getAtIndex(index);
+        int i = m_staticConstraints.getKeyAtIndex(index).getUid1();
+        x[i] = constraint.getDv(constraint.m_node);
+    }
+//
+//    for (int i = 0; i < x.size(); ++i)
+//    {
+//        x[i].setZero();
+//        if (m_staticConstraints.find(i) != NULL)
+//        {
+//            // if a node is fixed, dv = 0
+//            continue;
+//        }
+//        if (m_nodeRigidConstraints.find(i) != NULL)
+//        {
+//            btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraintsList = *m_nodeRigidConstraints[i];
+//            for (int j = 0; j < constraintsList.size(); ++j)
+//            {
+//                const btDeformableNodeRigidContactConstraint& constraint = constraintsList[j];
+////                x[i] += constraint.getDv(m_nodes->at(i));
+//                x[i] += constraint.getDv(constraint.getContact()->m_node);
+//            }
+//        }
         // todo xuchenhan@
 //        if (m_faceRigidConstraints.find(i) != NULL)
 //        {
 //
 //        }
-    }
+//    }
 }
 
 void btDeformableContactProjection::project(TVStack& x)
@@ -202,16 +302,27 @@ void btDeformableContactProjection::setProjection()
                         break;
                     }
                     const btVector3& local_normal = constraintsList[k].m_normal;
-                    // add another projection direction if it deviates from the average by more than about 15 degrees
                     normals.push_back(local_normal);
                     averagedNormal += local_normal;
                 }
             }
         
-            // todo: xuchenhan@ implement face
+            if (!existStaticConstraint && m_faceRigidConstraints.find(index) != NULL)
-//            if (!existStaticConstraint && m_faceRigidConstraints.find(index) != NULL)
+            {
-//            {
+                hasConstraint = true;
-//            }
+                btAlignedObjectArray<btDeformableFaceRigidContactConstraint*>& constraintsList = *m_faceRigidConstraints[index];
+                for (int k = 0; k < constraintsList.size(); ++k)
+                {
+                    if (constraintsList[k]->m_static)
+                    {
+                        existStaticConstraint = true;
+                        break;
+                    }
+                    const btVector3& local_normal = constraintsList[k]->m_normal;
+                    normals.push_back(local_normal);
+                    averagedNormal += local_normal;
+                }
+            }
             
             
             // build projections
@@ -252,6 +363,48 @@ void btDeformableContactProjection::setProjection()
 
 void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
 {
+    // loop over constraints
+    for (int i = 0; i < f.size(); ++i)
+    {
+        if (m_projectionsDict.find(i) != NULL)
+        {
+            // doesn't need to add friction force for fully constrained vertices
+            btAlignedObjectArray<btVector3>& projectionDirs = *m_projectionsDict[i];
+            if (projectionDirs.size() >= 3)
+            {
+                continue;
+            }
+        }
+        
+        if (m_nodeRigidConstraints.find(i) != NULL)
+        {
+            btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraintsList = *m_nodeRigidConstraints[i];
+            for (int j = 0; j < constraintsList.size(); ++j)
+            {
+                const btDeformableNodeRigidContactConstraint& constraint = constraintsList[j];
+                btSoftBody::Node* node = constraint.getContact()->m_node;
+                
+                // it's ok to add the friction force generated by the entire impulse here because the normal component of the residual will be projected out anyway.
+                f[i] += constraint.getDv(node)* (1./node->m_im);
+            }
+        }
+        // todo xuchenhan@
+        if (m_faceRigidConstraints.find(i) != NULL)
+        {
+            btAlignedObjectArray<btDeformableFaceRigidContactConstraint*>& constraintsList = *m_faceRigidConstraints[i];
+            for (int j = 0; j < constraintsList.size(); ++j)
+            {
+                const btDeformableFaceRigidContactConstraint* constraint = constraintsList[j];
+                btSoftBody::Face* face = constraint->getContact()->m_face;
+                
+                // it's ok to add the friction force generated by the entire impulse here because the normal component of the residual will be projected out anyway.
+                // todo xuchenhan@: figure out the index for m_faceRigidConstraints
+//                f[i] += constraint.getDv(face->m_n[0])* (1./face->m_n[0]->m_im);
+//                f[i] += constraint.getDv(face->m_n[1])* (1./face->m_n[1]->m_im);
+//                f[i] += constraint.getDv(face->m_n[2])* (1./face->m_n[2]->m_im);
+            }
+        }
+    }
 //    for (int index = 0; index < m_constraints.size(); ++index)
 //    {
 //        const DeformableContactConstraint& constraint = *m_constraints.getAtIndex(index);
@@ -286,7 +439,13 @@ void btDeformableContactProjection::reinitialize(bool nodeUpdated)
     btCGProjection::reinitialize(nodeUpdated);
     m_staticConstraints.clear();
     m_nodeRigidConstraints.clear();
-//    m_faceRigidConstraints.clear();
+    m_faceRigidConstraints.clear();
+    m_projectionsDict.clear();
+    for (int i = 0; i < m_allFaceConstraints.size(); ++i)
+    {
+        delete m_allFaceConstraints[i];
+    }
+    m_allFaceConstraints.clear();
 }
 
 

src/BulletSoftBody/btDeformableContactProjection.h

@@ -21,6 +21,7 @@
 #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
 #include "btDeformableContactConstraint.h"
 #include "LinearMath/btHashMap.h"
+#include <vector>
 class btDeformableContactProjection : public btCGProjection
 {
 public:
@@ -28,9 +29,10 @@ public:
     btHashMap<btHashInt, btDeformableStaticConstraint> m_staticConstraints;
     // map from node index to node rigid constraint
     btHashMap<btHashInt, btAlignedObjectArray<btDeformableNodeRigidContactConstraint> > m_nodeRigidConstraints;
-//    // map from node index to face rigid constraint
+    // map from node index to face rigid constraint
-//    btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceRigidContactConstraint> > m_faceRigidConstraints;
+    btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceRigidContactConstraint*> > m_faceRigidConstraints;
-
+    btAlignedObjectArray<btDeformableFaceRigidContactConstraint*> m_allFaceConstraints;
+    
     // map from node index to projection directions
     btHashMap<btHashInt, btAlignedObjectArray<btVector3> > m_projectionsDict;
     

src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp

@@ -39,6 +39,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlyIteration
                 printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration);
 #endif
                 m_analyticsData.m_numSolverCalls++;
+                std::cout << m_leastSquaresResidual << std::endl;
                 m_analyticsData.m_numIterationsUsed = iteration+1;
                 m_analyticsData.m_islandId = -2;
                 if (numBodies>0)

src/BulletSoftBody/btSoftBody.cpp

@@ -2368,7 +2368,7 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
     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;
+    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)
@@ -3431,7 +3431,7 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap
             docollideFace.m_rigidBody = prb1;
             docollideFace.dynmargin = basemargin + timemargin;
             docollideFace.stamargin = basemargin;
-            m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace);
+//            m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace);
         }
         break;
 	}

src/BulletSoftBody/btSoftBodyInternals.h

@@ -1104,6 +1104,7 @@ struct btSoftColliders
                     btSoftBody::sCti& cti = c.m_cti;
                     c.m_contactPoint = contact_point;
                     c.m_bary = bary;
+                    // todo xuchenhan@: check m_c2 and m_weights
                     c.m_weights = btScalar(2)/(btScalar(1) + bary.length2()) * bary;
                     c.m_face = &f;
                     const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();