diff --git a/build_visual_studio_vr_pybullet_double.bat b/build_visual_studio_vr_pybullet_double.bat index b6fd3cb934..d0c1eeb4a7 100644 --- a/build_visual_studio_vr_pybullet_double.bat +++ b/build_visual_studio_vr_pybullet_double.bat @@ -18,7 +18,7 @@ rem SET myvar=c:\python-3.5.2 cd build3 -premake4 --double --standalone-examples --enable_stable_pd --enable_multithreading --midi --enable_static_vr_plugin --enable_openvr --enable_pybullet --python_include_dir="%myvar%/include" --python_lib_dir="%myvar%/libs" --targetdir="../bin" vs2010 +premake4 --double --enable_stable_pd --enable_multithreading --midi --enable_static_vr_plugin --enable_openvr --enable_pybullet --python_include_dir="%myvar%/include" --python_lib_dir="%myvar%/libs" --targetdir="../bin" vs2010 rem premake4 --double --enable_multithreading --midi --enable_static_vr_plugin --enable_openvr --enable_pybullet --python_include_dir="%myvar%/include" --python_lib_dir="%myvar%/libs" --targetdir="../binserver" vs2010 rem premake4 --double --enable_grpc --enable_multithreading --midi --enable_static_vr_plugin --enable_openvr --enable_pybullet --python_include_dir="%myvar%/include" --python_lib_dir="%myvar%/libs" --targetdir="../binserver" vs2010 diff --git a/examples/SharedMemory/PhysicsClient.h b/examples/SharedMemory/PhysicsClient.h index b2633cb42d..33c139ef19 100644 --- a/examples/SharedMemory/PhysicsClient.h +++ b/examples/SharedMemory/PhysicsClient.h @@ -77,6 +77,8 @@ class PhysicsClient virtual void getCachedMassMatrix(int dofCountCheck, double* massMatrix) = 0; + virtual bool getCachedReturnData(struct b3UserDataValue* returnData) = 0; + virtual void setTimeOut(double timeOutInSeconds) = 0; virtual double getTimeOut() const = 0; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 1185ad5d0d..5906d82f17 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -3024,6 +3024,16 @@ B3_SHARED_API int b3GetStatusPluginCommandResult(b3SharedMemoryStatusHandle stat return statusUniqueId; } +B3_SHARED_API int b3GetStatusPluginCommandReturnData(b3PhysicsClientHandle physClient, struct b3UserDataValue* valueOut) +{ + PhysicsClient* cl = (PhysicsClient*)physClient; + if (cl) + { + return cl->getCachedReturnData(valueOut); + } + return false; +} + B3_SHARED_API int b3GetStatusPluginUniqueId(b3SharedMemoryStatusHandle statusHandle) { int statusUniqueId = -1; @@ -3059,7 +3069,7 @@ B3_SHARED_API void b3CustomCommandExecutePluginCommand(b3SharedMemoryCommandHand { command->m_updateFlags |= CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND; command->m_customCommandArgs.m_pluginUniqueId = pluginUniqueId; - + command->m_customCommandArgs.m_startingReturnBytes = 0; command->m_customCommandArgs.m_arguments.m_numInts = 0; command->m_customCommandArgs.m_arguments.m_numFloats = 0; command->m_customCommandArgs.m_arguments.m_text[0] = 0; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 6808b65fd3..cd35502fb0 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -66,7 +66,8 @@ extern "C" B3_SHARED_API void b3CustomCommandLoadPluginSetPostFix(b3SharedMemoryCommandHandle commandHandle, const char* postFix); B3_SHARED_API int b3GetStatusPluginUniqueId(b3SharedMemoryStatusHandle statusHandle); B3_SHARED_API int b3GetStatusPluginCommandResult(b3SharedMemoryStatusHandle statusHandle); - + B3_SHARED_API int b3GetStatusPluginCommandReturnData(b3PhysicsClientHandle physClient, struct b3UserDataValue* valueOut); + B3_SHARED_API void b3CustomCommandUnloadPlugin(b3SharedMemoryCommandHandle commandHandle, int pluginUniqueId); B3_SHARED_API void b3CustomCommandExecutePluginCommand(b3SharedMemoryCommandHandle commandHandle, int pluginUniqueId, const char* textArguments); B3_SHARED_API void b3CustomCommandExecuteAddIntArgument(b3SharedMemoryCommandHandle commandHandle, int intVal); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 5eb66fdd1d..bf1096e057 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -69,6 +69,9 @@ struct PhysicsClientSharedMemoryInternalData btHashMap m_userDataMap; btHashMap m_userDataHandleLookup; + btAlignedObjectArray m_cachedReturnData; + b3UserDataValue m_cachedReturnDataValue; + SharedMemoryStatus m_tempBackupServerStatus; SharedMemoryStatus m_lastServerStatus; @@ -1350,8 +1353,22 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() b3Warning("Request getCollisionInfo failed"); break; } + case CMD_CUSTOM_COMMAND_COMPLETED: { + + m_data->m_cachedReturnData.resize(serverCmd.m_customCommandResultArgs.m_returnDataSizeInBytes); + m_data->m_cachedReturnDataValue.m_length = serverCmd.m_customCommandResultArgs.m_returnDataSizeInBytes; + + if (serverCmd.m_customCommandResultArgs.m_returnDataSizeInBytes) + { + m_data->m_cachedReturnDataValue.m_type = serverCmd.m_customCommandResultArgs.m_returnDataType; + m_data->m_cachedReturnDataValue.m_data1 = &m_data->m_cachedReturnData[0]; + for (int i = 0; i < serverCmd.m_numDataStreamBytes; i++) + { + m_data->m_cachedReturnData[i + serverCmd.m_customCommandResultArgs.m_returnDataStart] = m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[i]; + } + } break; } case CMD_CALCULATED_JACOBIAN_COMPLETED: @@ -1664,6 +1681,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() m_data->m_lastServerStatus = m_data->m_tempBackupServerStatus; } + if (serverCmd.m_type == CMD_REMOVE_USER_DATA_COMPLETED) { B3_PROFILE("CMD_REMOVE_USER_DATA_COMPLETED"); @@ -1851,6 +1869,23 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() return 0; } + if (serverCmd.m_type == CMD_CUSTOM_COMMAND_COMPLETED) + { + int totalReceived = (serverCmd.m_numDataStreamBytes + serverCmd.m_customCommandResultArgs.m_returnDataStart); + int remaining = serverCmd.m_customCommandResultArgs.m_returnDataSizeInBytes - totalReceived; + + if (remaining > 0) + { + // continue requesting return data + SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0]; + command.m_type = CMD_CUSTOM_COMMAND; + command.m_customCommandArgs.m_startingReturnBytes = + totalReceived; + submitClientCommand(command); + return 0; + } + } + return &m_data->m_lastServerStatus; } else @@ -2006,6 +2041,16 @@ void PhysicsClientSharedMemory::getCachedMassMatrix(int dofCountCheck, double* m } } +bool PhysicsClientSharedMemory::getCachedReturnData(b3UserDataValue* returnData) +{ + if (m_data->m_cachedReturnDataValue.m_length) + { + *returnData = m_data->m_cachedReturnDataValue; + return true; + } + return false; + +} void PhysicsClientSharedMemory::getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo) { visualShapesInfo->m_numVisualShapes = m_data->m_cachedVisualShapes.size(); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.h b/examples/SharedMemory/PhysicsClientSharedMemory.h index 3a542efbc4..f1a67c5252 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.h +++ b/examples/SharedMemory/PhysicsClientSharedMemory.h @@ -90,6 +90,8 @@ class PhysicsClientSharedMemory : public PhysicsClient virtual void getCachedMassMatrix(int dofCountCheck, double* massMatrix); + virtual bool getCachedReturnData(b3UserDataValue* returnData); + virtual void setTimeOut(double timeOutInSeconds); virtual double getTimeOut() const; diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 7c0edfbf19..ab6ae5ba2c 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -79,6 +79,10 @@ struct PhysicsDirectInternalData btHashMap m_userDataMap; btHashMap m_userDataHandleLookup; + btAlignedObjectArray m_cachedReturnData; + b3UserDataValue m_cachedReturnDataValue; + + PhysicsCommandProcessorInterface* m_commandProcessor; bool m_ownsCommandProcessor; double m_timeOutInSeconds; @@ -1113,10 +1117,7 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd b3Warning("Request mesh data failed"); break; } - case CMD_CUSTOM_COMMAND_COMPLETED: - { - break; - } + case CMD_CUSTOM_COMMAND_FAILED: { b3Warning("custom plugin command failed"); @@ -1309,14 +1310,95 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd { break; } + case CMD_CUSTOM_COMMAND_COMPLETED: + { + break; + } default: { //b3Warning("Unknown server status type"); } + }; } + + +bool PhysicsDirect::processCustomCommand(const struct SharedMemoryCommand& orgCommand) +{ + SharedMemoryCommand command = orgCommand; + + const SharedMemoryStatus& serverCmd = m_data->m_serverStatus; + + int remaining = 0; + do + { + bool hasStatus = m_data->m_commandProcessor->processCommand(command, m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds() - startTime < timeOutInSeconds)) + { + const SharedMemoryStatus* stat = processServerStatus(); + if (stat) + { + hasStatus = true; + } + } + + m_data->m_hasStatus = hasStatus; + + if (hasStatus) + { + + if (m_data->m_verboseOutput) + { + b3Printf("Success receiving %d return data\n", + serverCmd.m_numDataStreamBytes); + } + + + btAssert(m_data->m_serverStatus.m_type == CMD_CUSTOM_COMMAND_COMPLETED); + + if (m_data->m_serverStatus.m_type == CMD_CUSTOM_COMMAND_COMPLETED) + { + m_data->m_cachedReturnData.resize(serverCmd.m_customCommandResultArgs.m_returnDataSizeInBytes); + m_data->m_cachedReturnDataValue.m_length = serverCmd.m_customCommandResultArgs.m_returnDataSizeInBytes; + + if (serverCmd.m_customCommandResultArgs.m_returnDataSizeInBytes) + { + m_data->m_cachedReturnDataValue.m_type = serverCmd.m_customCommandResultArgs.m_returnDataType; + m_data->m_cachedReturnDataValue.m_data1 = &m_data->m_cachedReturnData[0]; + for (int i = 0; i < serverCmd.m_numDataStreamBytes; i++) + { + m_data->m_cachedReturnData[i+ serverCmd.m_customCommandResultArgs.m_returnDataStart] = m_data->m_bulletStreamDataServerToClient[i]; + } + } + int totalReceived = serverCmd.m_numDataStreamBytes + serverCmd.m_customCommandResultArgs.m_returnDataStart; + remaining = serverCmd.m_customCommandResultArgs.m_returnDataSizeInBytes - totalReceived; + + if (remaining > 0) + { + m_data->m_hasStatus = false; + command.m_type = CMD_CUSTOM_COMMAND; + command.m_customCommandArgs.m_startingReturnBytes = + totalReceived; + } + } + } + + } while (remaining > 0); + + return m_data->m_hasStatus; +} + bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& command) { + if (command.m_type == CMD_CUSTOM_COMMAND) + { + return processCustomCommand(command); + } if (command.m_type == CMD_REQUEST_DEBUG_LINES) { return processDebugLines(command); @@ -1651,6 +1733,16 @@ void PhysicsDirect::getCachedMassMatrix(int dofCountCheck, double* massMatrix) } } +bool PhysicsDirect::getCachedReturnData(b3UserDataValue* returnData) +{ + if (m_data->m_cachedReturnDataValue.m_length) + { + *returnData = m_data->m_cachedReturnDataValue; + return true; + } + return false; +} + void PhysicsDirect::setTimeOut(double timeOutInSeconds) { m_data->m_timeOutInSeconds = timeOutInSeconds; diff --git a/examples/SharedMemory/PhysicsDirect.h b/examples/SharedMemory/PhysicsDirect.h index f0addbc116..2e09a95d27 100644 --- a/examples/SharedMemory/PhysicsDirect.h +++ b/examples/SharedMemory/PhysicsDirect.h @@ -22,12 +22,14 @@ class PhysicsDirect : public PhysicsClient bool processVisualShapeData(const struct SharedMemoryCommand& orgCommand); - bool processMeshData(const struct SharedMemoryCommand& orgCommand); + bool processMeshData(const struct SharedMemoryCommand& orgCommand); void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd); void processAddUserData(const struct SharedMemoryStatus& serverCmd); + bool processCustomCommand(const struct SharedMemoryCommand& orgCommand); + void postProcessStatus(const struct SharedMemoryStatus& serverCmd); void resetData(); @@ -112,6 +114,8 @@ class PhysicsDirect : public PhysicsClient virtual void getCachedMassMatrix(int dofCountCheck, double* massMatrix); + virtual bool getCachedReturnData(b3UserDataValue* returnData); + //the following APIs are for internal use for visualization: virtual bool connect(struct GUIHelperInterface* guiHelper); virtual void renderScene(); diff --git a/examples/SharedMemory/PhysicsLoopBack.cpp b/examples/SharedMemory/PhysicsLoopBack.cpp index 9872654329..58e4d59c7e 100644 --- a/examples/SharedMemory/PhysicsLoopBack.cpp +++ b/examples/SharedMemory/PhysicsLoopBack.cpp @@ -227,6 +227,10 @@ void PhysicsLoopBack::getCachedMassMatrix(int dofCountCheck, double* massMatrix) { m_data->m_physicsClient->getCachedMassMatrix(dofCountCheck, massMatrix); } +bool PhysicsLoopBack::getCachedReturnData(struct b3UserDataValue* returnData) +{ + return m_data->m_physicsClient->getCachedReturnData(returnData); +} void PhysicsLoopBack::setTimeOut(double timeOutInSeconds) { diff --git a/examples/SharedMemory/PhysicsLoopBack.h b/examples/SharedMemory/PhysicsLoopBack.h index 4a3c089a1a..4626aac812 100644 --- a/examples/SharedMemory/PhysicsLoopBack.h +++ b/examples/SharedMemory/PhysicsLoopBack.h @@ -88,6 +88,8 @@ class PhysicsLoopBack : public PhysicsClient virtual void getCachedMassMatrix(int dofCountCheck, double* massMatrix); + virtual bool getCachedReturnData(struct b3UserDataValue* returnData); + virtual void setTimeOut(double timeOutInSeconds); virtual double getTimeOut() const; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index dbec1dfe68..fa6506643e 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -5757,6 +5757,10 @@ bool PhysicsServerCommandProcessor::processCustomCommand(const struct SharedMemo SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_CUSTOM_COMMAND_FAILED; + serverCmd.m_customCommandResultArgs.m_returnDataSizeInBytes = 0; + serverCmd.m_customCommandResultArgs.m_returnDataType = -1; + serverCmd.m_customCommandResultArgs.m_returnDataStart = 0; + serverCmd.m_customCommandResultArgs.m_pluginUniqueId = -1; if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_LOAD_PLUGIN) @@ -5772,6 +5776,7 @@ bool PhysicsServerCommandProcessor::processCustomCommand(const struct SharedMemo if (pluginUniqueId >= 0) { serverCmd.m_customCommandResultArgs.m_pluginUniqueId = pluginUniqueId; + serverCmd.m_type = CMD_CUSTOM_COMMAND_COMPLETED; } } @@ -5780,10 +5785,34 @@ bool PhysicsServerCommandProcessor::processCustomCommand(const struct SharedMemo m_data->m_pluginManager.unloadPlugin(clientCmd.m_customCommandArgs.m_pluginUniqueId); serverCmd.m_type = CMD_CUSTOM_COMMAND_COMPLETED; } + if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND) { - int result = m_data->m_pluginManager.executePluginCommand(clientCmd.m_customCommandArgs.m_pluginUniqueId, &clientCmd.m_customCommandArgs.m_arguments); - serverCmd.m_customCommandResultArgs.m_executeCommandResult = result; + int startBytes = clientCmd.m_customCommandArgs.m_startingReturnBytes; + if (startBytes == 0) + { + int result = m_data->m_pluginManager.executePluginCommand(clientCmd.m_customCommandArgs.m_pluginUniqueId, &clientCmd.m_customCommandArgs.m_arguments); + serverCmd.m_customCommandResultArgs.m_executeCommandResult = result; + } + const b3UserDataValue* returnData = m_data->m_pluginManager.getReturnData(clientCmd.m_customCommandArgs.m_pluginUniqueId); + if (returnData) + { + int totalRemain = returnData->m_length - startBytes; + int numBytes = totalRemain <= bufferSizeInBytes ? totalRemain : bufferSizeInBytes; + serverStatusOut.m_numDataStreamBytes = numBytes; + for (int i = 0; i < numBytes; i++) + { + bufferServerToClient[i] = returnData->m_data1[i+ startBytes]; + } + serverCmd.m_customCommandResultArgs.m_returnDataSizeInBytes = returnData->m_length; + serverCmd.m_customCommandResultArgs.m_returnDataType = returnData->m_type; + serverCmd.m_customCommandResultArgs.m_returnDataStart = startBytes; + } + else + { + serverStatusOut.m_numDataStreamBytes = 0; + } + serverCmd.m_type = CMD_CUSTOM_COMMAND_COMPLETED; } return hasStatus; @@ -9663,6 +9692,11 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc mb->getBaseCollider()->setAnisotropicFriction(anisotropicFriction); } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_PROCESSING_THRESHOLD) + { + mb->getBaseCollider()->setContactProcessingThreshold(clientCmd.m_changeDynamicsInfoArgs.m_contactProcessingThreshold); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MAX_JOINT_VELOCITY) { mb->setMaxCoordinateVelocity(clientCmd.m_changeDynamicsInfoArgs.m_maxJointVelocity); @@ -9821,6 +9855,11 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc { mb->getLinkCollider(linkIndex)->setAnisotropicFriction(anisotropicFriction); } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_PROCESSING_THRESHOLD) + { + mb->getLinkCollider(linkIndex)->setContactProcessingThreshold(clientCmd.m_changeDynamicsInfoArgs.m_contactProcessingThreshold); + } + } } } diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index db08e0e1b8..197dbc0c68 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -132,12 +132,16 @@ struct b3CustomCommand b3PluginArguments m_arguments; char m_pluginPath[MAX_FILENAME_LENGTH]; char m_postFix[MAX_FILENAME_LENGTH]; + int m_startingReturnBytes; }; struct b3CustomCommandResultArgs { int m_pluginUniqueId; int m_executeCommandResult; + int m_returnDataType; + int m_returnDataSizeInBytes; + int m_returnDataStart; }; struct BulletDataStreamArgs diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index e9ee26f17b..46c69fe07f 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -9,7 +9,8 @@ -#define SHARED_MEMORY_MAGIC_NUMBER 202007060 +#define SHARED_MEMORY_MAGIC_NUMBER 202010061 +//#define SHARED_MEMORY_MAGIC_NUMBER 202007060 //#define SHARED_MEMORY_MAGIC_NUMBER 202005070 //#define SHARED_MEMORY_MAGIC_NUMBER 202002030 //#define SHARED_MEMORY_MAGIC_NUMBER 202001230 @@ -237,6 +238,7 @@ enum EnumSharedMemoryServerStatus CMD_REQUEST_MESH_DATA_COMPLETED, CMD_REQUEST_MESH_DATA_FAILED, + //don't go beyond 'CMD_MAX_SERVER_COMMANDS! CMD_MAX_SERVER_COMMANDS }; diff --git a/examples/SharedMemory/b3PluginManager.cpp b/examples/SharedMemory/b3PluginManager.cpp index 7703abb548..db7910e08c 100644 --- a/examples/SharedMemory/b3PluginManager.cpp +++ b/examples/SharedMemory/b3PluginManager.cpp @@ -51,6 +51,7 @@ struct b3Plugin PFN_GET_FILEIO_INTERFACE m_getFileIOFunc; void* m_userPointer; + b3UserDataValue* m_returnData; b3Plugin() : m_pluginHandle(0), @@ -67,7 +68,8 @@ struct b3Plugin m_getRendererFunc(0), m_getCollisionFunc(0), m_getFileIOFunc(0), - m_userPointer(0) + m_userPointer(0), + m_returnData(0) { } void clear() @@ -88,6 +90,7 @@ struct b3Plugin m_getCollisionFunc = 0; m_getFileIOFunc = 0; m_userPointer = 0; + m_returnData = 0; m_isInitialized = false; } }; @@ -331,6 +334,7 @@ void b3PluginManager::unloadPlugin(int pluginUniqueId) { plugin->m_exitFunc(&context); plugin->m_userPointer = 0; + plugin->m_returnData = 0; plugin->m_isInitialized = false; } m_data->m_pluginMap.remove(plugin->m_pluginPath.c_str()); @@ -451,6 +455,7 @@ int b3PluginManager::executePluginCommand(int pluginUniqueId, const b3PluginArgu context.m_rpcCommandProcessorInterface = m_data->m_rpcCommandProcessorInterface; result = plugin->m_executeCommandFunc(&context, arguments); plugin->m_userPointer = context.m_userPointer; + plugin->m_returnData = context.m_returnData; } return result; } @@ -478,6 +483,7 @@ int b3PluginManager::registerStaticLinkedPlugin(const char* pluginPath, b3Plugi pluginHandle->m_pluginHandle = 0; pluginHandle->m_pluginPath = pluginPath; pluginHandle->m_userPointer = 0; + pluginHandle->m_returnData = 0; if (pluginHandle->m_processNotificationsFunc) { @@ -490,11 +496,13 @@ int b3PluginManager::registerStaticLinkedPlugin(const char* pluginPath, b3Plugi { b3PluginContext context = {0}; context.m_userPointer = 0; + context.m_returnData = 0; context.m_physClient = (b3PhysicsClientHandle)m_data->m_physicsDirect; context.m_rpcCommandProcessorInterface = m_data->m_rpcCommandProcessorInterface; int result = pluginHandle->m_initFunc(&context); pluginHandle->m_isInitialized = true; pluginHandle->m_userPointer = context.m_userPointer; + pluginHandle->m_returnData = 0; } return pluginUniqueId; } @@ -569,3 +577,13 @@ struct b3PluginCollisionInterface* b3PluginManager::getCollisionInterface() } return collisionInterface; } + +const struct b3UserDataValue* b3PluginManager::getReturnData(int pluginUniqueId) +{ + b3PluginHandle* plugin = m_data->m_plugins.getHandle(pluginUniqueId); + if (plugin) + { + return plugin->m_returnData; + } + return 0; +} diff --git a/examples/SharedMemory/b3PluginManager.h b/examples/SharedMemory/b3PluginManager.h index 4beb05cd6d..240c3e2fc6 100644 --- a/examples/SharedMemory/b3PluginManager.h +++ b/examples/SharedMemory/b3PluginManager.h @@ -70,6 +70,8 @@ class b3PluginManager void selectCollisionPlugin(int pluginUniqueId); struct b3PluginCollisionInterface* getCollisionInterface(); + + const struct b3UserDataValue* getReturnData(int pluginUniqueId); }; #endif //B3_PLUGIN_MANAGER_H diff --git a/examples/SharedMemory/plugins/b3PluginContext.h b/examples/SharedMemory/plugins/b3PluginContext.h index 89ede4584d..045c9c2a80 100644 --- a/examples/SharedMemory/plugins/b3PluginContext.h +++ b/examples/SharedMemory/plugins/b3PluginContext.h @@ -10,6 +10,11 @@ struct b3PluginContext //plugin can modify the m_userPointer to store persistent object pointer (class or struct instance etc) void* m_userPointer; + //plugin can provide additional return data for executePluginCommand. + //Lifetime of this m_returnData pointer is minimum of + //next call to the next executePluginCommand or plugin termination. + b3UserDataValue* m_returnData; + const struct b3VRControllerEvent* m_vrControllerEvents; int m_numVRControllerEvents; const struct b3KeyboardEvent* m_keyEvents; diff --git a/examples/SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp b/examples/SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp index 780d1c7601..013314380b 100644 --- a/examples/SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp +++ b/examples/SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp @@ -21,11 +21,18 @@ while (1): struct MyRendererPluginClass { TinyRendererVisualShapeConverter m_renderer; + b3UserDataValue* m_returnData; MyRendererPluginClass() + :m_returnData(0) { } virtual ~MyRendererPluginClass() { + if (m_returnData) + { + delete[] m_returnData->m_data1; + } + delete m_returnData; } }; @@ -38,6 +45,20 @@ B3_SHARED_API int initPlugin_tinyRendererPlugin(struct b3PluginContext* context) B3_SHARED_API int executePluginCommand_tinyRendererPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments) { + MyRendererPluginClass* obj = (MyRendererPluginClass*)context->m_userPointer; + if (obj->m_returnData==0) + { + obj->m_returnData = new b3UserDataValue(); + obj->m_returnData->m_type = 1; + obj->m_returnData->m_length = 123; + char* data = new char[obj->m_returnData->m_length]; + for (int i = 0; i < obj->m_returnData->m_length; i++) + { + data[i] = i; + } + obj->m_returnData->m_data1 = data; + } + context->m_returnData = obj->m_returnData; return -1; } diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 98f264ee98..0128bcc847 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -11215,6 +11215,7 @@ static PyObject* pybullet_executePluginCommand(PyObject* self, b3SharedMemoryCommandHandle command = 0; b3SharedMemoryStatusHandle statusHandle = 0; int statusType = -1; + int statusResult = -1; PyObject* intArgs = 0; PyObject* floatArgs = 0; @@ -11263,8 +11264,34 @@ static PyObject* pybullet_executePluginCommand(PyObject* self, } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - statusType = b3GetStatusPluginCommandResult(statusHandle); - return PyInt_FromLong(statusType); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_CUSTOM_COMMAND_COMPLETED) + { + statusResult = b3GetStatusPluginCommandResult(statusHandle); + struct b3UserDataValue dv; + if (b3GetStatusPluginCommandReturnData(sm, &dv)) + { + assert(dv.m_length>0); + PyObject* pylist; + PyObject* pydata; + int i; + //return type + //user data type + //bytes + pylist = PyTuple_New(3); + PyTuple_SetItem(pylist, 0, PyInt_FromLong(statusResult)); + PyTuple_SetItem(pylist, 1, PyInt_FromLong(dv.m_type)); + pydata = PyTuple_New(dv.m_length); + for (i = 0; i < dv.m_length; i++) + { + PyTuple_SetItem(pydata, i, PyInt_FromLong(dv.m_data1[i])); + } + PyTuple_SetItem(pylist, 2, pydata); + return pylist; + } + return PyInt_FromLong(statusResult); + } + return PyInt_FromLong(-1); }