Update PhysicsServerCommandProcessor and plugins to support render

examples/Importers/ImportURDFDemo/UrdfRenderingInterface.h

@@ -14,8 +14,13 @@ struct UrdfRenderingInterface
 {
 	virtual ~UrdfRenderingInterface() {}
 	///given a URDF link, convert all visual shapes into internal renderer (loading graphics meshes, textures etc)
-	///use the collisionObjectUid as a unique identifier to synchronize the world transform and to remove the visual shape.
+	///use the visualShapeUniqueId as a unique identifier to synchronize the world transform and to remove the visual shape.
-	virtual int convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int collisionObjectUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO) = 0;
+	virtual int convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int visualShapeUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO) = 0;
+
+	///Given b3VisualShapeData, add render information (texture, rgbaColor etc) to the visualShape
+	///and visualShape to internal renderer.
+	///Returns a visualShapeUniqueId as a unique identifier to synchronize the world transform and to remove the visual shape.
+	virtual int addVisualShape(struct b3VisualShapeData* visualShape, struct CommonFileIOInterface* fileIO) = 0;
 
 	///remove a visual shapes, based on the shape unique id (shapeUid)
 	virtual void removeVisualShape(int collisionObjectUid) = 0;

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -4660,13 +4660,12 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S
 				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;
@@ -7450,11 +7449,35 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
 				int bodyUniqueId = m_data->m_bodyHandles.allocHandle();
 				InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId);
 				bodyHandle->m_softBody = psb;
+
+				b3VisualShapeData visualShape;
+
+				visualShape.m_objectUniqueId = bodyUniqueId;
+				visualShape.m_linkIndex = -1;
+				visualShape.m_visualGeometryType = URDF_GEOM_MESH;
+				//dimensions just contains the scale
+				visualShape.m_dimensions[0] = scale;
+				visualShape.m_dimensions[1] = scale;
+				visualShape.m_dimensions[2] = scale;
+				//filename
+				strncpy(visualShape.m_meshAssetFileName, relativeFileName, VISUAL_SHAPE_MAX_PATH_LEN);
+				visualShape.m_meshAssetFileName[VISUAL_SHAPE_MAX_PATH_LEN - 1] = 0;
+				//position and orientation
+				visualShape.m_localVisualFrame[0] = initialPos[0];
+				visualShape.m_localVisualFrame[1] = initialPos[1];
+				visualShape.m_localVisualFrame[2] = initialPos[2];
+				visualShape.m_localVisualFrame[3] = initialOrn[0];
+				visualShape.m_localVisualFrame[4] = initialOrn[1];
+				visualShape.m_localVisualFrame[5] = initialOrn[2];
+				visualShape.m_localVisualFrame[6] = initialOrn[3];
+
+				m_data->m_pluginManager.getRenderInterface()->addVisualShape(&visualShape, fileIO);
+
 				serverStatusOut.m_loadSoftBodyResultArguments.m_objectUniqueId = bodyUniqueId;
 				serverStatusOut.m_type = CMD_LOAD_SOFT_BODY_COMPLETED;
 
-                                int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
+				int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
-                                serverStatusOut.m_numDataStreamBytes = streamSizeInBytes;
+				serverStatusOut.m_numDataStreamBytes = streamSizeInBytes;
 
 #ifdef ENABLE_LINK_MAPPER
                                 if (m_data->m_urdfLinkNameMapper.size())
@@ -7741,7 +7764,7 @@ bool PhysicsServerCommandProcessor::processForwardDynamicsCommand(const struct S
 
 	if (numSteps > 0)
 	{
-		addTransformChangedNotifications();
+		addBodyChangedNotifications();
 	}
 
 	SharedMemoryStatus& serverCmd = serverStatusOut;
@@ -9088,7 +9111,7 @@ bool PhysicsServerCommandProcessor::processConfigureOpenGLVisualizerCommand(cons
 		m_data->m_remoteSyncTransformInterval = clientCmd.m_configureOpenGLVisualizerArguments.m_remoteSyncTransformInterval;
 	}
 
-	
+
 	return hasStatus;
 }
 
@@ -9624,6 +9647,23 @@ bool PhysicsServerCommandProcessor::processRemoveBodyCommand(const struct Shared
 				bodyHandle->m_rigidBody = 0;
 				serverCmd.m_type = CMD_REMOVE_BODY_COMPLETED;
 			}
+#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
+			if (bodyHandle->m_softBody)
+			{
+				btSoftBody* psb = bodyHandle->m_softBody;
+				if (m_data->m_pluginManager.getRenderInterface())
+				{
+					m_data->m_pluginManager.getRenderInterface()->removeVisualShape(psb->getBroadphaseHandle()->getUid());
+				}
+				serverCmd.m_removeObjectArgs.m_bodyUniqueIds[serverCmd.m_removeObjectArgs.m_numBodies++] = bodyUniqueId;
+				m_data->m_dynamicsWorld->removeSoftBody(psb);
+				int graphicsInstance = psb->getUserIndex2();
+				m_data->m_guiHelper->removeGraphicsInstance(graphicsInstance);
+				delete psb;
+				psb = 0;
+				serverCmd.m_type = CMD_REMOVE_BODY_COMPLETED;
+			}
+#endif
 			for (int i = 0; i < bodyHandle->m_userDataHandles.size(); i++)
 			{
 				int userDataHandle = bodyHandle->m_userDataHandles[i];
@@ -11410,7 +11450,6 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 			hasStatus = processRequestCollisionInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
 			break;
 		}
-
 		case CMD_REQUEST_ACTUAL_STATE:
 		{
 			hasStatus = processRequestActualStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
@@ -11992,7 +12031,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec, const
 			gNumSteps = numSteps;
 			gDtInSec = dtInSec;
 
-			addTransformChangedNotifications();
+			addBodyChangedNotifications();
 		}
 	}
 }
@@ -12021,7 +12060,18 @@ b3Notification createTransformChangedNotification(int bodyUniqueId, int linkInde
 	return notification;
 }
 
-void PhysicsServerCommandProcessor::addTransformChangedNotifications()
+#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
+b3Notification createSoftBodyChangedNotification(int bodyUniqueId, int linkIndex, const btSoftBody* psb)
+{
+	b3Notification notification;
+	notification.m_notificationType = SOFTBODY_CHANGED;
+	notification.m_softBodyChangeArgs.m_bodyUniqueId = bodyUniqueId;
+	notification.m_softBodyChangeArgs.m_linkIndex = linkIndex;
+	return notification;
+}
+#endif
+
+void PhysicsServerCommandProcessor::addBodyChangedNotifications()
 {
 	b3Notification notification;
 	notification.m_notificationType = SIMULATION_STEPPED;
@@ -12056,6 +12106,14 @@ void PhysicsServerCommandProcessor::addTransformChangedNotifications()
 		{
 			m_data->m_pluginManager.addNotification(createTransformChangedNotification(bodyUniqueId, -1, bodyData->m_rigidBody));
 		}
+#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
+		else if (bodyData->m_softBody)
+		{
+			btSoftBody* psb = bodyData->m_softBody;
+			int linkIndex = -1;
+			m_data->m_pluginManager.addNotification(createSoftBodyChangedNotification(bodyUniqueId, linkIndex, psb));
+		}
+#endif
 	}
 }
 
@@ -12063,7 +12121,7 @@ void PhysicsServerCommandProcessor::resetSimulation()
 {
 	//clean up all data
 	m_data->m_remoteSyncTransformTime = m_data->m_remoteSyncTransformInterval;
-	
+
 	m_data->m_simulationTimestamp = 0;
         m_data->m_cachedVUrdfisualShapes.clear();
 

examples/SharedMemory/PhysicsServerCommandProcessor.h

@@ -175,7 +175,7 @@ public:
 	virtual void setVRTeleportOrientation(const btQuaternion& vrTeleportOrn);
 
 private:
-	void addTransformChangedNotifications();
+	void addBodyChangedNotifications();
 };
 
 #endif  //PHYSICS_SERVER_COMMAND_PROCESSOR_H

examples/SharedMemory/SharedMemoryPublic.h

@@ -7,7 +7,8 @@
 //Please don't replace an existing magic number:
 //instead, only ADD a new one at the top, comment-out previous one
 
-#define SHARED_MEMORY_MAGIC_NUMBER 201904030
+#define SHARED_MEMORY_MAGIC_NUMBER 2019060190
+// #define SHARED_MEMORY_MAGIC_NUMBER 201904030
 //#define SHARED_MEMORY_MAGIC_NUMBER 201902120
 //#define SHARED_MEMORY_MAGIC_NUMBER 201811260
 //#define SHARED_MEMORY_MAGIC_NUMBER 201810250
@@ -548,6 +549,7 @@ enum b3NotificationType
 	VISUAL_SHAPE_CHANGED = 6,
 	TRANSFORM_CHANGED = 7,
 	SIMULATION_STEPPED = 8,
+	SOFTBODY_CHANGED = 9,
 };
 
 struct b3BodyNotificationArgs
@@ -586,6 +588,12 @@ struct b3TransformChangeNotificationArgs
 	double m_localScaling[3];
 };
 
+struct b3SoftBodyChangeNotificationArgs
+{
+	int m_bodyUniqueId;
+	int m_linkIndex;
+};
+
 struct b3Notification
 {
 	int m_notificationType;
@@ -595,6 +603,7 @@ struct b3Notification
 		struct b3LinkNotificationArgs m_linkArgs;
 		struct b3VisualShapeNotificationArgs m_visualShapeArgs;
 		struct b3TransformChangeNotificationArgs m_transformChangeArgs;
+		struct b3SoftBodyChangeNotificationArgs m_softBodyChangeArgs;
 	};
 };
 

examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.h

@@ -17,6 +17,8 @@ struct EGLRendererVisualShapeConverter : public UrdfRenderingInterface
 
 	virtual int getVisualShapesData(int bodyUniqueId, int shapeIndex, struct b3VisualShapeData* shapeData);
 
+	virtual int addVisualShape(b3VisualShapeData* visualShape, struct CommonFileIOInterface* fileIO) { return -1; }
+
 	virtual void changeRGBAColor(int bodyUniqueId, int linkIndex, int shapeIndex, const double rgbaColor[4]);
 
 	virtual void changeShapeTexture(int objectUniqueId, int linkIndex, int shapeIndex, int textureUniqueId);

examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp

@@ -828,6 +828,64 @@ int  TinyRendererVisualShapeConverter::convertVisualShapes(
 	return uniqueId;
 }
 
+int TinyRendererVisualShapeConverter::addVisualShape(
+	b3VisualShapeData* visualShape, struct CommonFileIOInterface* fileIO)
+{
+	int uniqueId = m_data->m_uidGenerator++;
+	visualShape->m_openglTextureId = -1;
+	visualShape->m_tinyRendererTextureId = -1;
+	visualShape->m_textureUniqueId = -1;
+	b3ImportMeshData meshData;
+	if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(
+			visualShape->m_meshAssetFileName, meshData, fileIO))
+	{
+		if (m_data->m_flags & URDF_USE_MATERIAL_COLORS_FROM_MTL)
+		{
+			if (meshData.m_flags & B3_IMPORT_MESH_HAS_RGBA_COLOR)
+			{
+				visualShape->m_rgbaColor[0] = meshData.m_rgbaColor[0];
+				visualShape->m_rgbaColor[1] = meshData.m_rgbaColor[1];
+				visualShape->m_rgbaColor[2] = meshData.m_rgbaColor[2];
+
+				if (m_data->m_flags & URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL)
+				{
+					visualShape->m_rgbaColor[3] = meshData.m_rgbaColor[3];
+				}
+				else
+				{
+					visualShape->m_rgbaColor[3] = 1;
+				}
+			}
+		}
+
+		MyTexture2 texture;
+		if (meshData.m_textureImage1)
+		{
+			texture.m_width = meshData.m_textureWidth;
+			texture.m_height = meshData.m_textureHeight;
+			texture.textureData1 = meshData.m_textureImage1;
+			texture.m_isCached = meshData.m_isCached;
+
+			visualShape->m_tinyRendererTextureId = m_data->m_textures.size();
+			m_data->m_textures.push_back(texture);
+		}
+		// meshData.m_gfxShape is allocated by a helper function used to create visualShape,
+		// but is not needed in this use case here
+		delete meshData.m_gfxShape;
+	}
+
+	btAlignedObjectArray<b3VisualShapeData>* shapes =
+		m_data->m_visualShapesMap[visualShape->m_objectUniqueId];
+	if (!shapes)
+	{
+		m_data->m_visualShapesMap.insert(visualShape->m_objectUniqueId,
+										 btAlignedObjectArray<b3VisualShapeData>());
+		shapes = m_data->m_visualShapesMap[visualShape->m_objectUniqueId];
+	}
+	shapes->push_back(*visualShape);
+	return uniqueId;
+}
+
 int TinyRendererVisualShapeConverter::getNumVisualShapes(int bodyUniqueId)
 {
 	btAlignedObjectArray<b3VisualShapeData>* shapes = m_data->m_visualShapesMap[bodyUniqueId];

examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.h

@@ -13,6 +13,8 @@ struct TinyRendererVisualShapeConverter : public UrdfRenderingInterface
 
 	virtual int convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int unused, int bodyUniqueId, struct CommonFileIOInterface* fileIO);
 
+	virtual int addVisualShape(struct b3VisualShapeData* visualShape, struct CommonFileIOInterface* fileIO);
+
 	virtual int getNumVisualShapes(int bodyUniqueId);
 
 	virtual int getVisualShapesData(int bodyUniqueId, int shapeIndex, struct b3VisualShapeData* shapeData);