restore behaviors of btSoftBody

examples/DeformableContact/DeformableContact.cpp

@@ -228,6 +228,7 @@ void DeformableContact::initPhysics()
         psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
         psb->m_cfg.kCHR = 1; // collision hardness with rigid body
         psb->m_cfg.kDF = .1;
+        psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
         getDeformableDynamicsWorld()->addSoftBody(psb);
         getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce());
         getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity));

examples/DeformableDemo/DeformableDemo.cpp

@@ -235,6 +235,7 @@ void DeformableDemo::initPhysics()
         psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
         psb->m_cfg.kCHR = 1; // collision hardness with rigid body
         psb->m_cfg.kDF = 1;
+        psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
         getDeformableDynamicsWorld()->addSoftBody(psb);
         getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce());
         getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity));

examples/Pinch/Pinch.cpp

@@ -342,6 +342,7 @@ void Pinch::initPhysics()
         psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
         psb->m_cfg.kCHR = 1; // collision hardness with rigid body
         psb->m_cfg.kDF = 2;
+        psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
         getDeformableDynamicsWorld()->addSoftBody(psb);
         getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce());
         getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity));

examples/VolumetricDeformable/VolumetricDeformable.cpp

@@ -241,6 +241,7 @@ void VolumetricDeformable::initPhysics()
         psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
         psb->m_cfg.kCHR = 1; // collision hardness with rigid body
         psb->m_cfg.kDF = 0.5;
+        psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
         getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce());
         getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity));
     }

src/BulletSoftBody/btDeformableBackwardEulerObjective.h

@@ -26,7 +26,6 @@ class btDeformableRigidDynamicsWorld;
 class btDeformableBackwardEulerObjective
 {
 public:
-//    using TVStack = btAlignedObjectArray<btVector3>;
     typedef btAlignedObjectArray<btVector3> TVStack;
     btScalar m_dt;
     btDeformableRigidDynamicsWorld* m_world;

src/BulletSoftBody/btDeformableBodySolver.cpp

@@ -75,7 +75,6 @@ void btDeformableBodySolver::setWorld(btDeformableRigidDynamicsWorld* world)
 
 void btDeformableBodySolver::updateVelocity()
 {
-    // serial implementation
     int counter = 0;
     for (int i = 0; i < m_softBodySet.size(); ++i)
     {
@@ -90,7 +89,6 @@ void btDeformableBodySolver::updateVelocity()
 
 void btDeformableBodySolver::backupVelocity()
 {
-    // serial implementation
     int counter = 0;
     for (int i = 0; i < m_softBodySet.size(); ++i)
     {
@@ -104,7 +102,6 @@ void btDeformableBodySolver::backupVelocity()
 
 void btDeformableBodySolver::revertVelocity()
 {
-    // serial implementation
     int counter = 0;
     for (int i = 0; i < m_softBodySet.size(); ++i)
     {

src/BulletSoftBody/btSoftBody.cpp

@@ -1758,7 +1758,6 @@ void btSoftBody::setSolver(eSolverPresets::_ preset)
 	}
 }
 
-//
 void btSoftBody::predictMotion(btScalar dt)
 {
     int i, ni;
@@ -1782,7 +1781,7 @@ void btSoftBody::predictMotion(btScalar dt)
     m_sst.radmrg = getCollisionShape()->getMargin();
     m_sst.updmrg = m_sst.radmrg * (btScalar)0.25;
     /* Forces                */
-//    addVelocity(m_worldInfo->m_gravity * m_sst.sdt);
+    addVelocity(m_worldInfo->m_gravity * m_sst.sdt);
     applyForces();
     /* Integrate            */
     for (i = 0, ni = m_nodes.size(); i < ni; ++i)
@@ -1867,6 +1866,7 @@ void btSoftBody::predictMotion(btScalar dt)
     m_cdbvt.optimizeIncremental(1);
 }
 
+
 //
 void btSoftBody::solveConstraints()
 {
@@ -2262,8 +2262,35 @@ btVector3 btSoftBody::evaluateCom() const
 	return (com);
 }
 
-//
 bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap,
+                              const btVector3& x,
+                              btScalar margin,
+                              btSoftBody::sCti& cti) const
+{
+    btVector3 nrm;
+    const btCollisionShape* shp = colObjWrap->getCollisionShape();
+    //    const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject());
+    //const btTransform &wtr = tmpRigid ? tmpRigid->getWorldTransform() : colObjWrap->getWorldTransform();
+    const btTransform& wtr = colObjWrap->getWorldTransform();
+    //todo: check which transform is needed here
+    
+    btScalar dst =
+    m_worldInfo->m_sparsesdf.Evaluate(
+                                      wtr.invXform(x),
+                                      shp,
+                                      nrm,
+                                      margin);
+    if (dst < 0)
+    {
+        cti.m_colObj = colObjWrap->getCollisionObject();
+        cti.m_normal = wtr.getBasis() * nrm;
+        cti.m_offset = -btDot(cti.m_normal, x - cti.m_normal * dst);
+        return (true);
+    }
+    return (false);
+}
+//
+bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWrap,
 							  const btVector3& x,
 							  btScalar margin,
 							  btSoftBody::sCti& cti, bool predict) const
@@ -3262,6 +3289,33 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap
 			collider.ProcessColObj(this, pcoWrap);
 		}
 		break;
+        case fCollision::SDF_RD:
+        {
+            btSoftColliders::CollideSDF_RD docollide;
+            btRigidBody* prb1 = (btRigidBody*)btRigidBody::upcast(pcoWrap->getCollisionObject());
+            btTransform wtr = pcoWrap->getWorldTransform();
+            
+            const btTransform ctr = pcoWrap->getWorldTransform();
+            const btScalar timemargin = (wtr.getOrigin() - ctr.getOrigin()).length();
+            const btScalar basemargin = getCollisionShape()->getMargin();
+            btVector3 mins;
+            btVector3 maxs;
+            ATTRIBUTE_ALIGNED16(btDbvtVolume)
+            volume;
+            pcoWrap->getCollisionShape()->getAabb(pcoWrap->getWorldTransform(),
+                                                  mins,
+                                                  maxs);
+            volume = btDbvtVolume::FromMM(mins, maxs);
+            volume.Expand(btVector3(basemargin, basemargin, basemargin));
+            docollide.psb = this;
+            docollide.m_colObj1Wrap = pcoWrap;
+            docollide.m_rigidBody = prb1;
+            
+            docollide.dynmargin = basemargin + timemargin;
+            docollide.stamargin = basemargin;
+            m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollide);
+        }
+        break;
 	}
 }
 

src/BulletSoftBody/btSoftBody.h

@@ -160,6 +160,7 @@ public:
 			RVSmask = 0x000f,  ///Rigid versus soft mask
 			SDF_RS = 0x0001,   ///SDF based rigid vs soft
 			CL_RS = 0x0002,    ///Cluster vs convex rigid vs soft
+            SDF_RD = 0x0003,   ///DF based rigid vs deformable
 
 			SVSmask = 0x0030,  ///Rigid versus soft mask
 			VF_SS = 0x0010,    ///Vertex vs face soft vs soft handling
@@ -1006,7 +1007,8 @@ public:
 				btScalar& mint, eFeature::_& feature, int& index, bool bcountonly) const;
 	void initializeFaceTree();
 	btVector3 evaluateCom() const;
-	bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const;
+	bool checkDeformableContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, 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();
 	void updatePose();

src/BulletSoftBody/btSoftBodyInternals.h

@@ -943,10 +943,62 @@ struct btSoftColliders
             const btScalar m = n.m_im > 0 ? dynmargin : stamargin;
             btSoftBody::RContact c;
             
+            if ((!n.m_battach) &&
+                psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti))
+            {
+                const btScalar ima = n.m_im;
+                const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
+                const btScalar ms = ima + imb;
+                if (ms > 0)
+                {
+                    const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
+                    static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
+                    const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
+                    const btVector3 ra = n.m_x - wtr.getOrigin();
+                    const btVector3 va = m_rigidBody ? m_rigidBody->getVelocityInLocalPoint(ra) * psb->m_sst.sdt : btVector3(0, 0, 0);
+                    const btVector3 vb = n.m_x - n.m_q;
+                    const btVector3 vr = vb - va;
+                    const btScalar dn = btDot(vr, c.m_cti.m_normal);
+                    const btVector3 fv = vr - c.m_cti.m_normal * dn;
+                    const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
+                    c.m_node = &n;
+                    c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra);
+                    c.m_c1 = ra;
+                    c.m_c2 = ima * psb->m_sst.sdt;
+                    c.m_c3 = fv.length2() < (dn * fc * dn * fc) ? 0 : 1 - fc;
+                    c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
+                    psb->m_rcontacts.push_back(c);
+                    if (m_rigidBody)
+                        m_rigidBody->activate();
+                }
+            }
+        }
+        btSoftBody* psb;
+        const btCollisionObjectWrapper* m_colObj1Wrap;
+        btRigidBody* m_rigidBody;
+        btScalar dynmargin;
+        btScalar stamargin;
+    };
+
+	//
+	// CollideSDF_RD
+	//
+	struct CollideSDF_RD : btDbvt::ICollide
+	{
+		void Process(const btDbvtNode* leaf)
+		{
+			btSoftBody::Node* node = (btSoftBody::Node*)leaf->data;
+			DoNode(*node);
+		}
+		void DoNode(btSoftBody::Node& n) const
+		{
+			const btScalar m = n.m_im > 0 ? dynmargin : stamargin;
+			btSoftBody::RContact c;
+
 			if (!n.m_battach)
             {
                 // check for collision at x_{n+1}^*
-                if (psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predicted = */ true))
+                if (psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predicted = */ true))
                 {
                     const btScalar ima = n.m_im;
                     const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
@@ -954,7 +1006,7 @@ struct btSoftColliders
                     if (ms > 0)
                     {
                         // resolve contact at x_n
-                        psb->checkContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predicted = */ false);
+                        psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predicted = */ false);
                         btSoftBody::sCti& cti = c.m_cti;
                         c.m_node = &n;
                         const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();