diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index c698b07a4..9261eb807 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -6172,7 +6172,13 @@ bool PhysicsServerCommandProcessor::processRequestContactpointInformationCommand pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnB()[j]; } pt.m_normalForce = srcPt.getAppliedImpulse() / m_data->m_physicsDeltaTime; - // pt.m_linearFrictionForce = srcPt.m_appliedImpulseLateral1; + pt.m_linearFrictionForce1 = srcPt.m_appliedImpulseLateral1 / m_data->m_physicsDeltaTime; + pt.m_linearFrictionForce2 = srcPt.m_appliedImpulseLateral2 / m_data->m_physicsDeltaTime; + for (int j = 0; j < 3; j++) + { + pt.m_linearFrictionDirection1[j] = srcPt.m_lateralFrictionDir1[j]; + pt.m_linearFrictionDirection2[j] = srcPt.m_lateralFrictionDir2[j]; + } m_data->m_cachedContactPoints.push_back(pt); } } @@ -6397,7 +6403,13 @@ bool PhysicsServerCommandProcessor::processRequestContactpointInformationCommand pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnB()[j]; } pt.m_normalForce = srcPt.getAppliedImpulse() / m_deltaTime; - // pt.m_linearFrictionForce = srcPt.m_appliedImpulseLateral1; + pt.m_linearFrictionForce1 = srcPt.m_appliedImpulseLateral1 / m_deltaTime; + pt.m_linearFrictionForce2 = srcPt.m_appliedImpulseLateral2 / m_deltaTime; + for (int j = 0; j < 3; j++) + { + pt.m_linearFrictionDirection1[j] = srcPt.m_lateralFrictionDir1[j]; + pt.m_linearFrictionDirection2[j] = srcPt.m_lateralFrictionDir2[j]; + } m_cachedContactPoints.push_back(pt); } return 1; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index cde1bd41b..e2c27ede1 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -572,13 +572,10 @@ struct b3ContactPointData double m_normalForce; - //todo: expose the friction forces as well - // double m_linearFrictionDirection0[3]; - // double m_linearFrictionForce0; - // double m_linearFrictionDirection1[3]; - // double m_linearFrictionForce1; - // double m_angularFrictionDirection[3]; - // double m_angularFrictionForce; + double m_linearFrictionForce1; + double m_linearFrictionForce2; + double m_linearFrictionDirection1[3]; + double m_linearFrictionDirection2[3]; }; enum diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 86c870a6a..713d79cdd 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -5754,6 +5754,10 @@ static PyObject* MyConvertContactPoint(struct b3ContactInformation* contactPoint 8 double m_contactDistance;//negative number is penetration, positive is distance. 9 double m_normalForce; + 10 double m_linearFrictionForce1; + 11 double double m_linearFrictionDirection1[3]; + 12 double m_linearFrictionForce2; + 13 double double m_linearFrictionDirection2[3]; */ int i; @@ -5761,7 +5765,7 @@ static PyObject* MyConvertContactPoint(struct b3ContactInformation* contactPoint PyObject* pyResultList = PyTuple_New(contactPointPtr->m_numContactPoints); for (i = 0; i < contactPointPtr->m_numContactPoints; i++) { - PyObject* contactObList = PyTuple_New(10); // see above 10 fields + PyObject* contactObList = PyTuple_New(14); // see above 10 fields PyObject* item; item = PyInt_FromLong(contactPointPtr->m_contactPointData[i].m_contactFlags); @@ -5832,6 +5836,44 @@ static PyObject* MyConvertContactPoint(struct b3ContactInformation* contactPoint contactPointPtr->m_contactPointData[i].m_normalForce); PyTuple_SetItem(contactObList, 9, item); + item = PyFloat_FromDouble( + contactPointPtr->m_contactPointData[i].m_linearFrictionForce1); + PyTuple_SetItem(contactObList, 10, item); + + { + PyObject* posAObj = PyTuple_New(3); + + item = PyFloat_FromDouble( + contactPointPtr->m_contactPointData[i].m_linearFrictionDirection1[0]); + PyTuple_SetItem(posAObj, 0, item); + item = PyFloat_FromDouble( + contactPointPtr->m_contactPointData[i].m_linearFrictionDirection1[1]); + PyTuple_SetItem(posAObj, 1, item); + item = PyFloat_FromDouble( + contactPointPtr->m_contactPointData[i].m_linearFrictionDirection1[2]); + PyTuple_SetItem(posAObj, 2, item); + PyTuple_SetItem(contactObList, 11, posAObj); + } + + item = PyFloat_FromDouble( + contactPointPtr->m_contactPointData[i].m_linearFrictionForce2); + PyTuple_SetItem(contactObList, 12, item); + + { + PyObject* posAObj = PyTuple_New(3); + + item = PyFloat_FromDouble( + contactPointPtr->m_contactPointData[i].m_linearFrictionDirection2[0]); + PyTuple_SetItem(posAObj, 0, item); + item = PyFloat_FromDouble( + contactPointPtr->m_contactPointData[i].m_linearFrictionDirection2[1]); + PyTuple_SetItem(posAObj, 1, item); + item = PyFloat_FromDouble( + contactPointPtr->m_contactPointData[i].m_linearFrictionDirection2[2]); + PyTuple_SetItem(posAObj, 2, item); + PyTuple_SetItem(contactObList, 13, posAObj); + } + PyTuple_SetItem(pyResultList, i, contactObList); } return pyResultList;