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 } 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 dafe5880e..6b3e97ae3 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; @@ -1195,6 +1231,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) @@ -1533,6 +1609,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; } @@ -1545,6 +1623,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 778e2e5b8..4ee0e394d 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -89,9 +89,15 @@ 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]); @@ -126,12 +132,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); @@ -248,6 +258,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 68cb5ce8b..2fd291e2b 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), @@ -2352,6 +2352,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); @@ -2359,7 +2381,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], @@ -2372,7 +2394,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], @@ -2784,6 +2806,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; @@ -2798,15 +2826,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); + } } } } @@ -2826,15 +2860,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); + } } } } @@ -3528,7 +3568,6 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm for( int i=0;igetRigidBodyByIndex(i); - if (colObj) { btRigidBody* rb = btRigidBody::upcast(colObj); @@ -3591,7 +3630,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/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index c4aa9455a..aff18b0f8 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -825,6 +825,7 @@ public: virtual bool wantsTermination(); virtual bool isConnected(); virtual void renderScene(); + void drawUserDebugLines(); virtual void exitPhysics(); virtual void physicsDebugDraw(int debugFlags); @@ -1353,7 +1354,51 @@ 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) + { + + 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); + + } + } + +} void PhysicsServerExample::renderScene() { @@ -1369,48 +1414,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 +1429,7 @@ void PhysicsServerExample::renderScene() static int count = 0; count++; +#if 0 if (0 == (count & 1)) { btScalar curTime = m_clock.getTimeSeconds(); @@ -1444,6 +1450,7 @@ void PhysicsServerExample::renderScene() worseFps = 1000000; } } +#endif #ifdef BT_ENABLE_VR if ((gInternalSimFlags&2 ) && m_tinyVrGui==0) @@ -1468,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); @@ -1544,6 +1553,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 eb91f3156..c360a5511 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]; }; @@ -160,6 +164,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 @@ -167,6 +173,8 @@ struct RequestContactDataArgs int m_startingContactPointIndex; int m_objectAIndexFilter; int m_objectBIndexFilter; + int m_linkIndexAIndexFilter; + int m_linkIndexBIndexFilter; double m_closestDistanceThreshold; int m_mode; }; @@ -526,7 +534,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 @@ -543,6 +554,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/TinyRenderer/TinyRenderer.cpp b/examples/TinyRenderer/TinyRenderer.cpp index 9607d43c4..2f346fcdb 100644 --- a/examples/TinyRenderer/TinyRenderer.cpp +++ b/examples/TinyRenderer/TinyRenderer.cpp @@ -172,6 +172,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(); @@ -193,6 +194,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(); diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 2f36fa669..9ed3bdd60 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) { @@ -1501,7 +1678,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 +1687,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; } } @@ -1647,8 +1824,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) { @@ -1848,23 +2062,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 +2094,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 +2214,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 +2226,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 +2242,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); @@ -3241,11 +3483,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. \ @@ -3306,6 +3543,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."}, @@ -3404,6 +3654,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/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/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/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/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/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 3196369e1..3bbf7586e 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); @@ -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)) diff --git a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp index 13cddc11a..7f4dea1c6 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)) @@ -271,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/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/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 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();