Scarfski/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3.git synced 2026-07-30 01:49:20 +00:00
Code Issues Packages Projects Releases Wiki Activity

PyBullet: fix cases where contact point objects are swapped, reported here

Browse Source 
https://github.com/bulletphysics/bullet3/issues/3023
Thanks to ichumuh for the report and reproduction!
This commit is contained in:
Erwin Coumans
2020-09-01 09:27:05 -07:00
parent 55451b6bfc
commit f9abae073b
1 changed files with 27 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -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;