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,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){
+B3_SHARED_API void b3GetMeshData(b3PhysicsClientHandle physClient, struct b3MeshData* meshData)
-  PhysicsClient* cl = (PhysicsClient*)physClient;
+{
+	PhysicsClient* cl = (PhysicsClient*)physClient;
 	if (cl)
 	{
 		cl->getCachedMeshData(meshData);
 	}
 }
 
-
-
-
 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

@@ -53,8 +53,8 @@ struct PhysicsClientSharedMemoryInternalData
 	btAlignedObjectArray<b3VisualShapeData> m_cachedVisualShapes;
 	btAlignedObjectArray<b3CollisionShapeData> m_cachedCollisionShapes;
 
-  b3MeshData m_cachedMeshData;
+	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();
@@ -1035,15 +1037,24 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus()
 				b3Warning("Camera image FAILED\n");
 				break;
 			}
-      case CMD_REQUEST_MESH_DATA_COMPLETED:
+			case CMD_REQUEST_MESH_DATA_COMPLETED:
-      {
+			{
-        break;
+				m_data->m_cachedVertexPositions.resize(serverCmd.m_sendMeshDataArgs.m_startingVertex + serverCmd.m_sendMeshDataArgs.m_numVerticesCopied);
-      }
+				btVector3* verticesReceived = (btVector3*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
-      case CMD_REQUEST_MESH_DATA_FAILED:
+				for (int i = 0; i < serverCmd.m_sendMeshDataArgs.m_numVerticesCopied; i++)
-      {
+				{
-        b3Warning("Request mesh data failed");
+					m_data->m_cachedVertexPositions[i + serverCmd.m_sendMeshDataArgs.m_startingVertex].x = verticesReceived[i].x();
-        break;
+					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)
-void PhysicsClientSharedMemory::getCachedMeshData(struct b3MeshData* meshData){
+{
- *meshData = m_data->m_cachedMeshData;
+	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

examples/SharedMemory/PhysicsDirect.cpp

@@ -66,8 +66,8 @@ struct PhysicsDirectInternalData
 	btAlignedObjectArray<b3VisualShapeData> m_cachedVisualShapes;
 	btAlignedObjectArray<b3CollisionShapeData> m_cachedCollisionShapes;
 
-  b3MeshData m_cachedMeshData;
+	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,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;
+			const b3SendMeshDataArgs& args = serverCmd.m_sendMeshDataArgs;
-			int numTotalPixels =args.m_startingVertex +
+			int numTotalPixels = args.m_startingVertex +
-								args.m_numVerticesCopied + args.m_numVerticesRemaining;
+								 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 +
-      m_data->m_cachedVertexPositions.resize((args.m_startingVertex +
+													args.m_numVerticesCopied);
-								args.m_numVerticesCopied)*dimension);
 
-			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 +
+				m_data->m_cachedMeshData.m_numVertices = args.m_startingVertex +
-					args.m_numVerticesCopied;
+														 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){
+void PhysicsDirect::getCachedMeshData(struct b3MeshData* meshData)
- *meshData = m_data->m_cachedMeshData;
+{
+	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

@@ -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;
+		if (bodyHandle->m_multiBody)
-    serverStatusOut.m_sendMeshDataArgs.m_numVerticesCopied = 1;
+		{
-  	serverStatusOut.m_sendMeshDataArgs.m_startingVertex = 0;
+			//todo
-	  serverStatusOut.m_sendMeshDataArgs.m_numVerticesRemaining = 0;
+		}
+		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:{
+		case CMD_REQUEST_MESH_DATA:
-      hasStatus = processRequestMeshDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
+		{
+			hasStatus = processRequestMeshDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
 			break;
-    }
+		}
 		case CMD_CREATE_MULTI_BODY:
 		{
 			hasStatus = processCreateMultiBodyCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);

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:
+				case GFX_CMD_CLIENT_COMMAND_COMPLETED:
-			{
+				{
-				B3_PROFILE("CMD_CLIENT_COMMAND_COMPLETED");
+					B3_PROFILE("CMD_CLIENT_COMMAND_COMPLETED");
 
-
+					break;
-				break;
+				}
-			}
+				default:
-			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 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);
 		}
 	}
@@ -285,46 +272,42 @@ void RemoteGUIHelper::syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWor
 
 void RemoteGUIHelper::syncPhysicsToGraphics2(const btDiscreteDynamicsWorld* rbWorld)
 {
-                       b3AlignedObjectArray<GUISyncPosition> updatedPositions;
+	b3AlignedObjectArray<GUISyncPosition> updatedPositions;
 
-                        int numCollisionObjects = rbWorld->getNumCollisionObjects();
+	int numCollisionObjects = rbWorld->getNumCollisionObjects();
-                        {
+	{
-                                B3_PROFILE("write all InstanceTransformToCPU2");
+		B3_PROFILE("write all InstanceTransformToCPU2");
-                                for (int i = 0; i < numCollisionObjects; i++)
+		for (int i = 0; i < numCollisionObjects; i++)
-                                {
+		{
-                                        //B3_PROFILE("writeSingleInstanceTransformToCPU");
+			//B3_PROFILE("writeSingleInstanceTransformToCPU");
-                                        btCollisionObject* colObj = rbWorld->getCollisionObjectArray()[i];
+			btCollisionObject* colObj = rbWorld->getCollisionObjectArray()[i];
-                                        btCollisionShape* collisionShape = colObj->getCollisionShape();
+			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());
-                        }
-}                
 
+			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,
+										  unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels,
-									 float* depthBuffer, int depthBufferSizeInPixels,
+										  float* depthBuffer, int depthBufferSizeInPixels,
-									 int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels,
+										  int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels,
-									 int startPixelIndex, int width, int height, int* numPixelsCopied)
+										  int startPixelIndex, int width, int height, int* numPixelsCopied)
 
 {
-		if (numPixelsCopied)
+	if (numPixelsCopied)
-			*numPixelsCopied = 0;
+		*numPixelsCopied = 0;
 }
 
 void RemoteGUIHelper::setProjectiveTextureMatrices(const float viewMatrix[16], const float projectionMatrix[16])

examples/SharedMemory/SharedMemoryCommands.h

@@ -1054,15 +1054,18 @@ struct AddUserDataRequestArgs
 	// Value data stored in m_bulletStreamDataServerToClientRefactor.
 };
 
-struct b3RequestMeshDataArgs{
+struct b3RequestMeshDataArgs
-  int m_bodyUniqueId;
+{
-  int m_startingVertex;
+	int m_bodyUniqueId;
+	int m_linkIndex;
+	int m_startingVertex;
 };
 
-struct b3SendMeshDataArgs{
+struct b3SendMeshDataArgs
-  int m_numVerticesCopied;
+{
-  int m_startingVertex;
+	int m_numVerticesCopied;
-  int m_numVerticesRemaining;
+	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;
 	};
 };
 

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_COMPLETED,
-  CMD_REQUEST_MESH_DATA_FAILED,
+	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;
+	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,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;
-    int bodyUniqueId = -1;
+	int linkIndex = -1;
-    b3PhysicsClientHandle sm = 0;
+	b3PhysicsClientHandle sm = 0;
-    b3SharedMemoryCommandHandle command;
+	b3SharedMemoryCommandHandle command;
-    b3SharedMemoryStatusHandle statusHandle;
+	b3SharedMemoryStatusHandle statusHandle;
-    struct b3MeshData meshData;
+	struct b3MeshData meshData;
-    int statusType;
+	int statusType;
 
-    int physicsClientId = 0;
+	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;
+		return NULL;
-    }
+	}
-    sm = getPhysicsClient(physicsClientId);
+	sm = getPhysicsClient(physicsClientId);
-    if (sm == 0)
+	if (sm == 0)
-    {
+	{
-      PyErr_SetString(SpamError, "Not connected to physics server.");
+		PyErr_SetString(SpamError, "Not connected to physics server.");
-      return NULL;
+		return NULL;
-    }
+	}
-    command = b3GetMeshDataCommandInit(sm, bodyUniqueId);
+	command = b3GetMeshDataCommandInit(sm, bodyUniqueId, linkIndex);
-    statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
-    statusType = b3GetStatusType(statusHandle);
+	statusType = b3GetStatusType(statusHandle);
-    if (statusType == CMD_REQUEST_MESH_DATA_COMPLETED)
+	if (statusType == CMD_REQUEST_MESH_DATA_COMPLETED)
-    {
+	{
-      b3GetMeshData(sm, &meshData);
+		int i;
-      PyObject* pyListMeshData = PyTuple_New(1);
+		PyObject* pyVertexData;
-      PyTuple_SetItem(pyListMeshData, 0, PyInt_FromLong(meshData.m_numVertices));
+		PyObject* pyListMeshData = PyTuple_New(2);
-      return pyListMeshData;
+		b3GetMeshData(sm, &meshData);
-    }
+		PyTuple_SetItem(pyListMeshData, 0, PyInt_FromLong(meshData.m_numVertices));
-  }
+		pyVertexData = PyTuple_New(meshData.m_numVertices);
-  PyErr_SetString(SpamError, "Couldn't get body info");
+		PyTuple_SetItem(pyListMeshData, 1, pyVertexData);
-  return NULL;
+		
+		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,