perform position correction only when objects are penetrating

3 files changed, 11 insertions, 9 deletions

diff --git a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
index 6ff5a9a41..0b8807366 100644
--- a/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
+++ b/src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
@@ -109,12 +109,14 @@ void btDeformableRigidDynamicsWorld::positionCorrection(btScalar dt)
                 if (cti.m_colObj->hasContactResponse())
                 {
                     btScalar dp = cti.m_offset;
-                    if (friction.m_static[j] == true)
-                    {
-                        c->m_node->m_v = va;
-                    }
+                    
+                    // only perform position correction when penetrating
                     if (dp < 0)
                     {
+                        if (friction.m_static[j] == true)
+                        {
+                            c->m_node->m_v = va;
+                        }
                         c->m_node->m_v -= dp * cti.m_normal / dt;
                     }
                 }
diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp
index 06e0b41d0..8a62d31bb 100644
--- a/src/BulletSoftBody/btSoftBody.cpp
+++ b/src/BulletSoftBody/btSoftBody.cpp
@@ -2271,9 +2271,9 @@ bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap,
 	btVector3 nrm;
 	const btCollisionShape* shp = colObjWrap->getCollisionShape();
     const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject();
-    // get the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg
+    // 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
     const btTransform &wtr = (predict) ? tmpCollisionObj->getInterpolationWorldTransform() : colObjWrap->getWorldTransform();
-//    const btTransform &wtr = colObjWrap->getWorldTransform();
 
 	btScalar dst =
 		m_worldInfo->m_sparsesdf.Evaluate(
@@ -2281,13 +2281,14 @@ bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap,
 			shp,
 			nrm,
 			margin);
-	if (dst < 0 || !predict)
+	if (!predict)
 	{
 		cti.m_colObj = colObjWrap->getCollisionObject();
 		cti.m_normal = wtr.getBasis() * nrm;
         cti.m_offset = dst;
-		return (true);
 	}
+    if (dst < 0)
+        return true;
 	return (false);
 }
 
diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h
index 34302ccc4..61dff4234 100644
--- a/src/BulletSoftBody/btSoftBodyInternals.h
+++ b/src/BulletSoftBody/btSoftBodyInternals.h
@@ -947,7 +947,6 @@ struct btSoftColliders
             {
                 // check for collision at x_{n+1}^*
                 if (psb->checkContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predicted = */ true))
-//                if (psb->checkContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predicted = */ false));
                 {
                     const btScalar ima = n.m_im;
                     const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;