limit num contacts reported. Add distance to Deformable Deformable contact

5 files changed, 13 insertions, 4 deletions

diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index dc2e89b66..aa2db7288 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -8134,8 +8134,10 @@ bool PhysicsServerCommandProcessor::processRequestDeformableDeformableContactpoi
     {
         return false;
     }
+    const int max_contacts_per_object = 4;
     for (int i = deformWorld->getSoftBodyArray().size() - 1; i >= 0; i--)
     {
+        int num_contacts_reported = 0;
         btSoftBody* psb = deformWorld->getSoftBodyArray()[i];
         for (int c = 0; c < psb->m_faceNodeContacts.size(); c++)
         {
@@ -8154,8 +8156,13 @@ bool PhysicsServerCommandProcessor::processRequestDeformableDeformableContactpoi
             if(RequestFiltered(clientCmd, linkIndexA, linkIndexB, objectIndexA, objectIndexB, swap)==true){
                 continue;
             }
+            if(++num_contacts_reported>max_contacts_per_object){
+                break;
+            }
             //Convert contact info
             b3ContactPointData pt;
+            btVector3 l = contact->m_node->m_x - BaryEval(contact->m_face->m_n[0]->m_x, contact->m_face->m_n[1]->m_x, contact->m_face->m_n[2]->m_x, contact->m_normal);
+            pt.m_contactDistance = -contact->m_margin + contact->m_normal.dot(l);
             pt.m_bodyUniqueIdA = objectIndexA;
             pt.m_bodyUniqueIdB = objectIndexB;
             pt.m_contactDistance = 0;
diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp
index 699c6bcad..9b92df006 100644
--- a/src/BulletSoftBody/btDeformableBodySolver.cpp
+++ b/src/BulletSoftBody/btDeformableBodySolver.cpp
@@ -411,6 +411,7 @@ void btDeformableBodySolver::predictMotion(btScalar solverdt)
 			psb->m_nodeRigidContacts.resize(0);
 			psb->m_faceRigidContacts.resize(0);
 			psb->m_faceNodeContacts.resize(0);
+            psb->m_faceNodeContactsCCD.resize(0);
 			// predict motion for collision detection
 			predictDeformableMotion(psb, solverdt);
 		}
diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
index 983e622b5..8b208d939 100644
--- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
+++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
@@ -201,7 +201,7 @@ void btDeformableMultiBodyDynamicsWorld::performGeometricCollisions(btScalar tim
 			if (psb->isActive())
 			{
 				// clear contact points in the previous iteration
-				psb->m_faceNodeContacts.clear();
+                psb->m_faceNodeContactsCCD.clear();
 
 				// update m_q and normals for CCD calculation
 				for (int j = 0; j < psb->m_nodes.size(); ++j)
@@ -237,7 +237,7 @@ void btDeformableMultiBodyDynamicsWorld::performGeometricCollisions(btScalar tim
 			btSoftBody* psb = m_softBodies[i];
 			if (psb->isActive())
 			{
-				penetration_count += psb->m_faceNodeContacts.size();
+                penetration_count += psb->m_faceNodeContactsCCD.size();;
 			}
 		}
 		if (penetration_count == 0)
diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h
index 742ae0276..45319f96e 100644
--- a/src/BulletSoftBody/btSoftBody.h
+++ b/src/BulletSoftBody/btSoftBody.h
@@ -824,6 +824,7 @@ public:
 	btAlignedObjectArray<DeformableNodeRigidContact> m_nodeRigidContacts;
 	btAlignedObjectArray<DeformableFaceNodeContact> m_faceNodeContacts;
 	btAlignedObjectArray<DeformableFaceRigidContact> m_faceRigidContacts;
+    btAlignedObjectArray<DeformableFaceNodeContact> m_faceNodeContactsCCD;
 	tSContactArray m_scontacts;     // Soft contacts
 	tJointArray m_joints;           // Joints
 	tMaterialArray m_materials;     // Materials
diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h
index 6ce5df7b6..be1a64b98 100644
--- a/src/BulletSoftBody/btSoftBodyInternals.h
+++ b/src/BulletSoftBody/btSoftBodyInternals.h
@@ -2053,7 +2053,7 @@ struct btSoftColliders
 				c.m_imf = 0;
 				c.m_c0 = 0;
 				c.m_colObj = psb[1];
-				psb[0]->m_faceNodeContacts.push_back(c);
+                psb[0]->m_faceNodeContactsCCD.push_back(c);
 			}
 		}
 		void Process(const btDbvntNode* lface1,
@@ -2118,7 +2118,7 @@ struct btSoftColliders
 					c.m_imf = 0;
 					c.m_c0 = 0;
 					c.m_colObj = psb[1];
-					psb[0]->m_faceNodeContacts.push_back(c);
+                    psb[0]->m_faceNodeContactsCCD.push_back(c);
 				}
 			}
 		}