diff options
author | Johannes Brust <johannes.brust@gmail.com> | 2021-10-21 17:38:13 +0200 |
---|---|---|
committer | Johannes Brust <johannes.brust@gmail.com> | 2021-10-21 17:38:13 +0200 |
commit | 61d5bdb783d8673eabedd10f660cb222b6a286fd (patch) | |
tree | 5d50a01d6ba5bd2c75d50c1c9623966092910094 | |
parent | 5795bd676f285c5e830218d73181dbf098a03cb4 (diff) | |
download | bullet3-61d5bdb783d8673eabedd10f660cb222b6a286fd.tar.gz |
Fix bug due to missing swap
-rw-r--r-- | examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 117 |
1 files changed, 59 insertions, 58 deletions
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 7d318d05a..ec7cb6181 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -8078,74 +8078,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; @@ -8188,12 +8188,13 @@ bool PhysicsServerCommandProcessor::processRequestDeformableContactpointHelper(c 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(); + // add normal force of additional node contact + 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 - if (!normalForce.fuzzyZero()) + if (!normalForce.fuzzyZero()) { // normalize for unit vectors if above numerical threshold normalForce.normalize(); |