From 4da456054c51844c5c3174774856e24762d538b9 Mon Sep 17 00:00:00 2001 From: Chuyuan Fu Date: Tue, 25 Jun 2019 17:59:15 -0700 Subject: [PATCH] Update PhysicsServerCommandProcessor and plugins to support render --- .../ImportURDFDemo/UrdfRenderingInterface.h | 9 ++- .../PhysicsServerCommandProcessor.cpp | 80 ++++++++++++++++--- .../PhysicsServerCommandProcessor.h | 2 +- examples/SharedMemory/SharedMemoryPublic.h | 11 ++- .../eglRendererVisualShapeConverter.h | 2 + .../TinyRendererVisualShapeConverter.cpp | 58 ++++++++++++++ .../TinyRendererVisualShapeConverter.h | 2 + 7 files changed, 149 insertions(+), 15 deletions(-) diff --git a/examples/Importers/ImportURDFDemo/UrdfRenderingInterface.h b/examples/Importers/ImportURDFDemo/UrdfRenderingInterface.h index 02aa42534..ded30157e 100644 --- a/examples/Importers/ImportURDFDemo/UrdfRenderingInterface.h +++ b/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. - 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; + ///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 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; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 33c47e493..084797508 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/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); - serverStatusOut.m_numDataStreamBytes = streamSizeInBytes; + int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes); + 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(); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index 49f1d9f69..fc8eab47b 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/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 diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 5e62ed172..40e54b1d6 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/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; }; }; diff --git a/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.h b/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.h index b0cfc36a7..1d1e83322 100644 --- a/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.h +++ b/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); diff --git a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp index 359a251eb..5c3238683 100644 --- a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp +++ b/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* shapes = + m_data->m_visualShapesMap[visualShape->m_objectUniqueId]; + if (!shapes) + { + m_data->m_visualShapesMap.insert(visualShape->m_objectUniqueId, + btAlignedObjectArray()); + shapes = m_data->m_visualShapesMap[visualShape->m_objectUniqueId]; + } + shapes->push_back(*visualShape); + return uniqueId; +} + int TinyRendererVisualShapeConverter::getNumVisualShapes(int bodyUniqueId) { btAlignedObjectArray* shapes = m_data->m_visualShapesMap[bodyUniqueId]; diff --git a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.h b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.h index 6759395aa..518086dc6 100644 --- a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.h +++ b/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);