diff options
| author | erwincoumans <erwincoumans@google.com> | 2020-09-01 09:29:03 -0700 |
|---|---|---|
| committer | GitHub <noreply@github.com> | 2020-09-01 09:29:03 -0700 |
| commit | 8fab756efd877bc874c5c1c30921d5aded97c054 (patch) | |
| tree | 2d969e10520a210573c1327a1042341389185303 | |
| parent | e79e44db6c3efede15db5e7fa77c71f32ffd3367 (diff) | |
| parent | 6fe6da0deecea6619f4fc79143ad9577aea205db (diff) | |
| download | bullet3-8fab756efd877bc874c5c1c30921d5aded97c054.tar.gz | |
Merge pull request #3026 from erwincoumans/master
PyBullet: fix cases where contact point objects are swapped, reported, attempt to fix travis python detection
| -rw-r--r-- | examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 30 |
1 files changed, 27 insertions, 3 deletions
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 7b4193ac3..8dfca94d9 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -8068,6 +8068,21 @@ bool PhysicsServerCommandProcessor::processRequestContactpointInformationCommand virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper* colObj1Wrap, int partId1, int index1) { + const btCollisionObject* colObj = (btCollisionObject*)colObj0Wrap->getCollisionObject(); + const btMultiBodyLinkCollider* mbl = btMultiBodyLinkCollider::upcast(colObj); + int bodyUniqueId = -1; + if (mbl) + { + bodyUniqueId = mbl->m_multiBody->getUserIndex2(); + } + else + { + bodyUniqueId = colObj->getUserIndex2(); + } + + + bool isSwapped = m_bodyUniqueIdA != bodyUniqueId; + if (cp.m_distance1 <= m_closestDistanceThreshold) { b3ContactPointData pt; @@ -8080,9 +8095,18 @@ bool PhysicsServerCommandProcessor::processRequestContactpointInformationCommand pt.m_linkIndexB = m_linkIndexB; for (int j = 0; j < 3; j++) { - pt.m_contactNormalOnBInWS[j] = srcPt.m_normalWorldOnB[j]; - pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnA()[j]; - pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnB()[j]; + if (isSwapped) + { + pt.m_contactNormalOnBInWS[j] = -srcPt.m_normalWorldOnB[j]; + pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnB()[j]; + pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnA()[j]; + } + else + { + pt.m_contactNormalOnBInWS[j] = srcPt.m_normalWorldOnB[j]; + pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnA()[j]; + pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnB()[j]; + } } pt.m_normalForce = srcPt.getAppliedImpulse() / m_deltaTime; pt.m_linearFrictionForce1 = srcPt.m_appliedImpulseLateral1 / m_deltaTime; |