From 3b945597d1ddbbc2bc7e37c0797b5a3a50165029 Mon Sep 17 00:00:00 2001
From: Xuchen Han <xuchenhan2015@u.northwestern.edu>
Date: Mon, 7 Oct 2019 14:40:11 -0700
Subject: [PATCH] prevent narrow phase collision detection between rigid and
 soft when both are sleeping

---
 src/BulletSoftBody/btSoftBody.cpp        | 67 +++++++++++++-----------
 src/BulletSoftBody/btSoftBodyInternals.h |  2 +
 2 files changed, 37 insertions(+), 32 deletions(-)

diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp
index ef3a201c5..debe95d61 100644
--- a/src/BulletSoftBody/btSoftBody.cpp
+++ b/src/BulletSoftBody/btSoftBody.cpp
@@ -2306,9 +2306,9 @@ bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWr
     const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject();
     // use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect
     // but resolve contact at x_n
-    //btTransform wtr = (predict) ?
-    //(colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
-                 //: colObjWrap->getWorldTransform();
+//    btTransform wtr = (predict) ?
+//    (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
+//                 : colObjWrap->getWorldTransform();
     const btTransform& wtr = colObjWrap->getWorldTransform();
 	btScalar dst =
 		m_worldInfo->m_sparsesdf.Evaluate(
@@ -3430,35 +3430,38 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap
         {
             
             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));
-            btSoftColliders::CollideSDF_RD docollideNode;
-            docollideNode.psb = this;
-            docollideNode.m_colObj1Wrap = pcoWrap;
-            docollideNode.m_rigidBody = prb1;
-            docollideNode.dynmargin = basemargin + timemargin;
-            docollideNode.stamargin = basemargin;
-            m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollideNode);
-            
-            btSoftColliders::CollideSDF_RDF docollideFace;
-            docollideFace.psb = this;
-            docollideFace.m_colObj1Wrap = pcoWrap;
-            docollideFace.m_rigidBody = prb1;
-            docollideFace.dynmargin = basemargin + timemargin;
-            docollideFace.stamargin = basemargin;
-            m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace);
+            if (pcoWrap->getCollisionObject()->isActive() || this->isActive())
+            {
+                const btTransform wtr = pcoWrap->getWorldTransform();
+//                const btTransform ctr = pcoWrap->getWorldTransform();
+//                const btScalar timemargin = (wtr.getOrigin() - ctr.getOrigin()).length();
+                const btScalar timemargin = 0;
+                const btScalar basemargin = getCollisionShape()->getMargin();
+                btVector3 mins;
+                btVector3 maxs;
+                ATTRIBUTE_ALIGNED16(btDbvtVolume)
+                volume;
+                pcoWrap->getCollisionShape()->getAabb(wtr,
+                                                      mins,
+                                                      maxs);
+                volume = btDbvtVolume::FromMM(mins, maxs);
+                volume.Expand(btVector3(basemargin, basemargin, basemargin));
+                btSoftColliders::CollideSDF_RD docollideNode;
+                docollideNode.psb = this;
+                docollideNode.m_colObj1Wrap = pcoWrap;
+                docollideNode.m_rigidBody = prb1;
+                docollideNode.dynmargin = basemargin + timemargin;
+                docollideNode.stamargin = basemargin;
+                m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollideNode);
+                
+                btSoftColliders::CollideSDF_RDF docollideFace;
+                docollideFace.psb = this;
+                docollideFace.m_colObj1Wrap = pcoWrap;
+                docollideFace.m_rigidBody = prb1;
+                docollideFace.dynmargin = basemargin + timemargin;
+                docollideFace.stamargin = basemargin;
+                m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace);
+            }
         }
         break;
 	}
diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h
index ff074eb43..afe9c7ea8 100644
--- a/src/BulletSoftBody/btSoftBodyInternals.h
+++ b/src/BulletSoftBody/btSoftBodyInternals.h
@@ -1074,6 +1074,7 @@ struct btSoftColliders
                 if (psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ true) || psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predict = */ true))
                 {
                     const btScalar ima = n.m_im;
+                    // todo: collision between multibody and fixed deformable node will be missed.
                     const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
                     const btScalar ms = ima + imb;
                     if (ms > 0)
@@ -1169,6 +1170,7 @@ struct btSoftColliders
             {
                 btScalar ima = n0->m_im + n1->m_im + n2->m_im;
                 const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
+                // todo: collision between multibody and fixed deformable face will be missed.
                 const btScalar ms = ima + imb;
                 if (ms > 0)
                 {