restore behaviors of btSoftBody

9 files changed, 221 insertions, 113 deletions

diff --git a/examples/DeformableContact/DeformableContact.cpp b/examples/DeformableContact/DeformableContact.cpp
index c40564867..102ad40a9 100644
--- a/examples/DeformableContact/DeformableContact.cpp
+++ b/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));
diff --git a/examples/DeformableDemo/DeformableDemo.cpp b/examples/DeformableDemo/DeformableDemo.cpp
index 2411670cf..a3cdbf847 100644
--- a/examples/DeformableDemo/DeformableDemo.cpp
+++ b/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));
diff --git a/examples/Pinch/Pinch.cpp b/examples/Pinch/Pinch.cpp
index 1e4afb748..c6e26ddb1 100644
--- a/examples/Pinch/Pinch.cpp
+++ b/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));
diff --git a/examples/VolumetricDeformable/VolumetricDeformable.cpp b/examples/VolumetricDeformable/VolumetricDeformable.cpp
index 15fb712f6..b30e4ac4d 100644
--- a/examples/VolumetricDeformable/VolumetricDeformable.cpp
+++ b/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));
     }
diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h
index 82af57f6d..30c2d995e 100644
--- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h
+++ b/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;
diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp
index 242b0a882..fc792bd7f 100644
--- a/src/BulletSoftBody/btDeformableBodySolver.cpp
+++ b/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)
     {
diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp
index 8a62d31bb..86a48bf62 100644
--- a/src/BulletSoftBody/btSoftBody.cpp
+++ b/src/BulletSoftBody/btSoftBody.cpp
@@ -1758,115 +1758,115 @@ void btSoftBody::setSolver(eSolverPresets::_ preset)
 	}
 }
 
-//
 void btSoftBody::predictMotion(btScalar dt)
 {
-	int i, ni;
-
-	/* Update				*/
-	if (m_bUpdateRtCst)
-	{
-		m_bUpdateRtCst = false;
-		updateConstants();
-		m_fdbvt.clear();
-		if (m_cfg.collisions & fCollision::VF_SS)
-		{
-			initializeFaceTree();
-		}
-	}
-
-	/* Prepare				*/
-	m_sst.sdt = dt * m_cfg.timescale;
-	m_sst.isdt = 1 / m_sst.sdt;
-	m_sst.velmrg = m_sst.sdt * 3;
-	m_sst.radmrg = getCollisionShape()->getMargin();
-	m_sst.updmrg = m_sst.radmrg * (btScalar)0.25;
-	/* Forces				*/
-//    addVelocity(m_worldInfo->m_gravity * m_sst.sdt);
-	applyForces();
-	/* Integrate			*/
-	for (i = 0, ni = m_nodes.size(); i < ni; ++i)
-	{
-		Node& n = m_nodes[i];
-		n.m_q = n.m_x;
-		btVector3 deltaV = n.m_f * n.m_im * m_sst.sdt;
-		{
-			btScalar maxDisplacement = m_worldInfo->m_maxDisplacement;
-			btScalar clampDeltaV = maxDisplacement / m_sst.sdt;
-			for (int c = 0; c < 3; c++)
-			{
-				if (deltaV[c] > clampDeltaV)
-				{
-					deltaV[c] = clampDeltaV;
-				}
-				if (deltaV[c] < -clampDeltaV)
-				{
-					deltaV[c] = -clampDeltaV;
-				}
-			}
-		}
+    int i, ni;
+    
+    /* Update                */
+    if (m_bUpdateRtCst)
+    {
+        m_bUpdateRtCst = false;
+        updateConstants();
+        m_fdbvt.clear();
+        if (m_cfg.collisions & fCollision::VF_SS)
+        {
+            initializeFaceTree();
+        }
+    }
+    
+    /* Prepare                */
+    m_sst.sdt = dt * m_cfg.timescale;
+    m_sst.isdt = 1 / m_sst.sdt;
+    m_sst.velmrg = m_sst.sdt * 3;
+    m_sst.radmrg = getCollisionShape()->getMargin();
+    m_sst.updmrg = m_sst.radmrg * (btScalar)0.25;
+    /* Forces                */
+    addVelocity(m_worldInfo->m_gravity * m_sst.sdt);
+    applyForces();
+    /* Integrate            */
+    for (i = 0, ni = m_nodes.size(); i < ni; ++i)
+    {
+        Node& n = m_nodes[i];
+        n.m_q = n.m_x;
+        btVector3 deltaV = n.m_f * n.m_im * m_sst.sdt;
+        {
+            btScalar maxDisplacement = m_worldInfo->m_maxDisplacement;
+            btScalar clampDeltaV = maxDisplacement / m_sst.sdt;
+            for (int c = 0; c < 3; c++)
+            {
+                if (deltaV[c] > clampDeltaV)
+                {
+                    deltaV[c] = clampDeltaV;
+                }
+                if (deltaV[c] < -clampDeltaV)
+                {
+                    deltaV[c] = -clampDeltaV;
+                }
+            }
+        }
         n.m_v += deltaV;
         n.m_x += n.m_v * m_sst.sdt;
-		n.m_f = btVector3(0, 0, 0);
-	}
-	/* Clusters				*/
-	updateClusters();
-	/* Bounds				*/
-	updateBounds();
-	/* Nodes				*/
-	ATTRIBUTE_ALIGNED16(btDbvtVolume)
-	vol;
-	for (i = 0, ni = m_nodes.size(); i < ni; ++i)
-	{
-		Node& n = m_nodes[i];
-		vol = btDbvtVolume::FromCR(n.m_x, m_sst.radmrg);
-		m_ndbvt.update(n.m_leaf,
-					   vol,
-					   n.m_v * m_sst.velmrg,
-					   m_sst.updmrg);
-	}
-	/* Faces				*/
-	if (!m_fdbvt.empty())
-	{
-		for (int i = 0; i < m_faces.size(); ++i)
-		{
-			Face& f = m_faces[i];
-			const btVector3 v = (f.m_n[0]->m_v +
-								 f.m_n[1]->m_v +
-								 f.m_n[2]->m_v) /
-								3;
-			vol = VolumeOf(f, m_sst.radmrg);
-			m_fdbvt.update(f.m_leaf,
-						   vol,
-						   v * m_sst.velmrg,
-						   m_sst.updmrg);
-		}
-	}
-	/* Pose					*/
-	updatePose();
-	/* Match				*/
-	if (m_pose.m_bframe && (m_cfg.kMT > 0))
-	{
-		const btMatrix3x3 posetrs = m_pose.m_rot;
-		for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
-		{
-			Node& n = m_nodes[i];
-			if (n.m_im > 0)
-			{
-				const btVector3 x = posetrs * m_pose.m_pos[i] + m_pose.m_com;
-				n.m_x = Lerp(n.m_x, x, m_cfg.kMT);
-			}
-		}
-	}
-	/* Clear contacts		*/
-	m_rcontacts.resize(0);
-	m_scontacts.resize(0);
-	/* Optimize dbvt's		*/
-	m_ndbvt.optimizeIncremental(1);
-	m_fdbvt.optimizeIncremental(1);
-	m_cdbvt.optimizeIncremental(1);
+        n.m_f = btVector3(0, 0, 0);
+    }
+    /* Clusters                */
+    updateClusters();
+    /* Bounds                */
+    updateBounds();
+    /* Nodes                */
+    ATTRIBUTE_ALIGNED16(btDbvtVolume)
+    vol;
+    for (i = 0, ni = m_nodes.size(); i < ni; ++i)
+    {
+        Node& n = m_nodes[i];
+        vol = btDbvtVolume::FromCR(n.m_x, m_sst.radmrg);
+        m_ndbvt.update(n.m_leaf,
+                       vol,
+                       n.m_v * m_sst.velmrg,
+                       m_sst.updmrg);
+    }
+    /* Faces                */
+    if (!m_fdbvt.empty())
+    {
+        for (int i = 0; i < m_faces.size(); ++i)
+        {
+            Face& f = m_faces[i];
+            const btVector3 v = (f.m_n[0]->m_v +
+                                 f.m_n[1]->m_v +
+                                 f.m_n[2]->m_v) /
+            3;
+            vol = VolumeOf(f, m_sst.radmrg);
+            m_fdbvt.update(f.m_leaf,
+                           vol,
+                           v * m_sst.velmrg,
+                           m_sst.updmrg);
+        }
+    }
+    /* Pose                    */
+    updatePose();
+    /* Match                */
+    if (m_pose.m_bframe && (m_cfg.kMT > 0))
+    {
+        const btMatrix3x3 posetrs = m_pose.m_rot;
+        for (int i = 0, ni = m_nodes.size(); i < ni; ++i)
+        {
+            Node& n = m_nodes[i];
+            if (n.m_im > 0)
+            {
+                const btVector3 x = posetrs * m_pose.m_pos[i] + m_pose.m_com;
+                n.m_x = Lerp(n.m_x, x, m_cfg.kMT);
+            }
+        }
+    }
+    /* Clear contacts        */
+    m_rcontacts.resize(0);
+    m_scontacts.resize(0);
+    /* Optimize dbvt's        */
+    m_ndbvt.optimizeIncremental(1);
+    m_fdbvt.optimizeIncremental(1);
+    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;
 	}
 }
 
diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h
index 87633a83c..f2934c94a 100644
--- a/src/BulletSoftBody/btSoftBody.h
+++ b/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();
diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h
index eb487a56f..80545ebad 100644
--- a/src/BulletSoftBody/btSoftBodyInternals.h
+++ b/src/BulletSoftBody/btSoftBodyInternals.h
@@ -928,10 +928,62 @@ struct btSoftColliders
 			psa->m_cdbvt.collideTT(psa->m_cdbvt.m_root, psb->m_cdbvt.m_root, *this);
 		}
 	};
+    //
+    // CollideSDF_RS
+    //
+    struct CollideSDF_RS : 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) &&
+                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_RS
+	// CollideSDF_RD
 	//
-	struct CollideSDF_RS : btDbvt::ICollide
+	struct CollideSDF_RD : btDbvt::ICollide
 	{
 		void Process(const btDbvtNode* leaf)
 		{
@@ -946,7 +998,7 @@ struct btSoftColliders
 			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();