From 9ee1c4ec245ff0fc860fc56c78910d4ab0dd0579 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sat, 19 Nov 2016 17:13:56 -0800 Subject: [PATCH] regular OR wireframe rendering, not both add option to perform filtering of 'getClosestPoints' using linkA/linkB. don't use 'realtimesimulation' as default add/remove debug items within same thread pybullet, report contact points and normal as [x,y,z] triplet/vector, not 3 scalars separate 'getClosestPointsAlgorithm': box-box doesn't report closest points with positive distance, and the query shouldn't impact regular 'closesst points' --- .../ExampleBrowser/OpenGLExampleBrowser.cpp | 2 +- examples/SharedMemory/PhysicsClientC_API.cpp | 33 +++ examples/SharedMemory/PhysicsClientC_API.h | 4 + .../PhysicsServerCommandProcessor.cpp | 36 ++- .../SharedMemory/PhysicsServerExample.cpp | 223 ++++++++---------- examples/SharedMemory/SharedMemoryCommands.h | 4 + examples/pybullet/pybullet.c | 112 +++++---- .../BroadphaseCollision/btDispatcher.h | 8 +- .../btCollisionConfiguration.h | 3 + .../btCollisionDispatcher.cpp | 28 ++- .../CollisionDispatch/btCollisionDispatcher.h | 8 +- .../CollisionDispatch/btCollisionWorld.cpp | 4 +- .../btCompoundCollisionAlgorithm.cpp | 39 ++- .../btCompoundCompoundCollisionAlgorithm.cpp | 29 ++- .../btConvexConcaveCollisionAlgorithm.cpp | 15 +- .../btDefaultCollisionConfiguration.cpp | 80 +++++++ .../btDefaultCollisionConfiguration.h | 2 + .../Gimpact/btGImpactCollisionAlgorithm.h | 2 +- .../btSoftBodyConcaveCollisionAlgorithm.cpp | 7 +- 19 files changed, 417 insertions(+), 222 deletions(-) diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index 962f04437..ca86cda97 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -1167,7 +1167,7 @@ void OpenGLExampleBrowser::update(float deltaTime) } BT_PROFILE("Render Scene"); sCurrentDemo->renderScene(); - } + } else { B3_PROFILE("physicsDebugDraw"); glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index d0b0413f1..e61130446 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1504,6 +1504,8 @@ b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClient command->m_requestContactPointArguments.m_startingContactPointIndex = 0; command->m_requestContactPointArguments.m_objectAIndexFilter = -1; command->m_requestContactPointArguments.m_objectBIndexFilter = -1; + command->m_requestContactPointArguments.m_linkIndexAIndexFilter = -2; + command->m_requestContactPointArguments.m_linkIndexBIndexFilter = -2; command->m_updateFlags = 0; return (b3SharedMemoryCommandHandle) command; } @@ -1516,6 +1518,37 @@ void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int body command->m_requestContactPointArguments.m_objectAIndexFilter = bodyUniqueIdA; } +void b3SetContactFilterLinkA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION); + command->m_updateFlags |= CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER; + command->m_requestContactPointArguments.m_linkIndexAIndexFilter= linkIndexA; +} + +void b3SetContactFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION); + command->m_updateFlags |= CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER; + command->m_requestContactPointArguments.m_linkIndexBIndexFilter = linkIndexB; +} + +void b3SetClosestDistanceFilterLinkA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA) +{ + b3SetContactFilterLinkA(commandHandle, linkIndexA); +} + +void b3SetClosestDistanceFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB) +{ + b3SetContactFilterLinkB(commandHandle, linkIndexB); +} + + + + void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 529e50e94..e9df467ec 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -123,12 +123,16 @@ void b3RequestCameraImageSetFOVProjectionMatrix(b3SharedMemoryCommandHandle comm b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClientHandle physClient); void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA); void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB); +void b3SetContactFilterLinkA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA); +void b3SetContactFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB); void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointInfo); ///compute the closest points between two bodies b3SharedMemoryCommandHandle b3InitClosestDistanceQuery(b3PhysicsClientHandle physClient); void b3SetClosestDistanceFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA); +void b3SetClosestDistanceFilterLinkA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA); void b3SetClosestDistanceFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB); +void b3SetClosestDistanceFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB); void b3SetClosestDistanceThreshold(b3SharedMemoryCommandHandle commandHandle, double distance); void b3GetClosestPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointInfo); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index ee7d06624..056784ada 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -542,7 +542,7 @@ struct PhysicsServerCommandProcessorInternalData m_kukaGripperMultiBody(0), m_kukaGripperRevolute1(0), m_kukaGripperRevolute2(0), - m_allowRealTimeSimulation(true), + m_allowRealTimeSimulation(false), m_huskyId(-1), m_KukaId(-1), m_sphereId(-1), @@ -2769,6 +2769,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm int bodyUniqueIdA = clientCmd.m_requestContactPointArguments.m_objectAIndexFilter; int bodyUniqueIdB = clientCmd.m_requestContactPointArguments.m_objectBIndexFilter; + bool hasLinkIndexAFilter = (0!=(clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER)); + bool hasLinkIndexBFilter = (0!=(clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER)); + + int linkIndexA = clientCmd.m_requestContactPointArguments.m_linkIndexAIndexFilter; + int linkIndexB = clientCmd.m_requestContactPointArguments.m_linkIndexBIndexFilter; + btAlignedObjectArray setA; btAlignedObjectArray setB; btAlignedObjectArray setALinkIndex; @@ -2783,15 +2789,21 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { if (bodyA->m_multiBody->getBaseCollider()) { - setA.push_back(bodyA->m_multiBody->getBaseCollider()); - setALinkIndex.push_back(-1); + if (!hasLinkIndexAFilter || (linkIndexA == -1)) + { + setA.push_back(bodyA->m_multiBody->getBaseCollider()); + setALinkIndex.push_back(-1); + } } for (int i = 0; i < bodyA->m_multiBody->getNumLinks(); i++) { if (bodyA->m_multiBody->getLink(i).m_collider) { - setA.push_back(bodyA->m_multiBody->getLink(i).m_collider); - setALinkIndex.push_back(i); + if (!hasLinkIndexAFilter || (linkIndexA == i)) + { + setA.push_back(bodyA->m_multiBody->getLink(i).m_collider); + setALinkIndex.push_back(i); + } } } } @@ -2811,15 +2823,21 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { if (bodyB->m_multiBody->getBaseCollider()) { - setB.push_back(bodyB->m_multiBody->getBaseCollider()); - setBLinkIndex.push_back(-1); + if (!hasLinkIndexBFilter || (linkIndexB == -1)) + { + setB.push_back(bodyB->m_multiBody->getBaseCollider()); + setBLinkIndex.push_back(-1); + } } for (int i = 0; i < bodyB->m_multiBody->getNumLinks(); i++) { if (bodyB->m_multiBody->getLink(i).m_collider) { - setB.push_back(bodyB->m_multiBody->getLink(i).m_collider); - setBLinkIndex.push_back(i); + if (!hasLinkIndexBFilter || (linkIndexB ==i)) + { + setB.push_back(bodyB->m_multiBody->getLink(i).m_collider); + setBLinkIndex.push_back(i); + } } } } diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index c4aa9455a..55cefb05a 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -126,10 +126,6 @@ enum MultiThreadedGUIHelperCommunicationEnums eGUIHelperRemoveAllGraphicsInstances, eGUIHelperCopyCameraImageData, eGUIHelperAutogenerateGraphicsObjects, - eGUIUserDebugAddText, - eGUIUserDebugAddLine, - eGUIUserDebugRemoveItem, - eGUIUserDebugRemoveAllItems, }; #include @@ -701,43 +697,36 @@ public: m_tmpText.m_textColorRGB[2] = textColorRGB[2]; m_cs->lock(); - m_cs->setSharedParam(1, eGUIUserDebugAddText); + m_userDebugText.push_back(m_tmpText); m_cs->unlock(); - while (m_cs->getSharedParam(1) != eGUIHelperIdle) - { - b3Clock::usleep(150); - } return m_userDebugText[m_userDebugText.size()-1].m_itemUniqueId; } btAlignedObjectArray m_userDebugLines; - UserDebugDrawLine m_tmpLine; virtual int addUserDebugLine(const double debugLineFromXYZ[3], const double debugLineToXYZ[3], const double debugLineColorRGB[3], double lineWidth, double lifeTime ) { - m_tmpLine.m_lifeTime = lifeTime; - m_tmpLine.m_lineWidth = lineWidth; - m_tmpLine.m_itemUniqueId = m_uidGenerator++; - m_tmpLine.m_debugLineFromXYZ[0] = debugLineFromXYZ[0]; - m_tmpLine.m_debugLineFromXYZ[1] = debugLineFromXYZ[1]; - m_tmpLine.m_debugLineFromXYZ[2] = debugLineFromXYZ[2]; + UserDebugDrawLine tmpLine; - m_tmpLine.m_debugLineToXYZ[0] = debugLineToXYZ[0]; - m_tmpLine.m_debugLineToXYZ[1] = debugLineToXYZ[1]; - m_tmpLine.m_debugLineToXYZ[2] = debugLineToXYZ[2]; + tmpLine.m_lifeTime = lifeTime; + tmpLine.m_lineWidth = lineWidth; + tmpLine.m_itemUniqueId = m_uidGenerator++; + tmpLine.m_debugLineFromXYZ[0] = debugLineFromXYZ[0]; + tmpLine.m_debugLineFromXYZ[1] = debugLineFromXYZ[1]; + tmpLine.m_debugLineFromXYZ[2] = debugLineFromXYZ[2]; + + tmpLine.m_debugLineToXYZ[0] = debugLineToXYZ[0]; + tmpLine.m_debugLineToXYZ[1] = debugLineToXYZ[1]; + tmpLine.m_debugLineToXYZ[2] = debugLineToXYZ[2]; - m_tmpLine.m_debugLineColorRGB[0] = debugLineColorRGB[0]; - m_tmpLine.m_debugLineColorRGB[1] = debugLineColorRGB[1]; - m_tmpLine.m_debugLineColorRGB[2] = debugLineColorRGB[2]; + tmpLine.m_debugLineColorRGB[0] = debugLineColorRGB[0]; + tmpLine.m_debugLineColorRGB[1] = debugLineColorRGB[1]; + tmpLine.m_debugLineColorRGB[2] = debugLineColorRGB[2]; m_cs->lock(); - m_cs->setSharedParam(1, eGUIUserDebugAddLine); + m_userDebugLines.push_back(tmpLine); m_cs->unlock(); - while (m_cs->getSharedParam(1) != eGUIHelperIdle) - { - b3Clock::usleep(150); - } return m_userDebugLines[m_userDebugLines.size()-1].m_itemUniqueId; } @@ -747,23 +736,38 @@ public: { m_removeDebugItemUid = debugItemUniqueId; m_cs->lock(); - m_cs->setSharedParam(1, eGUIUserDebugRemoveItem); - m_cs->unlock(); - while (m_cs->getSharedParam(1) != eGUIHelperIdle) + + for (int i = 0; iunlock(); + } virtual void removeAllUserDebugItems( ) { m_cs->lock(); - m_cs->setSharedParam(1, eGUIUserDebugRemoveAllItems); + m_userDebugLines.clear(); + m_userDebugText.clear(); + m_uidGenerator = 0; m_cs->unlock(); - while (m_cs->getSharedParam(1) != eGUIHelperIdle) - { - b3Clock::usleep(150); - } } @@ -825,6 +829,7 @@ public: virtual bool wantsTermination(); virtual bool isConnected(); virtual void renderScene(); + void drawUserDebugLines(); virtual void exitPhysics(); virtual void physicsDebugDraw(int debugFlags); @@ -1247,60 +1252,6 @@ void PhysicsServerExample::stepSimulation(float deltaTime) break; } - case eGUIUserDebugAddText: - { - m_multiThreadedHelper->m_userDebugText.push_back(m_multiThreadedHelper->m_tmpText); - m_multiThreadedHelper->getCriticalSection()->lock(); - m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperIdle); - m_multiThreadedHelper->getCriticalSection()->unlock(); - break; - } - case eGUIUserDebugAddLine: - { - m_multiThreadedHelper->m_userDebugLines.push_back(m_multiThreadedHelper->m_tmpLine); - m_multiThreadedHelper->getCriticalSection()->lock(); - m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperIdle); - m_multiThreadedHelper->getCriticalSection()->unlock(); - break; - } - case eGUIUserDebugRemoveItem: - { - for (int i=0;im_userDebugLines.size();i++) - { - if (m_multiThreadedHelper->m_userDebugLines[i].m_itemUniqueId == m_multiThreadedHelper->m_removeDebugItemUid) - { - m_multiThreadedHelper->m_userDebugLines.swap(i,m_multiThreadedHelper->m_userDebugLines.size()-1); - m_multiThreadedHelper->m_userDebugLines.pop_back(); - break; - } - } - - - for (int i=0;im_userDebugText.size();i++) - { - if (m_multiThreadedHelper->m_userDebugText[i].m_itemUniqueId == m_multiThreadedHelper->m_removeDebugItemUid) - { - m_multiThreadedHelper->m_userDebugText.swap(i,m_multiThreadedHelper->m_userDebugText.size()-1); - m_multiThreadedHelper->m_userDebugText.pop_back(); - break; - } - } - - m_multiThreadedHelper->getCriticalSection()->lock(); - m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperIdle); - m_multiThreadedHelper->getCriticalSection()->unlock(); - break; - } - case eGUIUserDebugRemoveAllItems: - { - m_multiThreadedHelper->m_userDebugLines.clear(); - m_multiThreadedHelper->m_userDebugText.clear(); - m_multiThreadedHelper->m_uidGenerator = 0; - m_multiThreadedHelper->getCriticalSection()->lock(); - m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperIdle); - m_multiThreadedHelper->getCriticalSection()->unlock(); - break; - } case eGUIHelperIdle: { break; @@ -1353,7 +1304,53 @@ extern int gHuskyId; extern btTransform huskyTr; +void PhysicsServerExample::drawUserDebugLines() +{ + static char line0[1024]; + static char line1[1024]; + //draw all user-debug-lines + + //add array of lines + + //draw all user- 'text3d' messages + if (m_multiThreadedHelper) + { + m_args[0].m_cs->lock(); + for (int i = 0; im_userDebugLines.size(); i++) + { + btVector3 from; + from.setValue(m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[0], + m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[1], + m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[2]); + btVector3 toX; + toX.setValue(m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[0], + m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[1], + m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[2]); + + btVector3 color; + color.setValue(m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[0], + m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[1], + m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[2]); + + + m_guiHelper->getAppInterface()->m_renderer->drawLine(from, toX, color, m_multiThreadedHelper->m_userDebugLines[i].m_lineWidth); + } + + for (int i = 0; im_userDebugText.size(); i++) + { + m_guiHelper->getAppInterface()->drawText3D(m_multiThreadedHelper->m_userDebugText[i].m_text, + m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[0], + m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[1], + m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[2], + m_multiThreadedHelper->m_userDebugText[i].textSize); + + } + m_args[0].m_cs->unlock(); + + } + +} void PhysicsServerExample::renderScene() { @@ -1369,48 +1366,8 @@ void PhysicsServerExample::renderScene() B3_PROFILE("PhysicsServerExample::RenderScene"); - static char line0[1024]; - static char line1[1024]; - - //draw all user-debug-lines - - //add array of lines - - //draw all user- 'text3d' messages - if (m_multiThreadedHelper) - { - - for (int i=0;im_userDebugLines.size();i++) - { - btVector3 from; - from.setValue( m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[0], - m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[1], - m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[2]); - btVector3 toX; - toX.setValue( m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[0], - m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[1], - m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[2]); - - btVector3 color; - color.setValue( m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[0], - m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[1], - m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[2]); - - - m_guiHelper->getAppInterface()->m_renderer->drawLine(from, toX, color, m_multiThreadedHelper->m_userDebugLines[i].m_lineWidth); - } - - for (int i=0;im_userDebugText.size();i++) - { - m_guiHelper->getAppInterface()->drawText3D(m_multiThreadedHelper->m_userDebugText[i].m_text, - m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[0], - m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[1], - m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[2], - m_multiThreadedHelper->m_userDebugText[i].textSize); - - } - } + drawUserDebugLines(); if (gEnableRealTimeSimVR) { @@ -1424,6 +1381,7 @@ void PhysicsServerExample::renderScene() static int count = 0; count++; +#if 0 if (0 == (count & 1)) { btScalar curTime = m_clock.getTimeSeconds(); @@ -1444,6 +1402,7 @@ void PhysicsServerExample::renderScene() worseFps = 1000000; } } +#endif #ifdef BT_ENABLE_VR if ((gInternalSimFlags&2 ) && m_tinyVrGui==0) @@ -1544,6 +1503,8 @@ void PhysicsServerExample::renderScene() void PhysicsServerExample::physicsDebugDraw(int debugDrawFlags) { + drawUserDebugLines(); + ///debug rendering m_physicsServer.physicsDebugDraw(debugDrawFlags); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 40b62f518..515fa5079 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -154,6 +154,8 @@ enum EnumRequestContactDataUpdateFlags { CMD_REQUEST_CONTACT_POINT_HAS_QUERY_MODE=1, CMD_REQUEST_CONTACT_POINT_HAS_CLOSEST_DISTANCE_THRESHOLD=2, + CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER = 4, + CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER = 8, }; struct RequestContactDataArgs @@ -161,6 +163,8 @@ struct RequestContactDataArgs int m_startingContactPointIndex; int m_objectAIndexFilter; int m_objectBIndexFilter; + int m_linkIndexAIndexFilter; + int m_linkIndexBIndexFilter; double m_closestDistanceThreshold; int m_mode; }; diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 17e782b8b..8163fbd1b 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1501,7 +1501,7 @@ static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObj res = pybullet_internalSetVectord(textPositionObj,posXYZ); if (!res) { - PyErr_SetString(SpamError, "Error converting lineFrom[3]"); + PyErr_SetString(SpamError, "Error converting textPositionObj[3]"); return NULL; } @@ -1510,7 +1510,7 @@ static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObj res = pybullet_internalSetVectord(textColorRGBObj,colorRGB); if (!res) { - PyErr_SetString(SpamError, "Error converting lineTo[3]"); + PyErr_SetString(SpamError, "Error converting textColorRGBObj[3]"); return NULL; } } @@ -1848,23 +1848,22 @@ static PyObject* MyConvertContactPoint( struct b3ContactInformation* contactPoin 2 int m_bodyUniqueIdB; 3 int m_linkIndexA; 4 int m_linkIndexB; - 5-6-7 double m_positionOnAInWS[3];//contact point location on object A, + 5 double m_positionOnAInWS[3];//contact point location on object A, in world space coordinates - 8-9-10 double m_positionOnBInWS[3];//contact point location on object + 6 double m_positionOnBInWS[3];//contact point location on object A, in world space coordinates - 11-12-13 double m_contactNormalOnBInWS[3];//the separating contact + 7 double m_contactNormalOnBInWS[3];//the separating contact normal, pointing from object B towards object A - 14 double m_contactDistance;//negative number is penetration, positive + 8 double m_contactDistance;//negative number is penetration, positive is distance. - - 15 double m_normalForce; + 9 double m_normalForce; */ int i; PyObject* pyResultList = PyTuple_New(contactPointPtr->m_numContactPoints); for (i = 0; i < contactPointPtr->m_numContactPoints; i++) { - PyObject* contactObList = PyTuple_New(16); // see above 16 fields + PyObject* contactObList = PyTuple_New(10); // see above 10 fields PyObject* item; item = PyInt_FromLong(contactPointPtr->m_contactPointData[i].m_contactFlags); @@ -1881,42 +1880,61 @@ static PyObject* MyConvertContactPoint( struct b3ContactInformation* contactPoin item = PyInt_FromLong(contactPointPtr->m_contactPointData[i].m_linkIndexB); PyTuple_SetItem(contactObList, 4, item); - item = PyFloat_FromDouble( - contactPointPtr->m_contactPointData[i].m_positionOnAInWS[0]); - PyTuple_SetItem(contactObList, 5, item); - item = PyFloat_FromDouble( - contactPointPtr->m_contactPointData[i].m_positionOnAInWS[1]); - PyTuple_SetItem(contactObList, 6, item); - item = PyFloat_FromDouble( - contactPointPtr->m_contactPointData[i].m_positionOnAInWS[2]); - PyTuple_SetItem(contactObList, 7, item); - item = PyFloat_FromDouble( - contactPointPtr->m_contactPointData[i].m_positionOnBInWS[0]); - PyTuple_SetItem(contactObList, 8, item); - item = PyFloat_FromDouble( - contactPointPtr->m_contactPointData[i].m_positionOnBInWS[1]); - PyTuple_SetItem(contactObList, 9, item); - item = PyFloat_FromDouble( - contactPointPtr->m_contactPointData[i].m_positionOnBInWS[2]); - PyTuple_SetItem(contactObList, 10, item); + { + PyObject* posAObj = PyTuple_New(3); - item = PyFloat_FromDouble( - contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[0]); - PyTuple_SetItem(contactObList, 11, item); - item = PyFloat_FromDouble( - contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[1]); - PyTuple_SetItem(contactObList, 12, item); - item = PyFloat_FromDouble( - contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[2]); - PyTuple_SetItem(contactObList, 13, item); + item = PyFloat_FromDouble( + contactPointPtr->m_contactPointData[i].m_positionOnAInWS[0]); + PyTuple_SetItem(posAObj, 0, item); + item = PyFloat_FromDouble( + contactPointPtr->m_contactPointData[i].m_positionOnAInWS[1]); + PyTuple_SetItem(posAObj, 1, item); + item = PyFloat_FromDouble( + contactPointPtr->m_contactPointData[i].m_positionOnAInWS[2]); + PyTuple_SetItem(posAObj, 2, item); + + PyTuple_SetItem(contactObList, 5, posAObj); + } + + { + PyObject* posBObj = PyTuple_New(3); + + + item = PyFloat_FromDouble( + contactPointPtr->m_contactPointData[i].m_positionOnBInWS[0]); + PyTuple_SetItem(posBObj, 0, item); + item = PyFloat_FromDouble( + contactPointPtr->m_contactPointData[i].m_positionOnBInWS[1]); + PyTuple_SetItem(posBObj, 1, item); + item = PyFloat_FromDouble( + contactPointPtr->m_contactPointData[i].m_positionOnBInWS[2]); + PyTuple_SetItem(posBObj, 2, item); + + PyTuple_SetItem(contactObList, 6, posBObj); + + } + + { + PyObject* normalOnB = PyTuple_New(3); + item = PyFloat_FromDouble( + contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[0]); + PyTuple_SetItem(normalOnB, 0, item); + item = PyFloat_FromDouble( + contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[1]); + PyTuple_SetItem(normalOnB, 1, item); + item = PyFloat_FromDouble( + contactPointPtr->m_contactPointData[i].m_contactNormalOnBInWS[2]); + PyTuple_SetItem(normalOnB, 2, item); + PyTuple_SetItem(contactObList, 7, normalOnB); + } item = PyFloat_FromDouble( contactPointPtr->m_contactPointData[i].m_contactDistance); - PyTuple_SetItem(contactObList, 14, item); + PyTuple_SetItem(contactObList, 8, item); item = PyFloat_FromDouble( contactPointPtr->m_contactPointData[i].m_normalForce); - PyTuple_SetItem(contactObList, 15, item); + PyTuple_SetItem(contactObList, 9, item); PyTuple_SetItem(pyResultList, i, contactObList); } @@ -1982,6 +2000,9 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py int size = PySequence_Size(args); int bodyUniqueIdA = -1; int bodyUniqueIdB = -1; + int linkIndexA = -2; + int linkIndexB = -2; + double distanceThreshold = 0.f; b3SharedMemoryCommandHandle commandHandle; @@ -1991,15 +2012,15 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py PyObject* pyResultList = 0; - static char *kwlist[] = { "bodyA", "bodyB", "distance", NULL }; + static char *kwlist[] = { "bodyA", "bodyB", "distance", "linkIndexA","linkIndexB",NULL }; if (0 == sm) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - if (!PyArg_ParseTupleAndKeywords(args, keywds, "iid", kwlist, - &bodyUniqueIdA, &bodyUniqueIdB, &distanceThreshold)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iid|ii", kwlist, + &bodyUniqueIdA, &bodyUniqueIdB, &distanceThreshold, &linkIndexA, &linkIndexB)) return NULL; @@ -2007,7 +2028,14 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py b3SetClosestDistanceFilterBodyA(commandHandle, bodyUniqueIdA); b3SetClosestDistanceFilterBodyB(commandHandle, bodyUniqueIdB); b3SetClosestDistanceThreshold(commandHandle, distanceThreshold); - + if (linkIndexA >= -1) + { + b3SetClosestDistanceFilterLinkA(commandHandle, linkIndexA); + } + if (linkIndexB >= -1) + { + b3SetClosestDistanceFilterLinkB(commandHandle, linkIndexB); + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); diff --git a/src/BulletCollision/BroadphaseCollision/btDispatcher.h b/src/BulletCollision/BroadphaseCollision/btDispatcher.h index 89c307d14..7b0f9489a 100644 --- a/src/BulletCollision/BroadphaseCollision/btDispatcher.h +++ b/src/BulletCollision/BroadphaseCollision/btDispatcher.h @@ -64,6 +64,12 @@ struct btDispatcherInfo btScalar m_convexConservativeDistanceThreshold; }; +enum ebtDispatcherQueryType +{ + BT_CONTACT_POINT_ALGORITHMS = 1, + BT_CLOSEST_POINT_ALGORITHMS = 2 +}; + ///The btDispatcher interface class can be used in combination with broadphase to dispatch calculations for overlapping pairs. ///For example for pairwise collision detection, calculating contact points stored in btPersistentManifold or user callbacks (game logic). class btDispatcher @@ -73,7 +79,7 @@ class btDispatcher public: virtual ~btDispatcher() ; - virtual btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold=0) = 0; + virtual btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold, ebtDispatcherQueryType queryType) = 0; virtual btPersistentManifold* getNewManifold(const btCollisionObject* b0,const btCollisionObject* b1)=0; diff --git a/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h b/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h index 669498494..35f77d4e6 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h +++ b/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h @@ -40,6 +40,9 @@ public: virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) =0; + virtual btCollisionAlgorithmCreateFunc* getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1) = 0; + + }; #endif //BT_COLLISION_CONFIGURATION diff --git a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp index cf1adbefa..737067ef9 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp @@ -50,8 +50,10 @@ m_dispatcherFlags(btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESH { for (int j=0;jgetCollisionAlgorithmCreateFunc(i,j); - btAssert(m_doubleDispatch[i][j]); + m_doubleDispatchContactPoints[i][j] = m_collisionConfiguration->getCollisionAlgorithmCreateFunc(i,j); + btAssert(m_doubleDispatchContactPoints[i][j]); + m_doubleDispatchClosestPoints[i][j] = m_collisionConfiguration->getClosestPointsAlgorithmCreateFunc(i, j); + } } @@ -61,7 +63,12 @@ m_dispatcherFlags(btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESH void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc) { - m_doubleDispatch[proxyType0][proxyType1] = createFunc; + m_doubleDispatchContactPoints[proxyType0][proxyType1] = createFunc; +} + +void btCollisionDispatcher::registerClosestPointsCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc) +{ + m_doubleDispatchClosestPoints[proxyType0][proxyType1] = createFunc; } btCollisionDispatcher::~btCollisionDispatcher() @@ -138,14 +145,23 @@ void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold) -btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold) + +btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold, ebtDispatcherQueryType algoType) { btCollisionAlgorithmConstructionInfo ci; ci.m_dispatcher1 = this; ci.m_manifold = sharedManifold; - btCollisionAlgorithm* algo = m_doubleDispatch[body0Wrap->getCollisionShape()->getShapeType()][body1Wrap->getCollisionShape()->getShapeType()]->CreateCollisionAlgorithm(ci,body0Wrap,body1Wrap); + btCollisionAlgorithm* algo = 0; + if (algoType == BT_CONTACT_POINT_ALGORITHMS) + { + algo = m_doubleDispatchContactPoints[body0Wrap->getCollisionShape()->getShapeType()][body1Wrap->getCollisionShape()->getShapeType()]->CreateCollisionAlgorithm(ci, body0Wrap, body1Wrap); + } + else + { + algo = m_doubleDispatchClosestPoints[body0Wrap->getCollisionShape()->getShapeType()][body1Wrap->getCollisionShape()->getShapeType()]->CreateCollisionAlgorithm(ci, body0Wrap, body1Wrap); + } return algo; } @@ -262,7 +278,7 @@ void btCollisionDispatcher::defaultNearCallback(btBroadphasePair& collisionPair, //dispatcher will keep algorithms persistent in the collision pair if (!collisionPair.m_algorithm) { - collisionPair.m_algorithm = dispatcher.findAlgorithm(&obj0Wrap,&obj1Wrap); + collisionPair.m_algorithm = dispatcher.findAlgorithm(&obj0Wrap,&obj1Wrap,0, BT_CONTACT_POINT_ALGORITHMS); } if (collisionPair.m_algorithm) diff --git a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h index 92696ee54..b97ee3c1b 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h +++ b/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h @@ -57,7 +57,9 @@ protected: btPoolAllocator* m_persistentManifoldPoolAllocator; - btCollisionAlgorithmCreateFunc* m_doubleDispatch[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES]; + btCollisionAlgorithmCreateFunc* m_doubleDispatchContactPoints[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES]; + + btCollisionAlgorithmCreateFunc* m_doubleDispatchClosestPoints[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES]; btCollisionConfiguration* m_collisionConfiguration; @@ -84,6 +86,8 @@ public: ///registerCollisionCreateFunc allows registration of custom/alternative collision create functions void registerCollisionCreateFunc(int proxyType0,int proxyType1, btCollisionAlgorithmCreateFunc* createFunc); + void registerClosestPointsCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc *createFunc); + int getNumManifolds() const { return int( m_manifoldsPtr.size()); @@ -115,7 +119,7 @@ public: virtual void clearManifold(btPersistentManifold* manifold); - btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold = 0); + btCollisionAlgorithm* findAlgorithm(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,btPersistentManifold* sharedManifold, ebtDispatcherQueryType queryType); virtual bool needsCollision(const btCollisionObject* body0,const btCollisionObject* body1); diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index 3196369e1..133994112 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -1231,7 +1231,7 @@ struct btSingleContactCallback : public btBroadphaseAabbCallback btCollisionObjectWrapper ob0(0,m_collisionObject->getCollisionShape(),m_collisionObject,m_collisionObject->getWorldTransform(),-1,-1); btCollisionObjectWrapper ob1(0,collisionObject->getCollisionShape(),collisionObject,collisionObject->getWorldTransform(),-1,-1); - btCollisionAlgorithm* algorithm = m_world->getDispatcher()->findAlgorithm(&ob0,&ob1); + btCollisionAlgorithm* algorithm = m_world->getDispatcher()->findAlgorithm(&ob0,&ob1,0, BT_CLOSEST_POINT_ALGORITHMS); if (algorithm) { btBridgedManifoldResult contactPointResult(&ob0,&ob1, m_resultCallback); @@ -1267,7 +1267,7 @@ void btCollisionWorld::contactPairTest(btCollisionObject* colObjA, btCollisionOb btCollisionObjectWrapper obA(0,colObjA->getCollisionShape(),colObjA,colObjA->getWorldTransform(),-1,-1); btCollisionObjectWrapper obB(0,colObjB->getCollisionShape(),colObjB,colObjB->getWorldTransform(),-1,-1); - btCollisionAlgorithm* algorithm = getDispatcher()->findAlgorithm(&obA,&obB); + btCollisionAlgorithm* algorithm = getDispatcher()->findAlgorithm(&obA,&obB, 0, BT_CLOSEST_POINT_ALGORITHMS); if (algorithm) { btBridgedManifoldResult contactPointResult(&obA,&obB, resultCallback); diff --git a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp index 13cddc11a..1fc960df5 100644 --- a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp @@ -65,7 +65,13 @@ void btCompoundCollisionAlgorithm::preallocateChildAlgorithms(const btCollisionO const btCollisionShape* childShape = compoundShape->getChildShape(i); btCollisionObjectWrapper childWrap(colObjWrap,childShape,colObjWrap->getCollisionObject(),colObjWrap->getWorldTransform(),-1,i);//wrong child trans, but unused (hopefully) - m_childCollisionAlgorithms[i] = m_dispatcher->findAlgorithm(&childWrap,otherObjWrap,m_sharedManifold); + m_childCollisionAlgorithms[i] = m_dispatcher->findAlgorithm(&childWrap,otherObjWrap,m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS); + + + btAlignedObjectArray m_childCollisionAlgorithmsContact; + btAlignedObjectArray m_childCollisionAlgorithmsClosestPoints; + + } } } @@ -128,8 +134,14 @@ public: btTransform newChildWorldTrans = orgTrans*childTrans ; //perform an AABB check first - btVector3 aabbMin0,aabbMax0,aabbMin1,aabbMax1; + btVector3 aabbMin0,aabbMax0; childShape->getAabb(newChildWorldTrans,aabbMin0,aabbMax0); + + btVector3 extendAabb(m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold); + aabbMin0 -= extendAabb; + aabbMax0 += extendAabb; + + btVector3 aabbMin1, aabbMax1; m_otherObjWrap->getCollisionShape()->getAabb(m_otherObjWrap->getWorldTransform(),aabbMin1,aabbMax1); if (gCompoundChildShapePairCallback) @@ -142,12 +154,22 @@ public: { btCollisionObjectWrapper compoundWrap(this->m_compoundColObjWrap,childShape,m_compoundColObjWrap->getCollisionObject(),newChildWorldTrans,-1,index); + + btCollisionAlgorithm* algo = 0; - - //the contactpoint is still projected back using the original inverted worldtrans - if (!m_childCollisionAlgorithms[index]) - m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(&compoundWrap,m_otherObjWrap,m_sharedManifold); - + if (m_resultOut->m_closestPointDistanceThreshold > 0) + { + algo = m_dispatcher->findAlgorithm(&compoundWrap, m_otherObjWrap, 0, BT_CLOSEST_POINT_ALGORITHMS); + } + else + { + //the contactpoint is still projected back using the original inverted worldtrans + if (!m_childCollisionAlgorithms[index]) + { + m_childCollisionAlgorithms[index] = m_dispatcher->findAlgorithm(&compoundWrap, m_otherObjWrap, m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS); + } + algo = m_childCollisionAlgorithms[index]; + } const btCollisionObjectWrapper* tmpWrap = 0; @@ -164,8 +186,7 @@ public: m_resultOut->setShapeIdentifiersB(-1,index); } - - m_childCollisionAlgorithms[index]->processCollision(&compoundWrap,m_otherObjWrap,m_dispatchInfo,m_resultOut); + algo->processCollision(&compoundWrap,m_otherObjWrap,m_dispatchInfo,m_resultOut); #if 0 if (m_dispatchInfo.m_debugDraw && (m_dispatchInfo.m_debugDraw->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) diff --git a/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp index fcaa9f851..8dd7e4403 100644 --- a/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp @@ -164,9 +164,7 @@ struct btCompoundCompoundLeafCallback : btDbvt::ICollide btVector3 thresholdVec(m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold, m_resultOut->m_closestPointDistanceThreshold); aabbMin0 -= thresholdVec; - aabbMin1 -= thresholdVec; aabbMax0 += thresholdVec; - aabbMax1 += thresholdVec; if (gCompoundCompoundChildShapePairCallback) { @@ -183,17 +181,24 @@ struct btCompoundCompoundLeafCallback : btDbvt::ICollide btSimplePair* pair = m_childCollisionAlgorithmCache->findPair(childIndex0,childIndex1); btCollisionAlgorithm* colAlgo = 0; + if (m_resultOut->m_closestPointDistanceThreshold > 0) + { + colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0, &compoundWrap1, 0, BT_CLOSEST_POINT_ALGORITHMS); + } + else + { + if (pair) + { + colAlgo = (btCollisionAlgorithm*)pair->m_userPointer; - if (pair) - { - colAlgo = (btCollisionAlgorithm*)pair->m_userPointer; - - } else - { - colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0,&compoundWrap1,m_sharedManifold); - pair = m_childCollisionAlgorithmCache->addOverlappingPair(childIndex0,childIndex1); - btAssert(pair); - pair->m_userPointer = colAlgo; + } + else + { + colAlgo = m_dispatcher->findAlgorithm(&compoundWrap0, &compoundWrap1, m_sharedManifold, BT_CONTACT_POINT_ALGORITHMS); + pair = m_childCollisionAlgorithmCache->addOverlappingPair(childIndex0, childIndex1); + btAssert(pair); + pair->m_userPointer = colAlgo; + } } btAssert(colAlgo); diff --git a/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp index 322b1288d..c774383dc 100644 --- a/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp @@ -118,8 +118,16 @@ partId, int triangleIndex) btCollisionObjectWrapper triObWrap(m_triBodyWrap,&tm,m_triBodyWrap->getCollisionObject(),m_triBodyWrap->getWorldTransform(),partId,triangleIndex);//correct transform? - btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap,&triObWrap,m_manifoldPtr); - + btCollisionAlgorithm* colAlgo = 0; + + if (m_resultOut->m_closestPointDistanceThreshold > 0) + { + colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap, &triObWrap, 0, BT_CLOSEST_POINT_ALGORITHMS); + } + else + { + colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBodyWrap, &triObWrap, m_manifoldPtr, BT_CONTACT_POINT_ALGORITHMS); + } const btCollisionObjectWrapper* tmpWrap = 0; if (m_resultOut->getBody0Internal() == m_triBodyWrap->getCollisionObject()) @@ -170,7 +178,8 @@ void btConvexTriangleCallback::setTimeStepAndCounters(btScalar collisionMarginTr const btCollisionShape* convexShape = static_cast(m_convexBodyWrap->getCollisionShape()); //CollisionShape* triangleShape = static_cast(triBody->m_collisionShape); convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax); - btScalar extraMargin = collisionMarginTriangle; + btScalar extraMargin = collisionMarginTriangle+ resultOut->m_closestPointDistanceThreshold; + btVector3 extra(extraMargin,extraMargin,extraMargin); m_aabbMax += extra; diff --git a/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp b/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp index 9e04d2aae..f6e4e57b0 100644 --- a/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp +++ b/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp @@ -198,6 +198,86 @@ btDefaultCollisionConfiguration::~btDefaultCollisionConfiguration() } +btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1) +{ + + + if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE)) + { + return m_sphereSphereCF; + } +#ifdef USE_BUGGY_SPHERE_BOX_ALGORITHM + if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == BOX_SHAPE_PROXYTYPE)) + { + return m_sphereBoxCF; + } + + if ((proxyType0 == BOX_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE)) + { + return m_boxSphereCF; + } +#endif //USE_BUGGY_SPHERE_BOX_ALGORITHM + + + if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == TRIANGLE_SHAPE_PROXYTYPE)) + { + return m_sphereTriangleCF; + } + + if ((proxyType0 == TRIANGLE_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE)) + { + return m_triangleSphereCF; + } + + if (btBroadphaseProxy::isConvex(proxyType0) && (proxyType1 == STATIC_PLANE_PROXYTYPE)) + { + return m_convexPlaneCF; + } + + if (btBroadphaseProxy::isConvex(proxyType1) && (proxyType0 == STATIC_PLANE_PROXYTYPE)) + { + return m_planeConvexCF; + } + + + + if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConvex(proxyType1)) + { + return m_convexConvexCreateFunc; + } + + if (btBroadphaseProxy::isConvex(proxyType0) && btBroadphaseProxy::isConcave(proxyType1)) + { + return m_convexConcaveCreateFunc; + } + + if (btBroadphaseProxy::isConvex(proxyType1) && btBroadphaseProxy::isConcave(proxyType0)) + { + return m_swappedConvexConcaveCreateFunc; + } + + + if (btBroadphaseProxy::isCompound(proxyType0) && btBroadphaseProxy::isCompound(proxyType1)) + { + return m_compoundCompoundCreateFunc; + } + + if (btBroadphaseProxy::isCompound(proxyType0)) + { + return m_compoundCreateFunc; + } + else + { + if (btBroadphaseProxy::isCompound(proxyType1)) + { + return m_swappedCompoundCreateFunc; + } + } + + //failed to find an algorithm + return m_emptyCreateFunc; + +} btCollisionAlgorithmCreateFunc* btDefaultCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) { diff --git a/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h b/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h index 93a45126b..17c7596cf 100644 --- a/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h +++ b/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h @@ -103,6 +103,8 @@ public: virtual btCollisionAlgorithmCreateFunc* getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1); + virtual btCollisionAlgorithmCreateFunc* getClosestPointsAlgorithmCreateFunc(int proxyType0, int proxyType1); + ///Use this method to allow to generate multiple contact points between at once, between two objects using the generic convex-convex algorithm. ///By default, this feature is disabled for best performance. ///@param numPerturbationIterations controls the number of collision queries. Set it to zero to disable the feature. diff --git a/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h b/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h index f85a94cb4..3e5675f72 100644 --- a/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h +++ b/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h @@ -122,7 +122,7 @@ protected: checkManifold(body0Wrap,body1Wrap); btCollisionAlgorithm * convex_algorithm = m_dispatcher->findAlgorithm( - body0Wrap,body1Wrap,getLastManifold()); + body0Wrap,body1Wrap,getLastManifold(), BT_CONTACT_POINT_ALGORITHMS); return convex_algorithm ; } diff --git a/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp b/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp index 9f0d44526..ab84bddf2 100644 --- a/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp +++ b/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp @@ -120,8 +120,8 @@ void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, btCollisionObjectWrapper softBody(0,m_softBody->getCollisionShape(),m_softBody,m_softBody->getWorldTransform(),-1,-1); //btCollisionObjectWrapper triBody(0,tm, ob, btTransform::getIdentity());//ob->getWorldTransform());//?? btCollisionObjectWrapper triBody(0,tm, m_triBody, m_triBody->getWorldTransform(),partId, triangleIndex); - - btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody,&triBody,0);//m_manifoldPtr); + ebtDispatcherQueryType algoType = m_resultOut->m_closestPointDistanceThreshold > 0 ? BT_CLOSEST_POINT_ALGORITHMS : BT_CONTACT_POINT_ALGORITHMS; + btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody,&triBody,0, algoType);//m_manifoldPtr); colAlgo->processCollision(&softBody,&triBody,*m_dispatchInfoPtr,m_resultOut); colAlgo->~btCollisionAlgorithm(); @@ -164,7 +164,8 @@ void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, btCollisionObjectWrapper softBody(0,m_softBody->getCollisionShape(),m_softBody,m_softBody->getWorldTransform(),-1,-1); btCollisionObjectWrapper triBody(0,tm, m_triBody, m_triBody->getWorldTransform(),partId, triangleIndex);//btTransform::getIdentity());//?? - btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody,&triBody,0);//m_manifoldPtr); + ebtDispatcherQueryType algoType = m_resultOut->m_closestPointDistanceThreshold > 0 ? BT_CLOSEST_POINT_ALGORITHMS : BT_CONTACT_POINT_ALGORITHMS; + btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(&softBody,&triBody,0, algoType);//m_manifoldPtr); colAlgo->processCollision(&softBody,&triBody,*m_dispatchInfoPtr,m_resultOut); colAlgo->~btCollisionAlgorithm();