Fix bug due to missing swap

(cherry picked from commit 61d5bdb)
imstylen · Dec 19, 2022 · 60512c7 · 60512c7
1 parent fa1d1fc
commit 60512c7
Showing 1 changed file with 57 additions and 56 deletions.
diff --git 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