From 9ee1c4ec245ff0fc860fc56c78910d4ab0dd0579 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sat, 19 Nov 2016 17:13:56 -0800 Subject: [PATCH 1/9] 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(); From f9c1e19587d5d0d0a87a70a1b8c1b85bc4dbafc4 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sun, 20 Nov 2016 15:38:42 -0800 Subject: [PATCH 2/9] revert 'addUserDebugLine/Text' to lockless rendering on main thread, at the cost of slower add/remove for now. --- .../SharedMemory/PhysicsServerExample.cpp | 136 ++++++++++++------ 1 file changed, 92 insertions(+), 44 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 55cefb05a..bf3bf20b5 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -126,6 +126,10 @@ enum MultiThreadedGUIHelperCommunicationEnums eGUIHelperRemoveAllGraphicsInstances, eGUIHelperCopyCameraImageData, eGUIHelperAutogenerateGraphicsObjects, + eGUIUserDebugAddText, + eGUIUserDebugAddLine, + eGUIUserDebugRemoveItem, + eGUIUserDebugRemoveAllItems, }; #include @@ -697,36 +701,43 @@ public: m_tmpText.m_textColorRGB[2] = textColorRGB[2]; m_cs->lock(); - m_userDebugText.push_back(m_tmpText); + m_cs->setSharedParam(1, eGUIUserDebugAddText); 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 ) { - UserDebugDrawLine tmpLine; + 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]; - 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_debugLineToXYZ[0] = debugLineToXYZ[0]; + m_tmpLine.m_debugLineToXYZ[1] = debugLineToXYZ[1]; + m_tmpLine.m_debugLineToXYZ[2] = debugLineToXYZ[2]; - tmpLine.m_debugLineColorRGB[0] = debugLineColorRGB[0]; - tmpLine.m_debugLineColorRGB[1] = debugLineColorRGB[1]; - tmpLine.m_debugLineColorRGB[2] = debugLineColorRGB[2]; + m_tmpLine.m_debugLineColorRGB[0] = debugLineColorRGB[0]; + m_tmpLine.m_debugLineColorRGB[1] = debugLineColorRGB[1]; + m_tmpLine.m_debugLineColorRGB[2] = debugLineColorRGB[2]; m_cs->lock(); - m_userDebugLines.push_back(tmpLine); + m_cs->setSharedParam(1, eGUIUserDebugAddLine); m_cs->unlock(); + while (m_cs->getSharedParam(1) != eGUIHelperIdle) + { + b3Clock::usleep(150); + } return m_userDebugLines[m_userDebugLines.size()-1].m_itemUniqueId; } @@ -736,38 +747,23 @@ public: { m_removeDebugItemUid = debugItemUniqueId; m_cs->lock(); - - for (int i = 0; isetSharedParam(1, eGUIUserDebugRemoveItem); m_cs->unlock(); + while (m_cs->getSharedParam(1) != eGUIHelperIdle) + { + b3Clock::usleep(150); + } } virtual void removeAllUserDebugItems( ) { m_cs->lock(); - m_userDebugLines.clear(); - m_userDebugText.clear(); - m_uidGenerator = 0; + m_cs->setSharedParam(1, eGUIUserDebugRemoveAllItems); m_cs->unlock(); + while (m_cs->getSharedParam(1) != eGUIHelperIdle) + { + b3Clock::usleep(150); + } } @@ -1252,6 +1248,60 @@ 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; @@ -1316,7 +1366,7 @@ void PhysicsServerExample::drawUserDebugLines() //draw all user- 'text3d' messages if (m_multiThreadedHelper) { - m_args[0].m_cs->lock(); + for (int i = 0; im_userDebugLines.size(); i++) { btVector3 from; @@ -1346,8 +1396,6 @@ void PhysicsServerExample::drawUserDebugLines() m_multiThreadedHelper->m_userDebugText[i].textSize); } - m_args[0].m_cs->unlock(); - } } From 1bc427df6b0405909d53fb58bb1f3ea631eb3860 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sun, 20 Nov 2016 16:22:20 -0800 Subject: [PATCH 3/9] fix compile issue when using VR --- examples/SharedMemory/PhysicsServerExample.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index bf3bf20b5..aff18b0f8 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -1475,7 +1475,9 @@ void PhysicsServerExample::renderScene() tr = tr*b3Transform(b3Quaternion(0,0,-SIMD_HALF_PI),b3MakeVector3(0,0,0)); b3Scalar dt = 0.01; m_tinyVrGui->clearTextArea(); - + static char line0[1024]; + static char line1[1024]; + m_tinyVrGui->grapicalPrintf(line0,0,0,0,0,0,255); m_tinyVrGui->grapicalPrintf(line1,0,16,255,255,255,255); From 0d5dcb3cc55ecc08c190bd6b21f8b8d1b54ba44a Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Mon, 21 Nov 2016 07:42:11 -0800 Subject: [PATCH 4/9] setDebugObjectColor --- examples/SharedMemory/PhysicsClientC_API.cpp | 51 ++++++++++++++++ examples/SharedMemory/PhysicsClientC_API.h | 7 +++ .../PhysicsServerCommandProcessor.cpp | 50 +++++++++++++++- examples/SharedMemory/SharedMemoryCommands.h | 15 ++++- .../TinyRendererVisualShapeConverter.cpp | 19 +++++- .../TinyRendererVisualShapeConverter.h | 1 + examples/TinyRenderer/TinyRenderer.cpp | 11 +++- examples/TinyRenderer/TinyRenderer.h | 1 + examples/pybullet/pybullet.c | 59 +++++++++++++++++-- .../CollisionDispatch/btCollisionObject.h | 24 +++++++- .../CollisionDispatch/btCollisionWorld.cpp | 2 + 11 files changed, 224 insertions(+), 16 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index e61130446..e60953750 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1195,6 +1195,46 @@ int b3GetDebugItemUniqueId(b3SharedMemoryStatusHandle statusHandle) return status->m_userDebugDrawArgs.m_debugItemUniqueId; } +b3SharedMemoryCommandHandle b3InitDebugDrawingCommand(b3PhysicsClientHandle physClient) +{ + PhysicsClient* cl = (PhysicsClient*)physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type = CMD_USER_DEBUG_DRAW; + command->m_updateFlags = 0; + return (b3SharedMemoryCommandHandle)command; +} + + + +void b3SetDebugObjectColor(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex, double objectColorRGB[3]) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_USER_DEBUG_DRAW); + command->m_updateFlags |= USER_DEBUG_SET_CUSTOM_OBJECT_COLOR; + command->m_userDebugDrawArgs.m_objectUniqueId = objectUniqueId; + command->m_userDebugDrawArgs.m_linkIndex = linkIndex; + command->m_userDebugDrawArgs.m_objectDebugColorRGB[0] = objectColorRGB[0]; + command->m_userDebugDrawArgs.m_objectDebugColorRGB[1] = objectColorRGB[1]; + command->m_userDebugDrawArgs.m_objectDebugColorRGB[2] = objectColorRGB[2]; +} + +void b3RemoveDebugObjectColor(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_USER_DEBUG_DRAW); + command->m_updateFlags |= USER_DEBUG_REMOVE_CUSTOM_OBJECT_COLOR; + command->m_userDebugDrawArgs.m_objectUniqueId = objectUniqueId; + command->m_userDebugDrawArgs.m_linkIndex = linkIndex; + +} + + + ///request an image from a simulated camera, using a software renderer. b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient) @@ -1244,6 +1284,17 @@ void b3RequestCameraImageSetLightDirection(b3SharedMemoryCommandHandle commandHa command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_LIGHT_DIRECTION; } +void b3RequestCameraImageSetLightColor(b3SharedMemoryCommandHandle commandHandle, const float lightColor[3]) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA); + for (int i = 0; i<3; i++) + { + command->m_requestPixelDataArguments.m_lightColor[i] = lightColor[i]; + } + command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_LIGHT_COLOR; +} void b3ComputeViewMatrixFromPositions(const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3], float viewMatrix[16]) { diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index e9df467ec..b9da28cd5 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -89,14 +89,21 @@ b3SharedMemoryCommandHandle b3InitUserDebugDrawAddLine3D(b3PhysicsClientHandle p b3SharedMemoryCommandHandle b3InitUserDebugDrawAddText3D(b3PhysicsClientHandle physClient, const char* txt, double positionXYZ[3], double colorRGB[3], double textSize, double lifeTime); b3SharedMemoryCommandHandle b3InitUserDebugDrawRemove(b3PhysicsClientHandle physClient, int debugItemUniqueId); b3SharedMemoryCommandHandle b3InitUserDebugDrawRemoveAll(b3PhysicsClientHandle physClient); + +b3SharedMemoryCommandHandle b3InitDebugDrawingCommand(b3PhysicsClientHandle physClient); +void b3SetDebugObjectColor(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex, double objectColorRGB[3]); +void b3RemoveDebugObjectColor(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex); + ///All debug items unique Ids are positive: a negative unique Id means failure. int b3GetDebugItemUniqueId(b3SharedMemoryStatusHandle statusHandle); + ///request an image from a simulated camera, using a software renderer. b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient); void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle command, float viewMatrix[16], float projectionMatrix[16]); void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle command, int width, int height ); void b3RequestCameraImageSetLightDirection(b3SharedMemoryCommandHandle commandHandle, const float lightDirection[3]); +void b3RequestCameraImageSetLightColor(b3SharedMemoryCommandHandle commandHandle, const float lightColor[3]); void b3RequestCameraImageSelectRenderer(b3SharedMemoryCommandHandle commandHandle, int renderer); void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 056784ada..0df90f614 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -3531,7 +3531,6 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm for( int i=0;igetRigidBodyByIndex(i); - if (colObj) { btRigidBody* rb = btRigidBody::upcast(colObj); @@ -3594,7 +3593,54 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_USER_DEBUG_DRAW_FAILED; hasStatus = true; - + + if ((clientCmd.m_updateFlags & USER_DEBUG_SET_CUSTOM_OBJECT_COLOR) || (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_CUSTOM_OBJECT_COLOR)) + { + int bodyUniqueId = clientCmd.m_userDebugDrawArgs.m_objectUniqueId; + InteralBodyData* body = m_data->getHandle(bodyUniqueId); + if (body) + { + btCollisionObject* destColObj = 0; + + if (body->m_multiBody) + { + if (clientCmd.m_userDebugDrawArgs.m_linkIndex == -1) + { + destColObj = body->m_multiBody->getBaseCollider(); + } + else + { + if (clientCmd.m_userDebugDrawArgs.m_linkIndex >= 0 && clientCmd.m_userDebugDrawArgs.m_linkIndex < body->m_multiBody->getNumLinks()) + { + destColObj = body->m_multiBody->getLink(clientCmd.m_userDebugDrawArgs.m_linkIndex).m_collider; + } + } + + } + if (body->m_rigidBody) + { + destColObj = body->m_rigidBody; + } + + if (destColObj) + { + if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_CUSTOM_OBJECT_COLOR) + { + destColObj->removeCustomDebugColor(); + serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; + } + if (clientCmd.m_updateFlags & USER_DEBUG_SET_CUSTOM_OBJECT_COLOR) + { + btVector3 objectColorRGB; + objectColorRGB.setValue(clientCmd.m_userDebugDrawArgs.m_objectDebugColorRGB[0], + clientCmd.m_userDebugDrawArgs.m_objectDebugColorRGB[1], + clientCmd.m_userDebugDrawArgs.m_objectDebugColorRGB[2]); + destColObj->setCustomDebugColor(objectColorRGB); + serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; + } + } + } + } if (clientCmd.m_updateFlags & USER_DEBUG_HAS_TEXT) { diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 515fa5079..5b3346433 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -139,13 +139,15 @@ struct RequestPixelDataArgs int m_pixelWidth; int m_pixelHeight; float m_lightDirection[3]; + float m_lightColor[3]; }; enum EnumRequestPixelDataUpdateFlags { REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES=1, - REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT=4, - REQUEST_PIXEL_ARGS_SET_LIGHT_DIRECTION=8, + REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT=2, + REQUEST_PIXEL_ARGS_SET_LIGHT_DIRECTION=4, + REQUEST_PIXEL_ARGS_SET_LIGHT_COLOR=8, //don't exceed (1<<15), because this enum is shared with EnumRenderer in SharedMemoryPublic.h }; @@ -524,7 +526,10 @@ enum EnumUserDebugDrawFlags USER_DEBUG_HAS_LINE=1, USER_DEBUG_HAS_TEXT=2, USER_DEBUG_REMOVE_ONE_ITEM=4, - USER_DEBUG_REMOVE_ALL=8 + USER_DEBUG_REMOVE_ALL=8, + USER_DEBUG_SET_CUSTOM_OBJECT_COLOR = 16, + USER_DEBUG_REMOVE_CUSTOM_OBJECT_COLOR = 32, + }; struct UserDebugDrawArgs @@ -541,6 +546,10 @@ struct UserDebugDrawArgs double m_textPositionXYZ[3]; double m_textColorRGB[3]; double m_textSize; + + double m_objectDebugColorRGB[3]; + int m_objectUniqueId; + int m_linkIndex; }; diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index 676ca65d3..34478bee7 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -75,6 +75,8 @@ struct TinyRendererVisualShapeConverterInternalData b3AlignedObjectArray m_segmentationMaskBuffer; btVector3 m_lightDirection; bool m_hasLightDirection; + btVector3 m_lightColor; + bool m_hasLightColor; SimpleCamera m_camera; @@ -83,7 +85,8 @@ struct TinyRendererVisualShapeConverterInternalData m_swWidth(START_WIDTH), m_swHeight(START_HEIGHT), m_rgbColorBuffer(START_WIDTH,START_HEIGHT,TGAImage::RGB), - m_hasLightDirection(false) + m_hasLightDirection(false), + m_hasLightColor(false) { m_depthBuffer.resize(m_swWidth*m_swHeight); m_segmentationMaskBuffer.resize(m_swWidth*m_swHeight,-1); @@ -117,6 +120,11 @@ void TinyRendererVisualShapeConverter::setLightDirection(float x, float y, float m_data->m_hasLightDirection = true; } +void TinyRendererVisualShapeConverter::setLightColor(float x, float y, float z) +{ + m_data->m_lightColor.setValue(x, y, z); + m_data->m_hasLightColor = true; +} void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut, btAlignedObjectArray& texturesOut, b3VisualShapeData& visualShapeOut) { @@ -704,6 +712,12 @@ void TinyRendererVisualShapeConverter::render(const float viewMat[16], const flo lightDirWorld.normalize(); + btVector3 lightColor(1.0,1.0,1.0); + if (m_data->m_hasLightColor) + { + lightColor = m_data->m_lightColor; + } + // printf("num m_swRenderInstances = %d\n", m_data->m_swRenderInstances.size()); for (int i=0;im_swRenderInstances.size();i++) { @@ -737,6 +751,7 @@ void TinyRendererVisualShapeConverter::render(const float viewMat[16], const flo renderObj->m_viewMatrix[i][j] = viewMat[i+4*j]; renderObj->m_localScaling = colObj->getCollisionShape()->getLocalScaling(); renderObj->m_lightDirWorld = lightDirWorld; + renderObj->m_lightColor = lightColor; } } TinyRenderer::renderObject(*renderObj); @@ -900,4 +915,4 @@ int TinyRendererVisualShapeConverter::loadTextureFile(const char* filename) return registerTexture(image, width, height); } return -1; -} \ No newline at end of file +} diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.h b/examples/SharedMemory/TinyRendererVisualShapeConverter.h index 1a4a02f76..b2ed166e6 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.h +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.h @@ -33,6 +33,7 @@ struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter void getWidthAndHeight(int& width, int& height); void setWidthAndHeight(int width, int height); void setLightDirection(float x, float y, float z); + void setLightColor(float x, float y, float z); void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels,int* segmentationMaskBuffer, int segmentationMaskSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied); diff --git a/examples/TinyRenderer/TinyRenderer.cpp b/examples/TinyRenderer/TinyRenderer.cpp index 0a5b7c861..76b9e4053 100644 --- a/examples/TinyRenderer/TinyRenderer.cpp +++ b/examples/TinyRenderer/TinyRenderer.cpp @@ -19,6 +19,7 @@ struct Shader : public IShader { Model* m_model; Vec3f m_light_dir_local; + Vec3f m_light_color; Matrix& m_modelMat; Matrix m_invModelMat; @@ -32,9 +33,10 @@ struct Shader : public IShader { mat<3,3,float> varying_nrm; // normal per vertex to be interpolated by FS //mat<3,3,float> ndc_tri; // triangle in normalized device coordinates - Shader(Model* model, Vec3f light_dir_local, Matrix& modelView, Matrix& projectionMatrix, Matrix& modelMat, Vec3f localScaling, const Vec4f& colorRGBA) + Shader(Model* model, Vec3f light_dir_local, Vec3f light_color, Matrix& modelView, Matrix& projectionMatrix, Matrix& modelMat, Vec3f localScaling, const Vec4f& colorRGBA) :m_model(model), m_light_dir_local(light_dir_local), + m_light_color(light_color), m_modelView1(modelView), m_projectionMatrix(projectionMatrix), m_modelMat(modelMat), @@ -83,6 +85,10 @@ struct Shader : public IShader { color.bgra[1] *= m_colorRGBA[1]; color.bgra[2] *= m_colorRGBA[2]; color.bgra[3] *= m_colorRGBA[3]; + + color.bgra[0] *= m_light_color[0]; + color.bgra[1] *= m_light_color[1]; + color.bgra[2] *= m_light_color[2]; return false; } @@ -260,6 +266,7 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData) int height = renderData.m_rgbColorBuffer.get_height(); Vec3f light_dir_local = Vec3f(renderData.m_lightDirWorld[0],renderData.m_lightDirWorld[1],renderData.m_lightDirWorld[2]); + Vec3f light_color = Vec3f(renderData.m_lightColor[0],renderData.m_lightColor[1],renderData.m_lightColor[2]); Model* model = renderData.m_model; if (0==model) return; @@ -278,7 +285,7 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData) { Matrix modelViewMatrix = renderData.m_viewMatrix*renderData.m_modelMatrix; Vec3f localScaling(renderData.m_localScaling[0],renderData.m_localScaling[1],renderData.m_localScaling[2]); - Shader shader(model, light_dir_local, modelViewMatrix, renderData.m_projectionMatrix,renderData.m_modelMatrix, localScaling, model->getColorRGBA()); + Shader shader(model, light_dir_local, light_color, modelViewMatrix, renderData.m_projectionMatrix,renderData.m_modelMatrix, localScaling, model->getColorRGBA()); //printf("Render %d triangles.\n",model->nfaces()); for (int i=0; infaces(); i++) diff --git a/examples/TinyRenderer/TinyRenderer.h b/examples/TinyRenderer/TinyRenderer.h index 1819790a6..fcb8298bd 100644 --- a/examples/TinyRenderer/TinyRenderer.h +++ b/examples/TinyRenderer/TinyRenderer.h @@ -18,6 +18,7 @@ struct TinyRenderObjectData Matrix m_viewportMatrix; btVector3 m_localScaling; btVector3 m_lightDirWorld; + btVector3 m_lightColor; //Model (vertices, indices, textures, shader) Matrix m_modelMatrix; diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 8163fbd1b..1947ec103 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1647,8 +1647,45 @@ static PyObject* pybullet_removeAllUserDebugItems(PyObject* self, PyObject* args Py_INCREF(Py_None); return Py_None; } - +static PyObject* pybullet_setDebugObjectColor(PyObject* self, PyObject* args, PyObject *keywds) +{ + PyObject* objectColorRGBObj = 0; + double objectColorRGB[3]; + + int objectUniqueId = -1; + int linkIndex = -2; + + static char *kwlist[] = { "objectUniqueId", "linkIndex","objectDebugColorRGB", NULL }; + + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|O", kwlist, + &objectUniqueId, &linkIndex, &objectColorRGBObj)) + return NULL; + + if (objectColorRGBObj) + { + if (pybullet_internalSetVectord(objectColorRGBObj, objectColorRGB)) + { + b3SharedMemoryCommandHandle commandHandle = b3InitDebugDrawingCommand(sm); + b3SetDebugObjectColor(commandHandle, objectUniqueId, linkIndex, objectColorRGB); + b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + } + } + else + { + b3SharedMemoryCommandHandle commandHandle = b3InitDebugDrawingCommand(sm); + b3RemoveDebugObjectColor(commandHandle, objectUniqueId, linkIndex); + b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + } + Py_INCREF(Py_None); + return Py_None; +} + static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args) { @@ -2237,17 +2274,18 @@ static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args, Py /// Render an image from the current timestep of the simulation, width, height are required, other args are optional -// getCameraImage(w, h, view[16], projection[16], lightpos[3]) +// getCameraImage(w, h, view[16], projection[16], lightDir[3], lightColor[3]) static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObject *keywds) { /// request an image from a simulated camera, using a software renderer. struct b3CameraImageData imageData; - PyObject* objViewMat = 0, *objProjMat = 0, *lightDirObj = 0; + PyObject* objViewMat = 0, *objProjMat = 0, *lightDirObj = 0, *lightColorObj = 0; int width, height; int size = PySequence_Size(args); float viewMatrix[16]; float projectionMatrix[16]; float lightDir[3]; + float lightColor[3]; // inialize cmd b3SharedMemoryCommandHandle command; @@ -2260,9 +2298,9 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec command = b3InitRequestCameraImage(sm); // set camera resolution, optionally view, projection matrix, light direction - static char *kwlist[] = { "width", "height", "viewMatrix", "projectionMatrix", "lightDirection",NULL }; + static char *kwlist[] = { "width", "height", "viewMatrix", "projectionMatrix", "lightDirection", "lightColor", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|OOO", kwlist, &width, &height, &objViewMat, &objProjMat, &lightDirObj)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|OOOO", kwlist, &width, &height, &objViewMat, &objProjMat, &lightDirObj, &lightColorObj)) { return NULL; } @@ -2273,11 +2311,16 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec { b3RequestCameraImageSetCameraMatrices(command, viewMatrix, projectionMatrix); } - //set light pos only if function succeeds + //set light direction only if function succeeds if (pybullet_internalSetVector(lightDirObj, lightDir)) { b3RequestCameraImageSetLightDirection(command, lightDir); } + //set light color only if function succeeds + if (pybullet_internalSetVector(lightColorObj, lightColor)) + { + b3RequestCameraImageSetLightColor(command, lightColor); + } if (b3CanSubmitCommand(sm)) @@ -3421,6 +3464,10 @@ static PyMethodDef SpamMethods[] = { "remove all user debug draw items" }, + { "setDebugObjectColor", (PyCFunction)pybullet_setDebugObjectColor, METH_VARARGS | METH_KEYWORDS, + "Override the wireframe debug drawing color for a particular object unique id / link index." + "If you ommit the color, the custom color will be removed." + }, {"getVisualShapeData", pybullet_getVisualShapeData, METH_VARARGS, diff --git a/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/src/BulletCollision/CollisionDispatch/btCollisionObject.h index c285a83fb..0cae21000 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/src/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -122,6 +122,7 @@ protected: ///internal update revision number. It will be increased when the object changes. This allows some subsystems to perform lazy evaluation. int m_updateRevision; + btVector3 m_customDebugColorRGB; public: @@ -136,7 +137,8 @@ public: CF_CHARACTER_OBJECT = 16, CF_DISABLE_VISUALIZE_OBJECT = 32, //disable debug drawing CF_DISABLE_SPU_COLLISION_PROCESSING = 64,//disable parallel/SPU processing - CF_HAS_CONTACT_STIFFNESS_DAMPING = 128 + CF_HAS_CONTACT_STIFFNESS_DAMPING = 128, + CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR = 256, }; enum CollisionObjectTypes @@ -556,6 +558,26 @@ public: return m_updateRevision; } + void setCustomDebugColor(const btVector3& colorRGB) + { + m_customDebugColorRGB = colorRGB; + m_collisionFlags |= CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR; + } + + void removeCustomDebugColor() + { + m_collisionFlags &= ~CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR; + } + + bool getCustomDebugColor(btVector3& colorRGB) const + { + bool hasCustomColor = (0!=(m_collisionFlags&CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR)); + if (hasCustomColor) + { + colorRGB = m_customDebugColorRGB; + } + return hasCustomColor; + } inline bool checkCollideWith(const btCollisionObject* co) const { diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index 133994112..3bbf7586e 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -1572,6 +1572,8 @@ void btCollisionWorld::debugDrawWorld() } }; + colObj->getCustomDebugColor(color); + debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color); } if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb)) From d4a18c563434d20e576a61102a5332a31ef3f5fd Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Mon, 21 Nov 2016 10:18:48 -0800 Subject: [PATCH 5/9] Update NN3DWalkers.cpp the excessive stack-space requirements in printWalkerConfigs breaks some proprietary/internal build systems. --- examples/Evolution/NN3DWalkers.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/examples/Evolution/NN3DWalkers.cpp b/examples/Evolution/NN3DWalkers.cpp index 148ebee0c..f1420410a 100755 --- a/examples/Evolution/NN3DWalkers.cpp +++ b/examples/Evolution/NN3DWalkers.cpp @@ -1035,6 +1035,7 @@ void NN3DWalkersExample::drawMarkings() { } void NN3DWalkersExample::printWalkerConfigs(){ +#if 0 char configString[25 + NUM_WALKERS*BODYPART_COUNT*JOINT_COUNT*(3+15+1) + NUM_WALKERS*4 + 1]; // 15 precision + [],\n char* runner = configString; sprintf(runner,"Population configuration:"); @@ -1058,4 +1059,5 @@ void NN3DWalkersExample::printWalkerConfigs(){ } runner[0] = '\0'; b3Printf(configString); +#endif } From ca71b84913a6284787ee0341ff8b06d901251c78 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 21 Nov 2016 22:33:23 -0800 Subject: [PATCH 6/9] fix uninitialized m_lightColor (see ExampleBrowser/Rendering/TinyRenderer, Software was black. --- examples/TinyRenderer/TinyRenderer.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/examples/TinyRenderer/TinyRenderer.cpp b/examples/TinyRenderer/TinyRenderer.cpp index 76b9e4053..a9f9ed9c3 100644 --- a/examples/TinyRenderer/TinyRenderer.cpp +++ b/examples/TinyRenderer/TinyRenderer.cpp @@ -107,6 +107,7 @@ m_objectIndex(-1) Vec3f center(0,0,0); Vec3f up(0,0,1); m_lightDirWorld.setValue(0,0,0); + m_lightColor.setValue(1, 1, 1); m_localScaling.setValue(1,1,1); m_modelMatrix = Matrix::identity(); @@ -127,6 +128,7 @@ m_objectIndex(objectIndex) Vec3f center(0,0,0); Vec3f up(0,0,1); m_lightDirWorld.setValue(0,0,0); + m_lightColor.setValue(1, 1, 1); m_localScaling.setValue(1,1,1); m_modelMatrix = Matrix::identity(); From 0516d2ecaaceb5ad43d8612413c676961e8e223d Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Tue, 22 Nov 2016 10:11:04 -0800 Subject: [PATCH 7/9] allow getClosestPoints for btCompoundCollisionAlgorithm and btSphereTriangleCollisionAlgorithm add optional 'lightColor' arg to testrender.py script --- examples/pybullet/testrender.py | 3 ++- .../CollisionDispatch/SphereTriangleDetector.cpp | 2 +- .../CollisionDispatch/btCompoundCollisionAlgorithm.cpp | 3 +++ .../CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp | 2 +- 4 files changed, 7 insertions(+), 3 deletions(-) diff --git a/examples/pybullet/testrender.py b/examples/pybullet/testrender.py index 5935c388f..0d3475c84 100644 --- a/examples/pybullet/testrender.py +++ b/examples/pybullet/testrender.py @@ -19,6 +19,7 @@ pixelHeight = 240 nearPlane = 0.01 farPlane = 1000 lightDirection = [0,1,0] +lightColor = [1,1,1]#optional argument fov = 60 #img_arr = pybullet.renderImage(pixelWidth, pixelHeight) @@ -28,7 +29,7 @@ for pitch in range (0,360,10) : viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex) aspect = pixelWidth / pixelHeight; projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane); - img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,projectionMatrix, lightDirection) + img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,projectionMatrix, lightDirection,lightColor) w=img_arr[0] h=img_arr[1] rgb=img_arr[2] diff --git a/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp b/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp index 634017809..006cc65a2 100644 --- a/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp +++ b/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp @@ -100,7 +100,7 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po btScalar radiusWithThreshold = radius + contactBreakingThreshold; btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]); - normal.normalize(); + normal.safeNormalize(); btVector3 p1ToCentre = sphereCenter - vertices[0]; btScalar distanceFromPlane = p1ToCentre.dot(normal); diff --git a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp index 1fc960df5..7f4dea1c6 100644 --- a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp @@ -292,6 +292,9 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap btTransform otherInCompoundSpace; otherInCompoundSpace = colObjWrap->getWorldTransform().inverse() * otherObjWrap->getWorldTransform(); otherObjWrap->getCollisionShape()->getAabb(otherInCompoundSpace,localAabbMin,localAabbMax); + btVector3 extraExtends(resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold); + localAabbMin -= extraExtends; + localAabbMax += extraExtends; const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax); //process all children, that overlap with the given AABB bounds diff --git a/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp index 280a4d355..86d4e7440 100644 --- a/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp @@ -56,7 +56,7 @@ void btSphereTriangleCollisionAlgorithm::processCollision (const btCollisionObje /// report a contact. internally this will be kept persistent, and contact reduction is done resultOut->setPersistentManifold(m_manifoldPtr); - SphereTriangleDetector detector(sphere,triangle, m_manifoldPtr->getContactBreakingThreshold()); + SphereTriangleDetector detector(sphere,triangle, m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold); btDiscreteCollisionDetectorInterface::ClosestPointInput input; input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);///@todo: tighter bounds From 2e372a525e05443e757041ac95f6823908e565e4 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 28 Nov 2016 12:36:52 -0800 Subject: [PATCH 8/9] remove duplicate 'setTimeStep' in pybullet.c --- examples/pybullet/pybullet.c | 5 ----- 1 file changed, 5 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 1947ec103..61b57c083 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -3301,11 +3301,6 @@ static PyMethodDef SpamMethods[] = { "Set the amount of time to proceed at each call to stepSimulation. (unit " "is seconds, typically range is 0.01 or 0.001)"}, - - {"setTimeStep", pybullet_setTimeStep, METH_VARARGS, - "Set the amount of time to proceed at each call to stepSimulation." - " (unit is seconds, typically range is 0.01 or 0.001)"}, - {"setDefaultContactERP", pybullet_setDefaultContactERP, METH_VARARGS, "Set the amount of contact penetration Error Recovery Paramater " "(ERP) in each time step. \ From 2d42c7963a545c1822289ee45ba41c574bc535ea Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 28 Nov 2016 15:11:34 -0800 Subject: [PATCH 9/9] add pybullet getBaseVelocity / resetBaseVelocity C-API b3CreatePoseCommandSetBaseLinearVelocity/b3CreatePoseCommandSetBaseAngularVelocity --- examples/SharedMemory/PhysicsClientC_API.cpp | 36 ++ examples/SharedMemory/PhysicsClientC_API.h | 3 + .../PhysicsServerCommandProcessor.cpp | 26 +- examples/SharedMemory/SharedMemoryCommands.h | 6 +- examples/pybullet/pybullet.c | 420 +++++++++++++----- 5 files changed, 373 insertions(+), 118 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index e60953750..24d7e298c 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -672,6 +672,40 @@ int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHan return 0; } +int b3CreatePoseCommandSetBaseLinearVelocity(b3SharedMemoryCommandHandle commandHandle, double linVel[3]) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_INIT_POSE); + command->m_updateFlags |= INIT_POSE_HAS_BASE_LINEAR_VELOCITY; + command->m_initPoseArgs.m_hasInitialStateQdot[0] = 1; + command->m_initPoseArgs.m_hasInitialStateQdot[1] = 1; + command->m_initPoseArgs.m_hasInitialStateQdot[2] = 1; + + command->m_initPoseArgs.m_initialStateQdot[0] = linVel[0]; + command->m_initPoseArgs.m_initialStateQdot[1] = linVel[1]; + command->m_initPoseArgs.m_initialStateQdot[2] = linVel[2]; + + return 0; +} + +int b3CreatePoseCommandSetBaseAngularVelocity(b3SharedMemoryCommandHandle commandHandle, double angVel[3]) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_INIT_POSE); + command->m_updateFlags |= INIT_POSE_HAS_BASE_ANGULAR_VELOCITY; + command->m_initPoseArgs.m_hasInitialStateQdot[3] = 1; + command->m_initPoseArgs.m_hasInitialStateQdot[4] = 1; + command->m_initPoseArgs.m_hasInitialStateQdot[5] = 1; + + command->m_initPoseArgs.m_initialStateQdot[3] = angVel[0]; + command->m_initPoseArgs.m_initialStateQdot[4] = angVel[1]; + command->m_initPoseArgs.m_initialStateQdot[5] = angVel[2]; + + return 0; +} + int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* jointPositions) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; @@ -686,6 +720,8 @@ int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHand return 0; } + + int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index b9da28cd5..f8377c089 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -256,6 +256,9 @@ int b3CreateBoxCommandSetColorRGBA(b3SharedMemoryCommandHandle commandHandle, do b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex); int b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ); int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW); +int b3CreatePoseCommandSetBaseLinearVelocity(b3SharedMemoryCommandHandle commandHandle, double linVel[3]); +int b3CreatePoseCommandSetBaseAngularVelocity(b3SharedMemoryCommandHandle commandHandle, double angVel[3]); + int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* jointPositions); int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 18231566f..d0a3dca38 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2342,6 +2342,28 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm if (body && body->m_multiBody) { btMultiBody* mb = body->m_multiBody; + btVector3 baseLinVel(0, 0, 0); + btVector3 baseAngVel(0, 0, 0); + + + if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY) + { + baseLinVel.setValue(clientCmd.m_initPoseArgs.m_initialStateQdot[0], + clientCmd.m_initPoseArgs.m_initialStateQdot[1], + clientCmd.m_initPoseArgs.m_initialStateQdot[2]); + mb->setBaseVel(baseLinVel); + + } + + if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_ANGULAR_VELOCITY) + { + baseAngVel.setValue(clientCmd.m_initPoseArgs.m_initialStateQdot[3], + clientCmd.m_initPoseArgs.m_initialStateQdot[4], + clientCmd.m_initPoseArgs.m_initialStateQdot[5]); + mb->setBaseOmega(baseAngVel); + } + + if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION) { btVector3 zero(0,0,0); @@ -2349,7 +2371,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm clientCmd.m_initPoseArgs.m_hasInitialStateQ[1] && clientCmd.m_initPoseArgs.m_hasInitialStateQ[2]); - mb->setBaseVel(zero); + mb->setBaseVel(baseLinVel); mb->setBasePos(btVector3( clientCmd.m_initPoseArgs.m_initialStateQ[0], clientCmd.m_initPoseArgs.m_initialStateQ[1], @@ -2362,7 +2384,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm clientCmd.m_initPoseArgs.m_hasInitialStateQ[5] && clientCmd.m_initPoseArgs.m_hasInitialStateQ[6]); - mb->setBaseOmega(btVector3(0,0,0)); + mb->setBaseOmega(baseAngVel); btQuaternion invOrn(clientCmd.m_initPoseArgs.m_initialStateQ[3], clientCmd.m_initPoseArgs.m_initialStateQ[4], clientCmd.m_initPoseArgs.m_initialStateQ[5], diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 5b3346433..5d25059c2 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -109,7 +109,9 @@ enum EnumInitPoseFlags { INIT_POSE_HAS_INITIAL_POSITION=1, INIT_POSE_HAS_INITIAL_ORIENTATION=2, - INIT_POSE_HAS_JOINT_STATE=4 + INIT_POSE_HAS_JOINT_STATE=4, + INIT_POSE_HAS_BASE_LINEAR_VELOCITY = 8, + INIT_POSE_HAS_BASE_ANGULAR_VELOCITY = 16, }; @@ -122,6 +124,8 @@ struct InitPoseArgs int m_bodyUniqueId; int m_hasInitialStateQ[MAX_DEGREE_OF_FREEDOM]; double m_initialStateQ[MAX_DEGREE_OF_FREEDOM]; + int m_hasInitialStateQdot[MAX_DEGREE_OF_FREEDOM]; + double m_initialStateQdot[MAX_DEGREE_OF_FREEDOM]; }; diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 61b57c083..7aeaab7e0 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -30,6 +30,123 @@ enum eCONNECT_METHOD { static PyObject* SpamError; static b3PhysicsClientHandle sm = 0; + +static double pybullet_internalGetFloatFromSequence(PyObject* seq, int index) { + double v = 0.0; + PyObject* item; + + if (PyList_Check(seq)) { + item = PyList_GET_ITEM(seq, index); + v = PyFloat_AsDouble(item); + } + else { + item = PyTuple_GET_ITEM(seq, index); + v = PyFloat_AsDouble(item); + } + return v; +} + + +// internal function to set a float matrix[16] +// used to initialize camera position with +// a view and projection matrix in renderImage() +// +// // Args: +// matrix - float[16] which will be set by values from objMat +static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16]) { + int i, len; + PyObject* seq; + + seq = PySequence_Fast(objMat, "expected a sequence"); + if (seq) + { + len = PySequence_Size(objMat); + if (len == 16) { + for (i = 0; i < len; i++) { + matrix[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + Py_DECREF(seq); + return 1; + } + Py_DECREF(seq); + } + return 0; +} + +// internal function to set a float vector[3] +// used to initialize camera position with +// a view and projection matrix in renderImage() +// +// // Args: +// vector - float[3] which will be set by values from objMat +static int pybullet_internalSetVector(PyObject* objVec, float vector[3]) { + int i, len; + PyObject* seq = 0; + if (objVec == NULL) + return 0; + + seq = PySequence_Fast(objVec, "expected a sequence"); + if (seq) + { + + len = PySequence_Size(objVec); + if (len == 3) { + for (i = 0; i < len; i++) { + vector[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + Py_DECREF(seq); + return 1; + } + Py_DECREF(seq); + } + return 0; +} + +// vector - double[3] which will be set by values from obVec +static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) { + int i, len; + PyObject* seq; + if (obVec == NULL) + return 0; + + seq = PySequence_Fast(obVec, "expected a sequence"); + if (seq) + { + len = PySequence_Size(obVec); + if (len == 3) { + for (i = 0; i < len; i++) { + vector[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + Py_DECREF(seq); + return 1; + } + Py_DECREF(seq); + } + return 0; +} + +// vector - double[3] which will be set by values from obVec +static int pybullet_internalSetVector4d(PyObject* obVec, double vector[4]) { + int i, len; + PyObject* seq; + if (obVec == NULL) + return 0; + + seq = PySequence_Fast(obVec, "expected a sequence"); + len = PySequence_Size(obVec); + if (len == 4) { + for (i = 0; i < len; i++) { + vector[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + Py_DECREF(seq); + return 1; + } + Py_DECREF(seq); + return 0; +} + + + // Step through one timestep of the simulation static PyObject* pybullet_stepSimulation(PyObject* self, PyObject* args) { if (0 == sm) { @@ -371,19 +488,6 @@ static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args) { return PyLong_FromLong(bodyIndex); } -static double pybullet_internalGetFloatFromSequence(PyObject* seq, int index) { - double v = 0.0; - PyObject* item; - - if (PyList_Check(seq)) { - item = PyList_GET_ITEM(seq, index); - v = PyFloat_AsDouble(item); - } else { - item = PyTuple_GET_ITEM(seq, index); - v = PyFloat_AsDouble(item); - } - return v; -} @@ -768,11 +872,68 @@ pybullet_setDefaultContactERP(PyObject* self, PyObject* args) return Py_None; } +static int pybullet_internalGetBaseVelocity( + int bodyIndex, double baseLinearVelocity[3], double baseAngularVelocity[3]) { + baseLinearVelocity[0] = 0.; + baseLinearVelocity[1] = 0.; + baseLinearVelocity[2] = 0.; + + baseAngularVelocity[0] = 0.; + baseAngularVelocity[1] = 0.; + baseAngularVelocity[2] = 0.; + + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return 0; + } + + { + { + b3SharedMemoryCommandHandle cmd_handle = + b3RequestActualStateCommandInit(sm, bodyIndex); + b3SharedMemoryStatusHandle status_handle = + b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); + + const int status_type = b3GetStatusType(status_handle); + const double* actualStateQdot; + // const double* jointReactionForces[]; + + + if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) { + PyErr_SetString(SpamError, "getBaseVelocity failed."); + return 0; + } + + b3GetStatusActualState( + status_handle, 0 /* body_unique_id */, + 0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */, + 0 /*root_local_inertial_frame*/, 0, + &actualStateQdot, 0 /* joint_reaction_forces */); + + // printf("joint reaction forces="); + // for (i=0; i < (sizeof(jointReactionForces)/sizeof(double)); i++) { + // printf("%f ", jointReactionForces[i]); + // } + // now, position x,y,z = actualStateQ[0],actualStateQ[1],actualStateQ[2] + // and orientation x,y,z,w = + // actualStateQ[3],actualStateQ[4],actualStateQ[5],actualStateQ[6] + baseLinearVelocity[0] = actualStateQdot[0]; + baseLinearVelocity[1] = actualStateQdot[1]; + baseLinearVelocity[2] = actualStateQdot[2]; + + baseAngularVelocity[0] = actualStateQdot[3]; + baseAngularVelocity[1] = actualStateQdot[4]; + baseAngularVelocity[2] = actualStateQdot[5]; + + } + } + return 1; +} // Internal function used to get the base position and orientation // Orientation is returned in quaternions static int pybullet_internalGetBasePositionAndOrientation( - int bodyIndex, double basePosition[3], double baseOrientation[3]) { + int bodyIndex, double basePosition[3], double baseOrientation[4]) { basePosition[0] = 0.; basePosition[1] = 0.; basePosition[2] = 0.; @@ -855,8 +1016,7 @@ static PyObject* pybullet_getBasePositionAndOrientation(PyObject* self, if (0 == pybullet_internalGetBasePositionAndOrientation( bodyIndex, basePosition, baseOrientation)) { PyErr_SetString(SpamError, - "GetBasePositionAndOrientation failed (#joints/links " - "exceeds maximum?)."); + "GetBasePositionAndOrientation failed."); return NULL; } @@ -891,6 +1051,62 @@ static PyObject* pybullet_getBasePositionAndOrientation(PyObject* self, } } + +static PyObject* pybullet_getBaseVelocity(PyObject* self, + PyObject* args) { + int bodyIndex = -1; + double baseLinearVelocity[3]; + double baseAngularVelocity[3]; + PyObject* pylistLinVel=0; + PyObject* pylistAngVel=0; + + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + if (!PyArg_ParseTuple(args, "i", &bodyIndex)) { + PyErr_SetString(SpamError, "Expected a body index (integer)."); + return NULL; + } + + if (0 == pybullet_internalGetBaseVelocity( + bodyIndex, baseLinearVelocity, baseAngularVelocity)) { + PyErr_SetString(SpamError, + "getBaseVelocity failed."); + return NULL; + } + + { + PyObject* item; + int i; + int num = 3; + pylistLinVel = PyTuple_New(num); + for (i = 0; i < num; i++) { + item = PyFloat_FromDouble(baseLinearVelocity[i]); + PyTuple_SetItem(pylistLinVel, i, item); + } + } + + { + PyObject* item; + int i; + int num = 3; + pylistAngVel = PyTuple_New(num); + for (i = 0; i < num; i++) { + item = PyFloat_FromDouble(baseAngularVelocity[i]); + PyTuple_SetItem(pylistAngVel, i, item); + } + } + + { + PyObject* pylist; + pylist = PyTuple_New(2); + PyTuple_SetItem(pylist, 0, pylistLinVel); + PyTuple_SetItem(pylist, 1, pylistAngVel); + return pylist; + } +} static PyObject* pybullet_getNumBodies(PyObject* self, PyObject* args) { if (0 == sm) { @@ -1035,6 +1251,66 @@ static PyObject* pybullet_resetJointState(PyObject* self, PyObject* args) { return NULL; } + + + +static PyObject* pybullet_resetBaseVelocity(PyObject* self, PyObject* args, PyObject *keywds) +{ + static char *kwlist[] = { "objectUniqueId", "linearVelocity", "angularVelocity", NULL }; + + if (0 == sm) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + int bodyIndex=0; + PyObject* linVelObj=0; + PyObject* angVelObj=0; + double linVel[3] = { 0, 0, 0 }; + double angVel[3] = { 0, 0, 0 }; + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OO", kwlist, &bodyIndex, &linVelObj, &angVelObj)) + { + return NULL; + } + if (linVelObj || angVelObj) + { + + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + + commandHandle = b3CreatePoseCommandInit(sm, bodyIndex); + + if (linVelObj) + { + pybullet_internalSetVectord(linVelObj, linVel); + b3CreatePoseCommandSetBaseLinearVelocity(commandHandle, linVel); + } + + if (angVelObj) + { + pybullet_internalSetVectord(angVelObj, angVel); + b3CreatePoseCommandSetBaseAngularVelocity(commandHandle, angVel); + } + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + Py_INCREF(Py_None); + return Py_None; + } + else + { + PyErr_SetString(SpamError, "expected at least linearVelocity and/or angularVelocity."); + return NULL; + } + } + PyErr_SetString(SpamError, "error in resetJointState."); + return NULL; +} + + + // Reset the position and orientation of the base/root link, position [x,y,z] // and orientation quaternion [x,y,z,w] static PyObject* pybullet_resetBasePositionAndOrientation(PyObject* self, @@ -1366,105 +1642,6 @@ static PyObject* pybullet_getLinkState(PyObject* self, PyObject* args) { return Py_None; } -// internal function to set a float matrix[16] -// used to initialize camera position with -// a view and projection matrix in renderImage() -// -// // Args: -// matrix - float[16] which will be set by values from objMat -static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16]) { - int i, len; - PyObject* seq; - - seq = PySequence_Fast(objMat, "expected a sequence"); - if (seq) - { - len = PySequence_Size(objMat); - if (len == 16) { - for (i = 0; i < len; i++) { - matrix[i] = pybullet_internalGetFloatFromSequence(seq, i); - } - Py_DECREF(seq); - return 1; - } - Py_DECREF(seq); - } - return 0; -} - -// internal function to set a float vector[3] -// used to initialize camera position with -// a view and projection matrix in renderImage() -// -// // Args: -// vector - float[3] which will be set by values from objMat -static int pybullet_internalSetVector(PyObject* objVec, float vector[3]) { - int i, len; - PyObject* seq=0; - if (objVec==NULL) - return 0; - - seq = PySequence_Fast(objVec, "expected a sequence"); - if (seq) - { - - len = PySequence_Size(objVec); - if (len == 3) { - for (i = 0; i < len; i++) { - vector[i] = pybullet_internalGetFloatFromSequence(seq, i); - } - Py_DECREF(seq); - return 1; - } - Py_DECREF(seq); - } - return 0; -} - -// vector - double[3] which will be set by values from obVec -static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) { - int i, len; - PyObject* seq; - if (obVec==NULL) - return 0; - - seq = PySequence_Fast(obVec, "expected a sequence"); - if (seq) - { - len = PySequence_Size(obVec); - if (len == 3) { - for (i = 0; i < len; i++) { - vector[i] = pybullet_internalGetFloatFromSequence(seq, i); - } - Py_DECREF(seq); - return 1; - } - Py_DECREF(seq); - } - return 0; -} - -// vector - double[3] which will be set by values from obVec -static int pybullet_internalSetVector4d(PyObject* obVec, double vector[4]) { - int i, len; - PyObject* seq; - if (obVec==NULL) - return 0; - - seq = PySequence_Fast(obVec, "expected a sequence"); - len = PySequence_Size(obVec); - if (len == 4) { - for (i = 0; i < len; i++) { - vector[i] = pybullet_internalGetFloatFromSequence(seq, i); - } - Py_DECREF(seq); - return 1; - } - Py_DECREF(seq); - return 0; -} - - static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObject *keywds) { @@ -3361,6 +3538,19 @@ static PyMethodDef SpamMethods[] = { "instantaneously, not through physics simulation. (x,y,z) position vector " "and (x,y,z,w) quaternion orientation."}, + { "getBaseVelocity", pybullet_getBaseVelocity, + METH_VARARGS, + "Get the linear and angular velocity of the base of the object " + " in world space coordinates. " + "(x,y,z) linear velocity vector and (x,y,z) angular velocity vector." }, + + { "resetBaseVelocity", (PyCFunction)pybullet_resetBaseVelocity, METH_VARARGS | METH_KEYWORDS, + "Reset the linear and/or angular velocity of the base of the object " + " in world space coordinates. " + "linearVelocity (x,y,z) and angularVelocity (x,y,z)." }, + + + {"getNumJoints", pybullet_getNumJoints, METH_VARARGS, "Get the number of joints for an object."},