Fix bug due to missing swap

(cherry picked from commit 61d5bdb783d8673eabedd10f660cb222b6a286fd)

1 files changed, 57 insertions, 56 deletions

diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index 59fd491d3..376e833a6 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -8331,74 +8331,74 @@ bool PhysicsServerCommandProcessor::processRequestDeformableContactpointHelper(c
 			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();
 
-				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();
-				}
-
-				//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)
-					{
-						swap = false;
-					}
-					else if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter == objectIndexB)
-					{
-						swap = true;
-					}
-					else
-					{
-						continue;
-					}
-				}
+			//apply the filter, if the user provides it
+			int linkIndexA = -1;
+			int linkIndexB = -1;
+			int objectIndexA = psb->getUserIndex2();
 
-				if (swap)
+			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();
+			}
+			bool swap = false;
+			if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter >= 0)
+			{
+				if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter == objectIndexA)
 				{
-					std::swap(objectIndexA, objectIndexB);
-					std::swap(linkIndexA, linkIndexB);
+					swap = false;
 				}
-
-				//apply the second object filter, if the user provides it
-				if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter >= 0)
+				else if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter == objectIndexB)
 				{
-					if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter != objectIndexB)
-					{
-						continue;
-					}
+					swap = true;
 				}
-
-				if (
-					(clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER) &&
-					clientCmd.m_requestContactPointArguments.m_linkIndexAIndexFilter != linkIndexA)
+				else
 				{
 					continue;
 				}
+			}
 
-				if (
-					(clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER) &&
-					clientCmd.m_requestContactPointArguments.m_linkIndexBIndexFilter != 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)
 				{
 					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;
+			}
+
+			if (idx < 0)
+			{
+				// add new node and contact point
+				nodesInContact.push_back(node);
 				b3ContactPointData pt;
 				pt.m_bodyUniqueIdA = objectIndexA;
 				pt.m_bodyUniqueIdB = objectIndexB;
@@ -8442,7 +8442,8 @@ bool PhysicsServerCommandProcessor::processRequestDeformableContactpointHelper(c
 												  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();
+				btScalar swapFactor = swap ? -1.0 : 1.0;
+				normalForce += swapFactor * 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