Merge pull request #2294 from erwincoumans/master

make pybullet.getMeshData work for softbody

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,13 +1221,16 @@ 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){
+B3_SHARED_API void b3GetMeshData(b3PhysicsClientHandle physClient, struct b3MeshData* meshData)
+{
 	PhysicsClient* cl = (PhysicsClient*)physClient;
 	if (cl)
 	{
@@ -1234,9 +1238,6 @@ B3_SHARED_API void b3GetMeshData(b3PhysicsClientHandle physClient, struct b3Mesh
 	}
 }
 
-
-
-
 B3_SHARED_API int b3CreateVisualShapeAddSphere(b3SharedMemoryCommandHandle commandHandle, double radius)
 {
 	return b3CreateCollisionShapeAddSphere(commandHandle, radius);

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);
 
 

examples/SharedMemory/PhysicsClientSharedMemory.cpp

@@ -54,7 +54,7 @@ struct PhysicsClientSharedMemoryInternalData
 	btAlignedObjectArray<b3CollisionShapeData> m_cachedCollisionShapes;
 
 	b3MeshData m_cachedMeshData;
-  btAlignedObjectArray<double> m_cachedVertexPositions;
+	btAlignedObjectArray<b3MeshVertex> m_cachedVertexPositions;
 
 	btAlignedObjectArray<b3VRControllerEvent> m_cachedVREvents;
 	btAlignedObjectArray<b3KeyboardEvent> 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();
@@ -1037,6 +1039,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus()
 			}
 			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:
@@ -1770,7 +1781,7 @@ 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];
@@ -1778,7 +1789,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus()
 			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,7 +1797,7 @@ 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;
 			}
 		}
 
@@ -1972,8 +1983,12 @@ void PhysicsClientSharedMemory::getCachedCollisionShapeInformation(struct b3Coll
 	collisionShapesInfo->m_collisionShapeData = collisionShapesInfo->m_numCollisionShapes ? &m_data->m_cachedCollisionShapes[0] : 0;
 }
 
+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;
 	
-void PhysicsClientSharedMemory::getCachedMeshData(struct b3MeshData* meshData){
 	*meshData = m_data->m_cachedMeshData;
 }
 

examples/SharedMemory/PhysicsDirect.cpp

@@ -67,7 +67,7 @@ struct PhysicsDirectInternalData
 	btAlignedObjectArray<b3CollisionShapeData> m_cachedCollisionShapes;
 
 	b3MeshData m_cachedMeshData;
-  btAlignedObjectArray<double> m_cachedVertexPositions;
+	btAlignedObjectArray<b3MeshVertex> m_cachedVertexPositions;
 
 	btAlignedObjectArray<b3VRControllerEvent> 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,7 +590,6 @@ bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand)
 	return m_data->m_hasStatus;
 }
 
-
 bool PhysicsDirect::processMeshData(const struct SharedMemoryCommand& orgCommand)
 {
 	SharedMemoryCommand command = orgCommand;
@@ -623,20 +623,22 @@ bool PhysicsDirect::processMeshData(const struct SharedMemoryCommand& orgCommand
 			}
 
 			const b3SendMeshDataArgs& args = serverCmd.m_sendMeshDataArgs;
-			int numTotalPixels =args.m_startingVertex +
+			int numTotalPixels = args.m_startingVertex +
 								 args.m_numVerticesCopied + args.m_numVerticesRemaining;
 
-			 double* verticesReceived =
+			btVector3* verticesReceived =
-				(double*)&m_data->m_bulletStreamDataServerToClient[0];
+				(btVector3*)&m_data->m_bulletStreamDataServerToClient[0];
 
-      // flattened position
-      const int dimension = 3;
-      m_data->m_cachedVertexPositions.resize((args.m_startingVertex +
-								args.m_numVerticesCopied)*dimension);
 			
-			for (int i = 0; i < dimension*args.m_numVerticesCopied; i++)
+			m_data->m_cachedVertexPositions.resize(args.m_startingVertex +
+													args.m_numVerticesCopied);
+
+			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)
@@ -650,7 +652,7 @@ bool PhysicsDirect::processMeshData(const struct SharedMemoryCommand& orgCommand
 			}
 			else
 			{
-        m_data->m_cachedMeshData.m_numVertices=args.m_startingVertex +
+				m_data->m_cachedMeshData.m_numVertices = args.m_startingVertex +
 														 args.m_numVerticesCopied;
 			}
 		}
@@ -1526,9 +1528,15 @@ void PhysicsDirect::getCachedCameraImage(b3CameraImageData* cameraData)
 	}
 }
 
-void PhysicsDirect::getCachedMeshData(struct b3MeshData* meshData){
+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();

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -4636,10 +4636,35 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S
 	InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_requestMeshDataArgs.m_bodyUniqueId);
 	if (bodyHandle)
 	{
+		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 = 1;
+			serverStatusOut.m_sendMeshDataArgs.m_numVerticesCopied = verticesCopied;
-  	serverStatusOut.m_sendMeshDataArgs.m_startingVertex = 0;
+			serverStatusOut.m_sendMeshDataArgs.m_startingVertex = clientCmd.m_requestMeshDataArgs.m_startingVertex;
-	  serverStatusOut.m_sendMeshDataArgs.m_numVerticesRemaining = 0;
+			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,7 +11333,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 			hasStatus = processCreateVisualShapeCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
 			break;
 		}
-    case CMD_REQUEST_MESH_DATA:{
+		case CMD_REQUEST_MESH_DATA:
+		{
 			hasStatus = processRequestMeshDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
 			break;
 		}

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;
@@ -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,7 +118,6 @@ struct RemoteGUIHelperInternalData
 			return &m_lastServerStatus;
 		}
 
-
 		if (m_testBlock1->m_numServerCommands >
 			m_testBlock1->m_numProcessedServerCommands)
 		{
@@ -140,7 +137,6 @@ struct RemoteGUIHelperInternalData
 				{
 					B3_PROFILE("CMD_CLIENT_COMMAND_COMPLETED");
 
-
 					break;
 				}
 				default:
@@ -163,7 +159,6 @@ struct RemoteGUIHelperInternalData
 				m_waitingForServer = true;
 			}
 
-
 			return &m_lastServerStatus;
 		}
 		return 0;
@@ -199,7 +194,6 @@ struct RemoteGUIHelperInternalData
 		return true;
 	}
 
-
 	void disconnect()
 	{
 		if (m_isConnected && m_sharedMemory)
@@ -208,15 +202,8 @@ struct RemoteGUIHelperInternalData
 		}
 		m_isConnected = false;
 	}
-
 };
 
-
-
-
-
-
-
 RemoteGUIHelper::RemoteGUIHelper()
 {
 	m_data = new RemoteGUIHelperInternalData;
@@ -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 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 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 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] };
+			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);
+			int graphicsInstanceId = registerGraphicsInstance(graphicsShapeId, pos, orn, colorRGBA, localScaling);
 			body->setUserIndex(graphicsInstanceId);
 		}
 	}
@@ -319,15 +306,12 @@ void RemoteGUIHelper::syncPhysicsToGraphics2(const btDiscreteDynamicsWorld* rbWo
 	}
 }
 
-
-
 void RemoteGUIHelper::syncPhysicsToGraphics2(const GUISyncPosition* positions, int numPositions)
 {
-	uploadData((unsigned char*) positions, numPositions * sizeof(GUISyncPosition), 0);
-	
 	GraphicsSharedMemoryCommand* cmd = m_data->getAvailableSharedMemoryCommand();
 	if (cmd)
 	{
+		uploadData((unsigned char*)positions, numPositions * sizeof(GUISyncPosition), 0);
 		cmd->m_updateFlags = 0;
 		cmd->m_syncTransformsCommand.m_numPositions = numPositions;
 		cmd->m_type = GFX_CMD_SYNCHRONIZE_TRANSFORMS;
@@ -337,8 +321,6 @@ void RemoteGUIHelper::syncPhysicsToGraphics2(const GUISyncPosition* positions, i
 	while ((status = m_data->processServerStatus()) == 0)
 	{
 	}
-
-
 }
 
 void RemoteGUIHelper::render(const btDiscreteDynamicsWorld* rbWorld)
@@ -363,7 +345,7 @@ 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;
@@ -389,11 +371,12 @@ int RemoteGUIHelper::registerTexture(const unsigned char* texels, int width, int
 	int textureId = -1;
 
 	//first upload all data
-	int sizeInBytes = width*height * 3;//rgb
+
-	uploadData(texels, sizeInBytes, 0);
 	GraphicsSharedMemoryCommand* cmd = m_data->getAvailableSharedMemoryCommand();
 	if (cmd)
 	{
+		int sizeInBytes = width * height * 3;  //rgb
+		uploadData(texels, sizeInBytes, 0);
 		cmd->m_updateFlags = 0;
 		cmd->m_type = GFX_CMD_REGISTER_TEXTURE;
 		cmd->m_registerTextureCommand.m_width = width;
@@ -409,7 +392,6 @@ int RemoteGUIHelper::registerTexture(const unsigned char* texels, int width, int
 		}
 	}
 
-
 	return textureId;
 }
 
@@ -417,11 +399,11 @@ int RemoteGUIHelper::registerGraphicsShape(const float* vertices, int numvertice
 {
 	int shapeId = -1;
 
-	uploadData((unsigned char*)vertices, numvertices * 9*sizeof(float), 0);
-	uploadData((unsigned char*)indices, numIndices * sizeof(int), 1);
 	GraphicsSharedMemoryCommand* cmd = m_data->getAvailableSharedMemoryCommand();
 	if (cmd)
 	{
+		uploadData((unsigned char*)vertices, numvertices * 9 * sizeof(float), 0);
+		uploadData((unsigned char*)indices, numIndices * sizeof(int), 1);
 		cmd->m_type = GFX_CMD_REGISTER_GRAPHICS_SHAPE;
 		cmd->m_updateFlags = 0;
 		cmd->m_registerGraphicsShapeCommand.m_numVertices = numvertices;
@@ -469,7 +451,6 @@ int RemoteGUIHelper::registerGraphicsInstance(int shapeIndex, const float* posit
 		{
 			graphicsInstanceId = status->m_registerGraphicsInstanceStatus.m_graphicsInstanceId;
 		}
-
 	}
 	return graphicsInstanceId;
 }

examples/SharedMemory/SharedMemoryCommands.h

@@ -1054,12 +1054,15 @@ struct AddUserDataRequestArgs
 	// Value data stored in m_bulletStreamDataServerToClientRefactor.
 };
 
-struct b3RequestMeshDataArgs{
+struct b3RequestMeshDataArgs
+{
 	int m_bodyUniqueId;
+	int m_linkIndex;
 	int m_startingVertex;
 };
 
-struct b3SendMeshDataArgs{
+struct b3SendMeshDataArgs
+{
 	int m_numVerticesCopied;
 	int m_startingVertex;
 	int m_numVerticesRemaining;

examples/SharedMemory/SharedMemoryPublic.h

@@ -101,7 +101,6 @@ enum EnumSharedMemoryClientCommand
 	CMD_ADD_USER_DATA,
 	CMD_REMOVE_USER_DATA,
 	CMD_COLLISION_FILTER,
-
 	CMD_REQUEST_MESH_DATA,
 
 	//don't go beyond this command!
@@ -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;
+	struct b3MeshVertex* m_vertices;
 };
 
 struct b3OpenGLVisualizerCameraInfo

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
 

examples/pybullet/pybullet.c

@@ -7358,11 +7358,10 @@ static PyObject* pybullet_createCollisionShapeArray(PyObject* self, PyObject* ar
 	return NULL;
 }
 
-
 static PyObject* pybullet_getMeshData(PyObject* self, PyObject* args, PyObject* keywds)
 {
-  {
 	int bodyUniqueId = -1;
+	int linkIndex = -1;
 	b3PhysicsClientHandle sm = 0;
 	b3SharedMemoryCommandHandle command;
 	b3SharedMemoryStatusHandle statusHandle;
@@ -7370,8 +7369,8 @@ static PyObject* pybullet_getMeshData(PyObject* self, PyObject* args, PyObject*
 	int statusType;
 
 	int physicsClientId = 0;
-    static char* kwlist[] = {"bodyUniqueId", "physicsClientId", NULL};
+	static char* kwlist[] = {"bodyUniqueId", "linkIndex", "physicsClientId", NULL};
-    if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &bodyUniqueId, &physicsClientId))
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|ii", kwlist, &bodyUniqueId, &linkIndex, &physicsClientId))
 	{
 		return NULL;
 	}
@@ -7381,23 +7380,35 @@ static PyObject* pybullet_getMeshData(PyObject* self, PyObject* args, PyObject*
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-    command = b3GetMeshDataCommandInit(sm, bodyUniqueId);
+	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);
-      PyObject* pyListMeshData = PyTuple_New(1);
 		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, "Couldn't get body info");
+	PyErr_SetString(SpamError, "getMeshData failed");
 	return NULL;
 }
 
-
-
 static PyObject* pybullet_createVisualShape(PyObject* self, PyObject* args, PyObject* keywds)
 {
 	int physicsClientId = 0;