From 7e76ee0ad77e75d78b2475859bb235c6e3e28276 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 17 Jun 2019 21:43:38 -0700 Subject: [PATCH] make pybullet.getMeshData work for softbody --- examples/SharedMemory/PhysicsClientC_API.cpp | 13 +- examples/SharedMemory/PhysicsClientC_API.h | 2 +- .../PhysicsClientSharedMemory.cpp | 57 ++++--- examples/SharedMemory/PhysicsDirect.cpp | 48 +++--- .../PhysicsServerCommandProcessor.cpp | 43 ++++- examples/SharedMemory/RemoteGUIHelper.cpp | 155 ++++++++---------- examples/SharedMemory/SharedMemoryCommands.h | 21 ++- examples/SharedMemory/SharedMemoryPublic.h | 16 +- examples/pybullet/examples/load_soft_body.py | 4 +- examples/pybullet/pybullet.c | 83 ++++++---- 10 files changed, 244 insertions(+), 198 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index ae2c10010..4a08c5e6d 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1210,7 +1210,8 @@ B3_SHARED_API int b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle co return -1; } -B3_SHARED_API b3SharedMemoryCommandHandle b3GetMeshDataCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId){ +B3_SHARED_API b3SharedMemoryCommandHandle b3GetMeshDataCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex) +{ PhysicsClient* cl = (PhysicsClient*)physClient; b3Assert(cl); b3Assert(cl->canSubmitCommand()); @@ -1220,23 +1221,23 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3GetMeshDataCommandInit(b3PhysicsClie b3Assert(command); command->m_type = CMD_REQUEST_MESH_DATA; command->m_updateFlags = 0; + command->m_requestMeshDataArgs.m_startingVertex = 0; command->m_requestMeshDataArgs.m_bodyUniqueId = bodyUniqueId; + command->m_requestMeshDataArgs.m_linkIndex = linkIndex; return (b3SharedMemoryCommandHandle)command; } return 0; } -B3_SHARED_API void b3GetMeshData(b3PhysicsClientHandle physClient, struct b3MeshData* meshData){ - PhysicsClient* cl = (PhysicsClient*)physClient; +B3_SHARED_API void b3GetMeshData(b3PhysicsClientHandle physClient, struct b3MeshData* meshData) +{ + PhysicsClient* cl = (PhysicsClient*)physClient; if (cl) { cl->getCachedMeshData(meshData); } } - - - B3_SHARED_API int b3CreateVisualShapeAddSphere(b3SharedMemoryCommandHandle commandHandle, double radius) { return b3CreateCollisionShapeAddSphere(commandHandle, radius); diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index bd4ca05b8..ec50a838a 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -491,7 +491,7 @@ extern "C" B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveCollisionShapeCommand(b3PhysicsClientHandle physClient, int collisionShapeId); - B3_SHARED_API b3SharedMemoryCommandHandle b3GetMeshDataCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId); + B3_SHARED_API b3SharedMemoryCommandHandle b3GetMeshDataCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex); B3_SHARED_API void b3GetMeshData(b3PhysicsClientHandle physClient, struct b3MeshData* meshData); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index ade598ae8..daebb7afd 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -53,8 +53,8 @@ struct PhysicsClientSharedMemoryInternalData btAlignedObjectArray m_cachedVisualShapes; btAlignedObjectArray m_cachedCollisionShapes; - b3MeshData m_cachedMeshData; - btAlignedObjectArray m_cachedVertexPositions; + b3MeshData m_cachedMeshData; + btAlignedObjectArray m_cachedVertexPositions; btAlignedObjectArray m_cachedVREvents; btAlignedObjectArray m_cachedKeyboardEvents; @@ -100,6 +100,8 @@ struct PhysicsClientSharedMemoryInternalData m_verboseOutput(false), m_timeOutInSeconds(1e30) { + m_cachedMeshData.m_numVertices = 0; + m_cachedMeshData.m_vertices = 0; } void processServerStatus(); @@ -1035,15 +1037,24 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() b3Warning("Camera image FAILED\n"); break; } - case CMD_REQUEST_MESH_DATA_COMPLETED: - { - break; - } - case CMD_REQUEST_MESH_DATA_FAILED: - { - b3Warning("Request mesh data failed"); - break; - } + case CMD_REQUEST_MESH_DATA_COMPLETED: + { + m_data->m_cachedVertexPositions.resize(serverCmd.m_sendMeshDataArgs.m_startingVertex + serverCmd.m_sendMeshDataArgs.m_numVerticesCopied); + btVector3* verticesReceived = (btVector3*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor; + for (int i = 0; i < serverCmd.m_sendMeshDataArgs.m_numVerticesCopied; i++) + { + m_data->m_cachedVertexPositions[i + serverCmd.m_sendMeshDataArgs.m_startingVertex].x = verticesReceived[i].x(); + m_data->m_cachedVertexPositions[i + serverCmd.m_sendMeshDataArgs.m_startingVertex].y = verticesReceived[i].y(); + m_data->m_cachedVertexPositions[i + serverCmd.m_sendMeshDataArgs.m_startingVertex].z = verticesReceived[i].z(); + m_data->m_cachedVertexPositions[i + serverCmd.m_sendMeshDataArgs.m_startingVertex].w = verticesReceived[i].w(); + } + break; + } + case CMD_REQUEST_MESH_DATA_FAILED: + { + b3Warning("Request mesh data failed"); + break; + } case CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED: { break; @@ -1770,15 +1781,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() } } - if(serverCmd.m_type == CMD_REQUEST_MESH_DATA_COMPLETED) - { + if (serverCmd.m_type == CMD_REQUEST_MESH_DATA_COMPLETED) + { B3_PROFILE("CMD_REQUEST_MESH_DATA_COMPLETED"); - SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; + SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; - if (serverCmd.m_sendMeshDataArgs.m_numVerticesRemaining > 0 && serverCmd.m_sendMeshDataArgs.m_numVerticesCopied) + if (serverCmd.m_sendMeshDataArgs.m_numVerticesRemaining > 0 && serverCmd.m_sendMeshDataArgs.m_numVerticesCopied) { command.m_type = CMD_REQUEST_MESH_DATA; - command.m_requestMeshDataArgs.m_startingVertex= + command.m_requestMeshDataArgs.m_startingVertex = serverCmd.m_sendMeshDataArgs.m_startingVertex + serverCmd.m_sendMeshDataArgs.m_numVerticesCopied; submitClientCommand(command); @@ -1786,9 +1797,9 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() } else { - m_data->m_cachedMeshData.m_numVertices =serverCmd.m_sendMeshDataArgs.m_startingVertex +serverCmd.m_sendMeshDataArgs.m_numVerticesCopied; + m_data->m_cachedMeshData.m_numVertices = serverCmd.m_sendMeshDataArgs.m_startingVertex + serverCmd.m_sendMeshDataArgs.m_numVerticesCopied; } - } + } if ((serverCmd.m_type == CMD_DEBUG_LINES_COMPLETED) && (serverCmd.m_sendDebugLinesArgs.m_numRemainingDebugLines > 0)) @@ -1972,9 +1983,13 @@ void PhysicsClientSharedMemory::getCachedCollisionShapeInformation(struct b3Coll collisionShapesInfo->m_collisionShapeData = collisionShapesInfo->m_numCollisionShapes ? &m_data->m_cachedCollisionShapes[0] : 0; } - -void PhysicsClientSharedMemory::getCachedMeshData(struct b3MeshData* meshData){ - *meshData = m_data->m_cachedMeshData; +void PhysicsClientSharedMemory::getCachedMeshData(struct b3MeshData* meshData) +{ + m_data->m_cachedMeshData.m_numVertices = m_data->m_cachedVertexPositions.size(); + + m_data->m_cachedMeshData.m_vertices = m_data->m_cachedMeshData.m_numVertices ? &m_data->m_cachedVertexPositions[0] : 0; + + *meshData = m_data->m_cachedMeshData; } const float* PhysicsClientSharedMemory::getDebugLinesFrom() const diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index fac907650..d838cc7ba 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -66,8 +66,8 @@ struct PhysicsDirectInternalData btAlignedObjectArray m_cachedVisualShapes; btAlignedObjectArray m_cachedCollisionShapes; - b3MeshData m_cachedMeshData; - btAlignedObjectArray m_cachedVertexPositions; + b3MeshData m_cachedMeshData; + btAlignedObjectArray m_cachedVertexPositions; btAlignedObjectArray m_cachedVREvents; @@ -94,6 +94,7 @@ struct PhysicsDirectInternalData m_ownsCommandProcessor(false), m_timeOutInSeconds(1e30) { + memset(&m_cachedMeshData.m_numVertices, 0, sizeof(b3MeshData)); memset(&m_command, 0, sizeof(m_command)); memset(&m_serverStatus, 0, sizeof(m_serverStatus)); memset(m_bulletStreamDataServerToClient, 0, sizeof(m_bulletStreamDataServerToClient)); @@ -589,13 +590,12 @@ bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand) return m_data->m_hasStatus; } - bool PhysicsDirect::processMeshData(const struct SharedMemoryCommand& orgCommand) { SharedMemoryCommand command = orgCommand; const SharedMemoryStatus& serverCmd = m_data->m_serverStatus; - do + do { bool hasStatus = m_data->m_commandProcessor->processCommand(command, m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); @@ -622,21 +622,23 @@ bool PhysicsDirect::processMeshData(const struct SharedMemoryCommand& orgCommand b3Printf("Mesh data OK\n"); } - const b3SendMeshDataArgs& args = serverCmd.m_sendMeshDataArgs; - int numTotalPixels =args.m_startingVertex + - args.m_numVerticesCopied + args.m_numVerticesRemaining; + const b3SendMeshDataArgs& args = serverCmd.m_sendMeshDataArgs; + int numTotalPixels = args.m_startingVertex + + args.m_numVerticesCopied + args.m_numVerticesRemaining; - double* verticesReceived = - (double*)&m_data->m_bulletStreamDataServerToClient[0]; + btVector3* verticesReceived = + (btVector3*)&m_data->m_bulletStreamDataServerToClient[0]; - // flattened position - const int dimension = 3; - m_data->m_cachedVertexPositions.resize((args.m_startingVertex + - args.m_numVerticesCopied)*dimension); + + m_data->m_cachedVertexPositions.resize(args.m_startingVertex + + args.m_numVerticesCopied); - for (int i = 0; i < dimension*args.m_numVerticesCopied; i++) + for (int i = 0; i < args.m_numVerticesCopied; i++) { - m_data->m_cachedVertexPositions[i + dimension*args.m_startingVertex] = verticesReceived[i]; + m_data->m_cachedVertexPositions[i + args.m_startingVertex].x = verticesReceived[i].x(); + m_data->m_cachedVertexPositions[i + args.m_startingVertex].y = verticesReceived[i].y(); + m_data->m_cachedVertexPositions[i + args.m_startingVertex].z = verticesReceived[i].z(); + m_data->m_cachedVertexPositions[i + args.m_startingVertex].w = verticesReceived[i].w(); } if (args.m_numVerticesRemaining > 0 && args.m_numVerticesCopied) @@ -646,12 +648,12 @@ bool PhysicsDirect::processMeshData(const struct SharedMemoryCommand& orgCommand // continue requesting remaining vertices command.m_type = CMD_REQUEST_MESH_DATA; command.m_requestMeshDataArgs.m_startingVertex = - args.m_startingVertex + args.m_numVerticesCopied; + args.m_startingVertex + args.m_numVerticesCopied; } else { - m_data->m_cachedMeshData.m_numVertices=args.m_startingVertex + - args.m_numVerticesCopied; + m_data->m_cachedMeshData.m_numVertices = args.m_startingVertex + + args.m_numVerticesCopied; } } } while (serverCmd.m_sendMeshDataArgs.m_numVerticesRemaining > 0 && serverCmd.m_sendMeshDataArgs.m_numVerticesCopied); @@ -1526,9 +1528,15 @@ void PhysicsDirect::getCachedCameraImage(b3CameraImageData* cameraData) } } -void PhysicsDirect::getCachedMeshData(struct b3MeshData* meshData){ - *meshData = m_data->m_cachedMeshData; +void PhysicsDirect::getCachedMeshData(struct b3MeshData* meshData) +{ + m_data->m_cachedMeshData.m_numVertices = m_data->m_cachedVertexPositions.size(); + + m_data->m_cachedMeshData.m_vertices = m_data->m_cachedMeshData.m_numVertices ? &m_data->m_cachedVertexPositions[0] : 0; + + *meshData = m_data->m_cachedMeshData; } + void PhysicsDirect::getCachedContactPointInformation(struct b3ContactInformation* contactPointData) { contactPointData->m_numContactPoints = m_data->m_cachedContactPoints.size(); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 2da6a6ee3..93a5f1d3d 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -4630,16 +4630,41 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S { bool hasStatus = true; BT_PROFILE("CMD_REQUEST_MESH_DATA"); - serverStatusOut.m_type = CMD_REQUEST_MESH_DATA_FAILED; + serverStatusOut.m_type = CMD_REQUEST_MESH_DATA_FAILED; serverStatusOut.m_numDataStreamBytes = 0; InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_requestMeshDataArgs.m_bodyUniqueId); if (bodyHandle) { - serverStatusOut.m_type = CMD_REQUEST_MESH_DATA_COMPLETED; - serverStatusOut.m_sendMeshDataArgs.m_numVerticesCopied = 1; - serverStatusOut.m_sendMeshDataArgs.m_startingVertex = 0; - serverStatusOut.m_sendMeshDataArgs.m_numVerticesRemaining = 0; + if (bodyHandle->m_multiBody) + { + //todo + } + if (bodyHandle->m_rigidBody) + { + //todo + } + if (bodyHandle->m_softBody) + { + btSoftBody* psb = bodyHandle->m_softBody; + int totalBytesPerVertex = sizeof(btVector3); + int numVertices = psb->m_nodes.size(); + int maxNumVertices = bufferSizeInBytes / totalBytesPerVertex - 1; + int numVerticesRemaining = numVertices - clientCmd.m_requestMeshDataArgs.m_startingVertex; + int verticesCopied = btMin(maxNumVertices, numVerticesRemaining); + btVector3* verticesOut = (btVector3*)bufferServerToClient; + for (int i = 0; i < verticesCopied; ++i) + { + const btSoftBody::Node& n = psb->m_nodes[i+ clientCmd.m_requestMeshDataArgs.m_startingVertex]; + verticesOut[i] = n.m_x; + } + + serverStatusOut.m_type = CMD_REQUEST_MESH_DATA_COMPLETED; + serverStatusOut.m_sendMeshDataArgs.m_numVerticesCopied = verticesCopied; + serverStatusOut.m_sendMeshDataArgs.m_startingVertex = clientCmd.m_requestMeshDataArgs.m_startingVertex; + serverStatusOut.m_sendMeshDataArgs.m_numVerticesRemaining = numVerticesRemaining - verticesCopied; + } + } serverStatusOut.m_numDataStreamBytes = 0; @@ -4647,7 +4672,6 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S return hasStatus; } - bool PhysicsServerCommandProcessor::processCreateVisualShapeCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) { bool hasStatus = true; @@ -11309,10 +11333,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm hasStatus = processCreateVisualShapeCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); break; } - case CMD_REQUEST_MESH_DATA:{ - hasStatus = processRequestMeshDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + case CMD_REQUEST_MESH_DATA: + { + hasStatus = processRequestMeshDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); break; - } + } case CMD_CREATE_MULTI_BODY: { hasStatus = processCreateMultiBodyCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); diff --git a/examples/SharedMemory/RemoteGUIHelper.cpp b/examples/SharedMemory/RemoteGUIHelper.cpp index be51adad3..3e804357e 100644 --- a/examples/SharedMemory/RemoteGUIHelper.cpp +++ b/examples/SharedMemory/RemoteGUIHelper.cpp @@ -16,7 +16,7 @@ struct RemoteGUIHelperInternalData { -// GUIHelperInterface* m_guiHelper; + // GUIHelperInterface* m_guiHelper; bool m_waitingForServer; GraphicsSharedMemoryBlock* m_testBlock1; SharedMemoryInterface* m_sharedMemory; @@ -26,7 +26,7 @@ struct RemoteGUIHelperInternalData RemoteGUIHelperInternalData() : m_waitingForServer(false), - m_testBlock1(0) + m_testBlock1(0) { #ifdef _WIN32 m_sharedMemory = new Win32SharedMemoryClient(); @@ -36,7 +36,6 @@ struct RemoteGUIHelperInternalData m_sharedMemoryKey = GRAPHICS_SHARED_MEMORY_KEY; m_isConnected = false; connect(); - } virtual ~RemoteGUIHelperInternalData() @@ -98,7 +97,6 @@ struct RemoteGUIHelperInternalData return false; } - const GraphicsSharedMemoryStatus* processServerStatus() { // SharedMemoryStatus* stat = 0; @@ -120,13 +118,12 @@ struct RemoteGUIHelperInternalData return &m_lastServerStatus; } - if (m_testBlock1->m_numServerCommands > m_testBlock1->m_numProcessedServerCommands) { B3_PROFILE("processServerCMD"); b3Assert(m_testBlock1->m_numServerCommands == - m_testBlock1->m_numProcessedServerCommands + 1); + m_testBlock1->m_numProcessedServerCommands + 1); const GraphicsSharedMemoryStatus& serverCmd = m_testBlock1->m_serverCommands[0]; @@ -136,22 +133,21 @@ struct RemoteGUIHelperInternalData // consume the command switch (serverCmd.m_type) { - case GFX_CMD_CLIENT_COMMAND_COMPLETED: - { - B3_PROFILE("CMD_CLIENT_COMMAND_COMPLETED"); + case GFX_CMD_CLIENT_COMMAND_COMPLETED: + { + B3_PROFILE("CMD_CLIENT_COMMAND_COMPLETED"); - - break; - } - default: - { - } + break; + } + default: + { + } } m_testBlock1->m_numProcessedServerCommands++; // we don't have more than 1 command outstanding (in total, either server or client) b3Assert(m_testBlock1->m_numProcessedServerCommands == - m_testBlock1->m_numServerCommands); + m_testBlock1->m_numServerCommands); if (m_testBlock1->m_numServerCommands == m_testBlock1->m_numProcessedServerCommands) @@ -163,7 +159,6 @@ struct RemoteGUIHelperInternalData m_waitingForServer = true; } - return &m_lastServerStatus; } return 0; @@ -182,7 +177,7 @@ struct RemoteGUIHelperInternalData { b3Error("Error connecting to shared memory: please start server before client\n"); m_sharedMemory->releaseSharedMemory(m_sharedMemoryKey, - GRAPHICS_SHARED_MEMORY_SIZE); + GRAPHICS_SHARED_MEMORY_SIZE); m_testBlock1 = 0; return false; } @@ -199,7 +194,6 @@ struct RemoteGUIHelperInternalData return true; } - void disconnect() { if (m_isConnected && m_sharedMemory) @@ -208,16 +202,9 @@ struct RemoteGUIHelperInternalData } m_isConnected = false; } - }; - - - - - - -RemoteGUIHelper::RemoteGUIHelper() +RemoteGUIHelper::RemoteGUIHelper() { m_data = new RemoteGUIHelperInternalData; if (m_data->canSubmitCommand()) @@ -264,11 +251,11 @@ void RemoteGUIHelper::createCollisionObjectGraphicsObject(btCollisionObject* bod { // btAssert(graphicsShapeId >= 0); //the graphics shape is already scaled - float localScaling[4] = { 1.f, 1.f, 1.f, 1.f }; - float colorRGBA[4] = { (float)color[0], (float)color[1], (float)color[2], (float) color[3] }; - float pos[4] = { (float)startTransform.getOrigin()[0], (float)startTransform.getOrigin()[1], (float)startTransform.getOrigin()[2], (float)startTransform.getOrigin()[3] }; - float orn[4] = { (float)startTransform.getRotation()[0], (float)startTransform.getRotation()[1], (float)startTransform.getRotation()[2], (float)startTransform.getRotation()[3] }; - int graphicsInstanceId = registerGraphicsInstance(graphicsShapeId, pos,orn, colorRGBA, localScaling); + float localScaling[4] = {1.f, 1.f, 1.f, 1.f}; + float colorRGBA[4] = {(float)color[0], (float)color[1], (float)color[2], (float)color[3]}; + float pos[4] = {(float)startTransform.getOrigin()[0], (float)startTransform.getOrigin()[1], (float)startTransform.getOrigin()[2], (float)startTransform.getOrigin()[3]}; + float orn[4] = {(float)startTransform.getRotation()[0], (float)startTransform.getRotation()[1], (float)startTransform.getRotation()[2], (float)startTransform.getRotation()[3]}; + int graphicsInstanceId = registerGraphicsInstance(graphicsShapeId, pos, orn, colorRGBA, localScaling); body->setUserIndex(graphicsInstanceId); } } @@ -285,46 +272,42 @@ void RemoteGUIHelper::syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWor void RemoteGUIHelper::syncPhysicsToGraphics2(const btDiscreteDynamicsWorld* rbWorld) { - b3AlignedObjectArray updatedPositions; + b3AlignedObjectArray updatedPositions; - int numCollisionObjects = rbWorld->getNumCollisionObjects(); - { - B3_PROFILE("write all InstanceTransformToCPU2"); - for (int i = 0; i < numCollisionObjects; i++) - { - //B3_PROFILE("writeSingleInstanceTransformToCPU"); - btCollisionObject* colObj = rbWorld->getCollisionObjectArray()[i]; - btCollisionShape* collisionShape = colObj->getCollisionShape(); - - btVector3 pos = colObj->getWorldTransform().getOrigin(); - btQuaternion orn = colObj->getWorldTransform().getRotation(); - int index = colObj->getUserIndex(); - if (index >= 0) - { - GUISyncPosition p; - p.m_graphicsInstanceId = index; - for (int q = 0; q < 4; q++) - { - p.m_pos[q] = pos[q]; - p.m_orn[q] = orn[q]; - } - updatedPositions.push_back(p); - } - } - } - - if (updatedPositions.size()) - { - syncPhysicsToGraphics2(&updatedPositions[0], updatedPositions.size()); - } -} + int numCollisionObjects = rbWorld->getNumCollisionObjects(); + { + B3_PROFILE("write all InstanceTransformToCPU2"); + for (int i = 0; i < numCollisionObjects; i++) + { + //B3_PROFILE("writeSingleInstanceTransformToCPU"); + btCollisionObject* colObj = rbWorld->getCollisionObjectArray()[i]; + btCollisionShape* collisionShape = colObj->getCollisionShape(); + btVector3 pos = colObj->getWorldTransform().getOrigin(); + btQuaternion orn = colObj->getWorldTransform().getRotation(); + int index = colObj->getUserIndex(); + if (index >= 0) + { + GUISyncPosition p; + p.m_graphicsInstanceId = index; + for (int q = 0; q < 4; q++) + { + p.m_pos[q] = pos[q]; + p.m_orn[q] = orn[q]; + } + updatedPositions.push_back(p); + } + } + } + if (updatedPositions.size()) + { + syncPhysicsToGraphics2(&updatedPositions[0], updatedPositions.size()); + } +} void RemoteGUIHelper::syncPhysicsToGraphics2(const GUISyncPosition* positions, int numPositions) { - - GraphicsSharedMemoryCommand* cmd = m_data->getAvailableSharedMemoryCommand(); if (cmd) { @@ -338,8 +321,6 @@ void RemoteGUIHelper::syncPhysicsToGraphics2(const GUISyncPosition* positions, i while ((status = m_data->processServerStatus()) == 0) { } - - } void RemoteGUIHelper::render(const btDiscreteDynamicsWorld* rbWorld) @@ -364,9 +345,9 @@ int RemoteGUIHelper::uploadData(const unsigned char* data, int sizeInBytes, int { for (int i = 0; i < curBytes; i++) { - m_data->m_testBlock1->m_bulletStreamData[i] = data[i+offset]; + m_data->m_testBlock1->m_bulletStreamData[i] = data[i + offset]; } - + cmd->m_updateFlags = 0; cmd->m_type = GFX_CMD_UPLOAD_DATA; cmd->m_uploadDataCommand.m_numBytes = curBytes; @@ -386,16 +367,15 @@ int RemoteGUIHelper::uploadData(const unsigned char* data, int sizeInBytes, int } int RemoteGUIHelper::registerTexture(const unsigned char* texels, int width, int height) -{ +{ int textureId = -1; //first upload all data - - + GraphicsSharedMemoryCommand* cmd = m_data->getAvailableSharedMemoryCommand(); if (cmd) { - int sizeInBytes = width*height * 3;//rgb + int sizeInBytes = width * height * 3; //rgb uploadData(texels, sizeInBytes, 0); cmd->m_updateFlags = 0; cmd->m_type = GFX_CMD_REGISTER_TEXTURE; @@ -411,16 +391,14 @@ int RemoteGUIHelper::registerTexture(const unsigned char* texels, int width, int textureId = status->m_registerTextureStatus.m_textureId; } } - - return textureId; + return textureId; } -int RemoteGUIHelper::registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices, int primitiveType, int textureId) -{ +int RemoteGUIHelper::registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices, int primitiveType, int textureId) +{ int shapeId = -1; - GraphicsSharedMemoryCommand* cmd = m_data->getAvailableSharedMemoryCommand(); if (cmd) { @@ -444,11 +422,11 @@ int RemoteGUIHelper::registerGraphicsShape(const float* vertices, int numvertice } } - return shapeId; + return shapeId; } int RemoteGUIHelper::registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling) -{ +{ int graphicsInstanceId = -1; GraphicsSharedMemoryCommand* cmd = m_data->getAvailableSharedMemoryCommand(); @@ -473,9 +451,8 @@ int RemoteGUIHelper::registerGraphicsInstance(int shapeIndex, const float* posit { graphicsInstanceId = status->m_registerGraphicsInstanceStatus.m_graphicsInstanceId; } - } - return graphicsInstanceId; + return graphicsInstanceId; } void RemoteGUIHelper::removeAllGraphicsInstances() @@ -551,14 +528,14 @@ void RemoteGUIHelper::resetCamera(float camDist, float yaw, float pitch, float c } void RemoteGUIHelper::copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], - unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, - float* depthBuffer, int depthBufferSizeInPixels, - int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels, - int startPixelIndex, int width, int height, int* numPixelsCopied) + unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, + float* depthBuffer, int depthBufferSizeInPixels, + int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels, + int startPixelIndex, int width, int height, int* numPixelsCopied) { - if (numPixelsCopied) - *numPixelsCopied = 0; + if (numPixelsCopied) + *numPixelsCopied = 0; } void RemoteGUIHelper::setProjectiveTextureMatrices(const float viewMatrix[16], const float projectionMatrix[16]) diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index b0aa30f61..21aefba50 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -1054,15 +1054,18 @@ struct AddUserDataRequestArgs // Value data stored in m_bulletStreamDataServerToClientRefactor. }; -struct b3RequestMeshDataArgs{ - int m_bodyUniqueId; - int m_startingVertex; +struct b3RequestMeshDataArgs +{ + int m_bodyUniqueId; + int m_linkIndex; + int m_startingVertex; }; -struct b3SendMeshDataArgs{ - int m_numVerticesCopied; - int m_startingVertex; - int m_numVerticesRemaining; +struct b3SendMeshDataArgs +{ + int m_numVerticesCopied; + int m_startingVertex; + int m_numVerticesRemaining; }; struct SharedMemoryCommand @@ -1124,7 +1127,7 @@ struct SharedMemoryCommand struct AddUserDataRequestArgs m_addUserDataRequestArgs; struct UserDataRequestArgs m_removeUserDataRequestArgs; struct b3CollisionFilterArgs m_collisionFilterArgs; - struct b3RequestMeshDataArgs m_requestMeshDataArgs; + struct b3RequestMeshDataArgs m_requestMeshDataArgs; }; }; @@ -1200,7 +1203,7 @@ struct SharedMemoryStatus struct UserDataResponseArgs m_userDataResponseArgs; struct UserDataRequestArgs m_removeUserDataResponseArgs; struct b3ForwardDynamicsAnalyticsArgs m_forwardDynamicsAnalyticsArgs; - struct b3SendMeshDataArgs m_sendMeshDataArgs; + struct b3SendMeshDataArgs m_sendMeshDataArgs; }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 44287bc64..5e62ed172 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -101,9 +101,8 @@ enum EnumSharedMemoryClientCommand CMD_ADD_USER_DATA, CMD_REMOVE_USER_DATA, CMD_COLLISION_FILTER, + CMD_REQUEST_MESH_DATA, - CMD_REQUEST_MESH_DATA, - //don't go beyond this command! CMD_MAX_CLIENT_COMMANDS, }; @@ -224,8 +223,8 @@ enum EnumSharedMemoryServerStatus CMD_REMOVE_STATE_COMPLETED, CMD_REMOVE_STATE_FAILED, - CMD_REQUEST_MESH_DATA_COMPLETED, - CMD_REQUEST_MESH_DATA_FAILED, + CMD_REQUEST_MESH_DATA_COMPLETED, + CMD_REQUEST_MESH_DATA_FAILED, //don't go beyond 'CMD_MAX_SERVER_COMMANDS! CMD_MAX_SERVER_COMMANDS }; @@ -411,10 +410,15 @@ struct b3CameraImageData const int* m_segmentationMaskValues; //m_pixelWidth*m_pixelHeight ints }; +struct b3MeshVertex +{ + double x, y, z, w; +}; + struct b3MeshData { - int m_numVertices; - double* m_vertices; + int m_numVertices; + struct b3MeshVertex* m_vertices; }; struct b3OpenGLVisualizerCameraInfo diff --git a/examples/pybullet/examples/load_soft_body.py b/examples/pybullet/examples/load_soft_body.py index 27eaa52b5..401c25a92 100644 --- a/examples/pybullet/examples/load_soft_body.py +++ b/examples/pybullet/examples/load_soft_body.py @@ -1,11 +1,13 @@ import pybullet as p from time import sleep -physicsClient = p.connect(p.GUI) +physicsClient = p.connect(p.DIRECT) p.setGravity(0, 0, -10) planeId = p.loadURDF("plane.urdf") bunnyId = p.loadSoftBody("bunny.obj") +#meshData = p.getMeshData(bunnyId) +#print("meshData=",meshData) p.loadURDF("cube_small.urdf", [1, 0, 1]) useRealTimeSimulation = 1 diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 15855b387..5ee53bd5c 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -7358,46 +7358,57 @@ static PyObject* pybullet_createCollisionShapeArray(PyObject* self, PyObject* ar return NULL; } - static PyObject* pybullet_getMeshData(PyObject* self, PyObject* args, PyObject* keywds) { - { - int bodyUniqueId = -1; - b3PhysicsClientHandle sm = 0; - b3SharedMemoryCommandHandle command; - b3SharedMemoryStatusHandle statusHandle; - struct b3MeshData meshData; - int statusType; + int bodyUniqueId = -1; + int linkIndex = -1; + b3PhysicsClientHandle sm = 0; + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + struct b3MeshData meshData; + int statusType; - int physicsClientId = 0; - static char* kwlist[] = {"bodyUniqueId", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &bodyUniqueId, &physicsClientId)) - { - return NULL; - } - sm = getPhysicsClient(physicsClientId); - if (sm == 0) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - command = b3GetMeshDataCommandInit(sm, bodyUniqueId); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - statusType = b3GetStatusType(statusHandle); - if (statusType == CMD_REQUEST_MESH_DATA_COMPLETED) - { - b3GetMeshData(sm, &meshData); - PyObject* pyListMeshData = PyTuple_New(1); - PyTuple_SetItem(pyListMeshData, 0, PyInt_FromLong(meshData.m_numVertices)); - return pyListMeshData; - } - } - PyErr_SetString(SpamError, "Couldn't get body info"); - return NULL; + int physicsClientId = 0; + static char* kwlist[] = {"bodyUniqueId", "linkIndex", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|ii", kwlist, &bodyUniqueId, &linkIndex, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + command = b3GetMeshDataCommandInit(sm, bodyUniqueId, linkIndex); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_REQUEST_MESH_DATA_COMPLETED) + { + int i; + PyObject* pyVertexData; + PyObject* pyListMeshData = PyTuple_New(2); + b3GetMeshData(sm, &meshData); + PyTuple_SetItem(pyListMeshData, 0, PyInt_FromLong(meshData.m_numVertices)); + pyVertexData = PyTuple_New(meshData.m_numVertices); + PyTuple_SetItem(pyListMeshData, 1, pyVertexData); + + for (i = 0; i < meshData.m_numVertices; i++) + { + PyObject* pyListVertex = PyTuple_New(3); + PyTuple_SetItem(pyListVertex, 0, PyFloat_FromDouble(meshData.m_vertices[i].x)); + PyTuple_SetItem(pyListVertex, 1, PyFloat_FromDouble(meshData.m_vertices[i].y)); + PyTuple_SetItem(pyListVertex, 2, PyFloat_FromDouble(meshData.m_vertices[i].z)); + PyTuple_SetItem(pyVertexData, i, pyListVertex); + } + + return pyListMeshData; + } + + PyErr_SetString(SpamError, "getMeshData failed"); + return NULL; } - - static PyObject* pybullet_createVisualShape(PyObject* self, PyObject* args, PyObject* keywds) { int physicsClientId = 0; @@ -10485,7 +10496,7 @@ static PyMethodDef SpamMethods[] = { {"removeCollisionShape", (PyCFunction)pybullet_removeCollisionShape, METH_VARARGS | METH_KEYWORDS, "Remove a collision shape. Only useful when the collision shape is not used in a body (to perform a getClosestPoint query)."}, - {"getMeshData", (PyCFunction)pybullet_getMeshData, METH_VARARGS | METH_KEYWORDS, + {"getMeshData", (PyCFunction)pybullet_getMeshData, METH_VARARGS | METH_KEYWORDS, "Get mesh data. Returns vertices etc from the mesh."}, {"createVisualShape", (PyCFunction)pybullet_createVisualShape, METH_VARARGS | METH_KEYWORDS,