Add deformable contact forces info

- add contact information for collisions between rigid and soft bodies
- collisions between different soft bodies are not supported
- uses impulse acting on tetrahedral nodes for calculation of forces
- contact points are approximated by node positions
- multiple forces acting on identical nodes are accumulated

(cherry picked from commit 5795bd676f285c5e830218d73181dbf098a03cb4)

3 files changed, 152 insertions, 95 deletions

diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index 8275451ed..59fd491d3 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -2802,7 +2802,7 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld(int flags)
 	m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP = 0.2;  //need to check if there are artifacts with frictionERP
 	m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.00001;
 	m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 50;
-	if (flags & RESET_USE_REDUCED_DEFORMABLE_WORLD) 
+	if (flags & RESET_USE_REDUCED_DEFORMABLE_WORLD)
 	{
 		m_data->m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = 128;
 	}
@@ -5677,7 +5677,7 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S
 				separateRenderMesh = (psb->m_renderNodes.size() != 0);
 			}
             bool requestVelocity = clientCmd.m_updateFlags & B3_MESH_DATA_SIMULATION_MESH_VELOCITY;
-            
+
 			int numVertices = separateRenderMesh ? psb->m_renderNodes.size() : psb->m_nodes.size();
 			int maxNumVertices = bufferSizeInBytes / totalBytesPerVertex - 1;
 			int numVerticesRemaining = numVertices - clientCmd.m_requestMeshDataArgs.m_startingVertex;
@@ -8187,7 +8187,7 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc
 }
 
 bool RequestFiltered(const struct SharedMemoryCommand& clientCmd, int& linkIndexA, int& linkIndexB, int& objectIndexA, int& objectIndexB, bool& swap){
-   
+
     if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter >= 0)
     {
         if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter == objectIndexA)
@@ -8232,7 +8232,7 @@ bool RequestFiltered(const struct SharedMemoryCommand& clientCmd, int& linkIndex
     {
         return true;
     }
-    
+
     return false;
 }
 
@@ -8313,115 +8313,167 @@ bool PhysicsServerCommandProcessor::processRequestDeformableContactpointHelper(c
 	{
 		return false;
 	}
-	int numSoftbodyContact = 0;
-	for (int i = deformWorld->getSoftBodyArray().size() - 1; i >= 0; i--)
-	{
-		numSoftbodyContact += deformWorld->getSoftBodyArray()[i]->m_faceRigidContacts.size();
-	}
-	int num_contact_points = m_data->m_cachedContactPoints.size();
-	m_data->m_cachedContactPoints.reserve(num_contact_points + numSoftbodyContact);
 
 	for (int i = deformWorld->getSoftBodyArray().size() - 1; i >= 0; i--)
 	{
 		btSoftBody* psb = deformWorld->getSoftBodyArray()[i];
+		btAlignedObjectArray<b3ContactPointData> distinctContactPoints;
+		btAlignedObjectArray<btSoftBody::Node*> nodesInContact;
 		for (int c = 0; c < psb->m_faceRigidContacts.size(); c++)
 		{
 			const btSoftBody::DeformableFaceRigidContact* contact = &psb->m_faceRigidContacts[c];
-			//convert rigidbody contact
-			int linkIndexA = -1;
-			int linkIndexB = -1;
-			int objectIndexA = psb->getUserIndex2();
-
-			int objectIndexB = -1;
-			const btRigidBody* bodyB = btRigidBody::upcast(contact->m_cti.m_colObj);
-			if (bodyB)
-			{
-				objectIndexB = bodyB->getUserIndex2();
-			}
-			const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(contact->m_cti.m_colObj);
-			if (mblB && mblB->m_multiBody)
-			{
-				linkIndexB = mblB->m_link;
-				objectIndexB = mblB->m_multiBody->getUserIndex2();
-			}
+			// calculate normal and tangent impulse
+			btVector3 impulse = contact->m_cti.m_impulse;
+			btVector3 impulseNormal = impulse.dot(contact->m_cti.m_normal) * contact->m_cti.m_normal;
+			btVector3 impulseTangent = impulse - impulseNormal;
+			// get node in contact
+			int contactNodeIdx = contact->m_bary.maxAxis();
+			btSoftBody::Node* node = contact->m_face->m_n[contactNodeIdx];
+			// check if node is already in the list
+			int idx = nodesInContact.findLinearSearch2(node);
+			if (idx < 0)
+			{
+				// add new node and contact point
+				nodesInContact.push_back(node);
+				//convert rigidbody contact
+				int linkIndexA = -1;
+				int linkIndexB = -1;
+				int objectIndexA = psb->getUserIndex2();
 
-			//apply the filter, if the user provides it
-			bool swap = false;
-			if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter >= 0)
-			{
-				if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter == objectIndexA)
+				int objectIndexB = -1;
+				const btRigidBody* bodyB = btRigidBody::upcast(contact->m_cti.m_colObj);
+				if (bodyB)
 				{
-					swap = false;
+					objectIndexB = bodyB->getUserIndex2();
 				}
-				else if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter == objectIndexB)
+				const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(contact->m_cti.m_colObj);
+				if (mblB && mblB->m_multiBody)
 				{
-					swap = true;
+					linkIndexB = mblB->m_link;
+					objectIndexB = mblB->m_multiBody->getUserIndex2();
 				}
-				else
+
+				//apply the filter, if the user provides it
+				bool swap = false;
+				if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter >= 0)
 				{
-					continue;
+					if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter == objectIndexA)
+					{
+						swap = false;
+					}
+					else if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter == objectIndexB)
+					{
+						swap = true;
+					}
+					else
+					{
+						continue;
+					}
 				}
-			}
 
-			if (swap)
-			{
-				std::swap(objectIndexA, objectIndexB);
-				std::swap(linkIndexA, linkIndexB);
-			}
+				if (swap)
+				{
+					std::swap(objectIndexA, objectIndexB);
+					std::swap(linkIndexA, linkIndexB);
+				}
 
-			//apply the second object filter, if the user provides it
-			if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter >= 0)
-			{
-				if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter != objectIndexB)
+				//apply the second object filter, if the user provides it
+				if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter >= 0)
 				{
-					continue;
+					if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter != objectIndexB)
+					{
+						continue;
+					}
 				}
-			}
 
-			if (
-				(clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER) &&
-				clientCmd.m_requestContactPointArguments.m_linkIndexAIndexFilter != linkIndexA)
-			{
-				continue;
-			}
+				if (
+					(clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER) &&
+					clientCmd.m_requestContactPointArguments.m_linkIndexAIndexFilter != linkIndexA)
+				{
+					continue;
+				}
 
-			if (
-				(clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER) &&
-				clientCmd.m_requestContactPointArguments.m_linkIndexBIndexFilter != linkIndexB)
-			{
-				continue;
-			}
-			b3ContactPointData pt;
-			pt.m_bodyUniqueIdA = objectIndexA;
-			pt.m_bodyUniqueIdB = objectIndexB;
-			pt.m_contactDistance = contact->m_cti.m_offset;
-			pt.m_contactFlags = 0;
-			pt.m_linkIndexA = linkIndexA;
-			pt.m_linkIndexB = linkIndexB;
-			for (int j = 0; j < 3; j++)
-			{
-				if (swap)
+				if (
+					(clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER) &&
+					clientCmd.m_requestContactPointArguments.m_linkIndexBIndexFilter != linkIndexB)
 				{
-					pt.m_contactNormalOnBInWS[j] = -contact->m_cti.m_normal[j];
-					pt.m_positionOnAInWS[j] = contact->m_cti.m_normal[j];
-					pt.m_positionOnBInWS[j] = -contact->m_cti.m_normal[j];
+					continue;
 				}
-				else
+				b3ContactPointData pt;
+				pt.m_bodyUniqueIdA = objectIndexA;
+				pt.m_bodyUniqueIdB = objectIndexB;
+				pt.m_contactDistance = -contact->m_cti.m_offset;
+				pt.m_contactFlags = 0;
+				pt.m_linkIndexA = linkIndexA;
+				pt.m_linkIndexB = linkIndexB;
+				for (int j = 0; j < 3; j++)
 				{
-					pt.m_contactNormalOnBInWS[j] = contact->m_cti.m_normal[j];
-					pt.m_positionOnAInWS[j] = -contact->m_cti.m_normal[j];
-					pt.m_positionOnBInWS[j] = contact->m_cti.m_normal[j];
+					if (swap)
+					{
+						pt.m_contactNormalOnBInWS[j] = -contact->m_cti.m_normal[j];
+						pt.m_positionOnAInWS[j] = node->m_x[j] - pt.m_contactDistance * pt.m_contactNormalOnBInWS[j]; // not really precise because of margins in btSoftBody.cpp:line 2912
+						// node is force application point, therefore node position is contact point (not contact->m_contactPoint, because not equal to node)
+						pt.m_positionOnBInWS[j] = node->m_x[j];
+					}
+					else
+					{
+						pt.m_contactNormalOnBInWS[j] = contact->m_cti.m_normal[j];
+						// node is force application point, therefore node position is contact point (not contact->m_contactPoint, because not equal to node)
+						pt.m_positionOnAInWS[j] = node->m_x[j];
+						pt.m_positionOnBInWS[j] = node->m_x[j] - pt.m_contactDistance * pt.m_contactNormalOnBInWS[j]; // not really precise because of margins in btSoftBody.cpp:line 2912
+					}
+				}
+				pt.m_normalForce = (impulseNormal / m_data->m_physicsDeltaTime).norm();
+				pt.m_linearFrictionForce1 = (impulseTangent.dot(contact->t1) * contact->t1 / m_data->m_physicsDeltaTime).norm();
+				pt.m_linearFrictionForce2 = (impulseTangent.dot(contact->t2) * contact->t2 / m_data->m_physicsDeltaTime).norm();
+				for (int j = 0; j < 3; j++)
+				{
+					pt.m_linearFrictionDirection1[j] = contact->t1[j];
+					pt.m_linearFrictionDirection2[j] = contact->t2[j];
 				}
+				distinctContactPoints.push_back(pt);
 			}
-			pt.m_normalForce = 1;
-			pt.m_linearFrictionForce1 = 0;
-			pt.m_linearFrictionForce2 = 0;
-			for (int j = 0; j < 3; j++)
+			else
 			{
-				pt.m_linearFrictionDirection1[j] = 0;
-				pt.m_linearFrictionDirection2[j] = 0;
+				// add values to existing contact point
+				b3ContactPointData* pt = &distinctContactPoints[idx];
+				// current normal force of node
+				btVector3 normalForce = btVector3(btScalar(pt->m_contactNormalOnBInWS[0]),
+												  btScalar(pt->m_contactNormalOnBInWS[1]),
+												  btScalar(pt->m_contactNormalOnBInWS[2])) * pt->m_normalForce;
+				// add normal force of additional node contact
+				normalForce += contact->m_cti.m_normal * (impulseNormal / m_data->m_physicsDeltaTime).norm();
+				// get magnitude of normal force
+				pt->m_normalForce = normalForce.norm();
+				// get direction of normal force
+				if (!normalForce.fuzzyZero())
+				{
+					// normalize for unit vectors if above numerical threshold
+					normalForce.normalize();
+					for (int j = 0; j < 3; j++)
+					{
+						pt->m_contactNormalOnBInWS[j] = normalForce[j];
+					}
+				}
+
+				// add magnitudes of tangential forces in existing directions
+				btVector3 linearFrictionDirection1 = btVector3(btScalar(pt->m_linearFrictionDirection1[0]),
+															   btScalar(pt->m_linearFrictionDirection1[1]),
+															   btScalar(pt->m_linearFrictionDirection1[2]));
+				btVector3 linearFrictionDirection2 = btVector3(btScalar(pt->m_linearFrictionDirection2[0]),
+															   btScalar(pt->m_linearFrictionDirection2[1]),
+															   btScalar(pt->m_linearFrictionDirection2[2]));
+				pt->m_linearFrictionForce1 = (impulseTangent.dot(linearFrictionDirection1) * linearFrictionDirection1 / m_data->m_physicsDeltaTime).norm();
+				pt->m_linearFrictionForce2 = (impulseTangent.dot(linearFrictionDirection2) * linearFrictionDirection2 / m_data->m_physicsDeltaTime).norm();
 			}
-			m_data->m_cachedContactPoints.push_back(pt);
+		}
+
+		int num_contact_points = m_data->m_cachedContactPoints.size() + distinctContactPoints.size();
+		m_data->m_cachedContactPoints.reserve(num_contact_points);
+		// add points to contact points cache
+		for (int p = 0; p < distinctContactPoints.size(); p++)
+		{
+			m_data->m_cachedContactPoints.push_back(distinctContactPoints[p]);
 		}
 	}
 #endif
@@ -9929,7 +9981,7 @@ bool PhysicsServerCommandProcessor::processReducedDeformable(const UrdfReducedDe
 				softWorld->addSoftBody(rsb);
 			}
 		}
-		
+
 		*bodyUniqueId = m_data->m_bodyHandles.allocHandle();
 		InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(*bodyUniqueId);
 		bodyHandle->m_softBody = rsb;
@@ -9980,7 +10032,7 @@ bool PhysicsServerCommandProcessor::processReducedDeformable(const UrdfReducedDe
 			float scaling[4] = { 1,1,1,1 };
  			int instanceUid = m_data->m_guiHelper->registerGraphicsInstance(shapeUid1, position, orientation, color, scaling);
 			rsb->setUserIndex(instanceUid);
-			
+
 			if (m_data->m_enableTinyRenderer)
 			{
 				int texUid2 = m_data->m_pluginManager.getRenderInterface()->registerTexture(meshData.m_textureImage1, meshData.m_textureWidth, meshData.m_textureHeight);
@@ -10080,7 +10132,7 @@ bool PhysicsServerCommandProcessor::processReducedDeformable(const UrdfReducedDe
 				}
 			}
 		}
-		
+
 
 
 		btAlignedObjectArray<btVector3> vertices;
@@ -10093,7 +10145,7 @@ bool PhysicsServerCommandProcessor::processReducedDeformable(const UrdfReducedDe
 
 			for (int i = 0; i < rsb->m_faces.size(); i++)  // Foreach face
 			{
-				
+
 				for (int k = 0; k < 3; k++)  // Foreach vertex on a face
 				{
 					int currentIndex = i * 3 + k;
diff --git a/src/BulletSoftBody/btDeformableContactConstraint.cpp b/src/BulletSoftBody/btDeformableContactConstraint.cpp
index 09398d79a..9c8e72f50 100644
--- a/src/BulletSoftBody/btDeformableContactConstraint.cpp
+++ b/src/BulletSoftBody/btDeformableContactConstraint.cpp
@@ -268,7 +268,7 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv
 	{
 		dn += m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep;
 	}
-	// dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
+	// dn is the normal component of velocity difference. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
 	btVector3 impulse = m_contact->m_c0 * (vr + m_total_normal_dv * infoGlobal.m_deformable_cfm + ((m_penetration > 0) ? m_penetration / infoGlobal.m_timeStep * cti.m_normal : btVector3(0, 0, 0)));
 	if (!infoGlobal.m_splitImpulse)
 	{
@@ -487,6 +487,9 @@ void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impul
 	btVector3 dv = impulse * contact->m_c2;
 	btSoftBody::Face* face = contact->m_face;
 
+	// save applied impulse
+	contact->m_cti.m_impulse = impulse;
+
 	btVector3& v0 = face->m_n[0]->m_v;
 	btVector3& v1 = face->m_n[1]->m_v;
 	btVector3& v2 = face->m_n[2]->m_v;
diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h
index f91640acb..27705e600 100644
--- a/src/BulletSoftBody/btSoftBody.h
+++ b/src/BulletSoftBody/btSoftBody.h
@@ -223,10 +223,12 @@ public:
 	/* sCti is Softbody contact info	*/
 	struct sCti
 	{
-		const btCollisionObject* m_colObj; /* Rigid body			*/
-		btVector3 m_normal;                /* Outward normal		*/
-		btScalar m_offset;                 /* Offset from origin	*/
+		const btCollisionObject* m_colObj; /* Rigid body			        */
+		btVector3 m_normal;                /* Outward normal		        */
+		mutable btVector3 m_impulse;	   /* Applied impulse        	    */
+		btScalar m_offset;                 /* Offset from origin	        */
 		btVector3 m_bary;                  /* Barycentric weights for faces */
+		sCti() : m_impulse(0, 0, 0) {}
 	};
 
 	/* sMedium		*/
@@ -897,7 +899,7 @@ public:
 				   int node1) const;
 	bool checkLink(const Node* node0,
 				   const Node* node1) const;
-	/* Check for existring face												*/
+	/* Check for existing face												*/
 	bool checkFace(int node0,
 				   int node1,
 				   int node2) const;