diff --git a/examples/SharedMemory/BodyJointInfoUtility.h b/examples/SharedMemory/BodyJointInfoUtility.h index decced5ba..6bf018808 100644 --- a/examples/SharedMemory/BodyJointInfoUtility.h +++ b/examples/SharedMemory/BodyJointInfoUtility.h @@ -34,8 +34,8 @@ template void addJointInfoFromMultiBodyData(const T* mb { { b3JointInfo info; - info.m_jointName = 0; - info.m_linkName = 0; + info.m_jointName[0] = 0; + info.m_linkName[0] = 0; info.m_flags = 0; info.m_jointIndex = link; info.m_qIndex = @@ -48,14 +48,16 @@ template void addJointInfoFromMultiBodyData(const T* mb b3Printf("mb->m_links[%d].m_linkName = %s\n", link, mb->m_links[link].m_linkName); } - info.m_linkName = strDup(mb->m_links[link].m_linkName); + strcpy(info.m_linkName,mb->m_links[link].m_linkName); + } if (mb->m_links[link].m_jointName) { if (verboseOutput) { b3Printf("mb->m_links[%d].m_jointName = %s\n", link, mb->m_links[link].m_jointName); } - info.m_jointName = strDup(mb->m_links[link].m_jointName); + strcpy(info.m_jointName,mb->m_links[link].m_jointName); + //info.m_jointName = strDup(mb->m_links[link].m_jointName); } info.m_jointType = mb->m_links[link].m_jointType; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index d8c2fff7f..ce39a2dd2 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1438,7 +1438,10 @@ B3_SHARED_API int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle com B3_SHARED_API void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; - cl->disconnectSharedMemory(); + if (cl) + { + cl->disconnectSharedMemory(); + } delete cl; } @@ -1777,6 +1780,23 @@ B3_SHARED_API void b3CustomCommandLoadPlugin(b3SharedMemoryCommandHandle command } } +B3_SHARED_API void b3CustomCommandLoadPluginSetPostFix(b3SharedMemoryCommandHandle commandHandle, const char* postFix) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_CUSTOM_COMMAND); + if (command->m_type == CMD_CUSTOM_COMMAND) + { + command->m_updateFlags |= CMD_CUSTOM_COMMAND_LOAD_PLUGIN_POSTFIX; + command->m_customCommandArgs.m_postFix[0] = 0; + + int len = strlen(postFix); + if (lenm_customCommandArgs.m_postFix, postFix); + } + } +} + B3_SHARED_API int b3GetStatusPluginCommandResult(b3SharedMemoryStatusHandle statusHandle) diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index d18a547fe..0026a7355 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -63,6 +63,7 @@ B3_SHARED_API int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle); ///Plugin system, load and unload a plugin, execute a command B3_SHARED_API b3SharedMemoryCommandHandle b3CreateCustomCommand(b3PhysicsClientHandle physClient); B3_SHARED_API void b3CustomCommandLoadPlugin(b3SharedMemoryCommandHandle commandHandle, const char* pluginPath); +B3_SHARED_API void b3CustomCommandLoadPluginSetPostFix(b3SharedMemoryCommandHandle commandHandle, const char* postFix); B3_SHARED_API int b3GetStatusPluginUniqueId(b3SharedMemoryStatusHandle statusHandle); B3_SHARED_API int b3GetStatusPluginCommandResult(b3SharedMemoryStatusHandle statusHandle); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 94c73f5f2..e3f133acf 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -108,8 +108,8 @@ bool PhysicsClientSharedMemory::getBodyInfo(int bodyUniqueId, struct b3BodyInfo& if (bodyJointsPtr && *bodyJointsPtr) { BodyJointInfoCache* bodyJoints = *bodyJointsPtr; - info.m_baseName = bodyJoints->m_baseName.c_str(); - info.m_bodyName = bodyJoints->m_bodyName.c_str(); + strcpy(info.m_baseName,bodyJoints->m_baseName.c_str()); + strcpy(info.m_bodyName,bodyJoints->m_bodyName.c_str()); return true; } @@ -234,16 +234,6 @@ void PhysicsClientSharedMemory::resetData() if (bodyJointsPtr && *bodyJointsPtr) { BodyJointInfoCache* bodyJoints = *bodyJointsPtr; - for (int j=0;jm_jointInfo.size();j++) { - if (bodyJoints->m_jointInfo[j].m_jointName) - { - free(bodyJoints->m_jointInfo[j].m_jointName); - } - if (bodyJoints->m_jointInfo[j].m_linkName) - { - free(bodyJoints->m_jointInfo[j].m_linkName); - } - } delete (*bodyJointsPtr); } } @@ -392,8 +382,8 @@ void PhysicsClientSharedMemory::processBodyJointInfo(int bodyUniqueId, const Sha template void addJointInfoFromConstraint(int linkIndex, const T* con, U* bodyJoints, bool verboseOutput) { b3JointInfo info; - info.m_jointName = 0; - info.m_linkName = 0; + info.m_jointName[0] = 0; + info.m_linkName[0] = 0; info.m_flags = 0; info.m_jointIndex = linkIndex; info.m_qIndex = linkIndex+7; @@ -402,7 +392,8 @@ template void addJointInfoFromConstraint(int linkIndex, if (con->m_typeConstraintData.m_name) { - info.m_jointName = strDup(con->m_typeConstraintData.m_name); + strcpy(info.m_jointName,con->m_typeConstraintData.m_name); + //info.m_linkName = strDup(con->m_typeConstraintData.m_name); } diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 76d09dfe7..94ca9bea6 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -112,17 +112,6 @@ void PhysicsDirect::resetData() BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i); if (bodyJointsPtr && *bodyJointsPtr) { - BodyJointInfoCache2* bodyJoints = *bodyJointsPtr; - for (int j = 0; jm_jointInfo.size(); j++) { - if (bodyJoints->m_jointInfo[j].m_jointName) - { - free(bodyJoints->m_jointInfo[j].m_jointName); - } - if (bodyJoints->m_jointInfo[j].m_linkName) - { - free(bodyJoints->m_jointInfo[j].m_linkName); - } - } delete (*bodyJointsPtr); } } @@ -1085,8 +1074,8 @@ bool PhysicsDirect::getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const if (bodyJointsPtr && *bodyJointsPtr) { BodyJointInfoCache2* bodyJoints = *bodyJointsPtr; - info.m_baseName = bodyJoints->m_baseName.c_str(); - info.m_bodyName = bodyJoints->m_bodyName.c_str(); + strcpy(info.m_baseName,bodyJoints->m_baseName.c_str()); + strcpy(info.m_bodyName ,bodyJoints->m_bodyName .c_str()); return true; } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index f9e0cd6a9..971d42f15 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -40,6 +40,11 @@ #include "../Utils/b3Clock.h" #include "b3PluginManager.h" + +#ifdef STATIC_LINK_VR_PLUGIN +#include "plugins/vrSyncPlugin/vrSyncPlugin.h" +#endif + #ifdef B3_ENABLE_TINY_AUDIO #include "../TinyAudio/b3SoundEngine.h" #endif @@ -1546,10 +1551,11 @@ struct PhysicsServerCommandProcessorInternalData { - //test to statically link a plugin - //#include "plugins/testPlugin/testplugin.h" //register static plugins: - //m_pluginManager.registerStaticLinkedPlugin("path", initPlugin, exitPlugin, executePluginCommand); +#ifdef STATIC_LINK_VR_PLUGIN + m_pluginManager.registerStaticLinkedPlugin("vrSyncPlugin", initPlugin_vrSyncPlugin, exitPlugin_vrSyncPlugin, executePluginCommand_vrSyncPlugin,preTickPluginCallback_vrSyncPlugin,0); +#endif //STATIC_LINK_VR_PLUGIN + } m_vrControllerEvents.init(); @@ -1699,7 +1705,6 @@ void logCallback(btDynamicsWorld *world, btScalar timeStep) bool isPreTick = false; proc->tickPlugins(timeStep, isPreTick); - } bool MyContactAddedCallback(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1) @@ -1810,6 +1815,11 @@ void PhysicsServerCommandProcessor::processCollisionForces(btScalar timeStep) void PhysicsServerCommandProcessor::tickPlugins(btScalar timeStep, bool isPreTick) { m_data->m_pluginManager.tickPlugins(timeStep, isPreTick); + if (!isPreTick) + { + //clear events after each postTick, so we don't receive events multiple ticks + m_data->m_pluginManager.clearEvents(); + } } @@ -8027,7 +8037,13 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_LOAD_PLUGIN) { //pluginPath could be registered or load from disk - int pluginUniqueId = m_data->m_pluginManager.loadPlugin(clientCmd.m_customCommandArgs.m_pluginPath); + const char* postFix = ""; + if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_LOAD_PLUGIN_POSTFIX) + { + postFix = clientCmd.m_customCommandArgs.m_postFix; + } + + int pluginUniqueId = m_data->m_pluginManager.loadPlugin(clientCmd.m_customCommandArgs.m_pluginPath, postFix); if (pluginUniqueId>=0) { serverCmd.m_customCommandResultArgs.m_pluginUniqueId = pluginUniqueId; @@ -8291,7 +8307,7 @@ bool PhysicsServerCommandProcessor::isRealTimeSimulationEnabled() const void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec,const struct b3VRControllerEvent* vrControllerEvents, int numVRControllerEvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents, const struct b3MouseEvent* mouseEvents, int numMouseEvents) { m_data->m_vrControllerEvents.addNewVREvents(vrControllerEvents,numVRControllerEvents); - + m_data->m_pluginManager.addEvents(vrControllerEvents, numVRControllerEvents, keyEvents, numKeyEvents, mouseEvents, numMouseEvents); for (int i=0;im_stateLoggers.size();i++) { diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index b196d6c71..f68196809 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -114,6 +114,7 @@ enum CustomCommandEnum CMD_CUSTOM_COMMAND_LOAD_PLUGIN=1, CMD_CUSTOM_COMMAND_UNLOAD_PLUGIN=2, CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND=4, + CMD_CUSTOM_COMMAND_LOAD_PLUGIN_POSTFIX=8, }; struct b3CustomCommand @@ -121,6 +122,8 @@ struct b3CustomCommand int m_pluginUniqueId; b3PluginArguments m_arguments; char m_pluginPath[MAX_FILENAME_LENGTH]; + char m_postFix[MAX_FILENAME_LENGTH]; + }; struct b3CustomCommandResultArgs diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 28b7ec5b1..3b2d152a7 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -214,8 +214,8 @@ enum b3JointInfoFlags struct b3JointInfo { - char* m_linkName; - char* m_jointName; + char m_linkName[1024]; + char m_jointName[1024]; int m_jointType; int m_qIndex; int m_uIndex; @@ -254,8 +254,8 @@ struct b3UserConstraint struct b3BodyInfo { - const char* m_baseName; - const char* m_bodyName; // for btRigidBody, it does not have a base, but can still have a body name from urdf + char m_baseName[1024]; + char m_bodyName[1024]; // for btRigidBody, it does not have a base, but can still have a body name from urdf }; struct b3DynamicsInfo diff --git a/examples/SharedMemory/b3PluginManager.cpp b/examples/SharedMemory/b3PluginManager.cpp index 7f6052349..7ea16c33a 100644 --- a/examples/SharedMemory/b3PluginManager.cpp +++ b/examples/SharedMemory/b3PluginManager.cpp @@ -74,8 +74,11 @@ typedef b3PoolBodyHandle b3PluginHandle; struct b3PluginManagerInternalData { b3ResizablePool m_plugins; - b3HashMap m_pluginMap; + b3HashMap m_pluginMap; PhysicsDirect* m_physicsDirect; + b3AlignedObjectArray m_keyEvents; + b3AlignedObjectArray m_vrEvents; + b3AlignedObjectArray m_mouseEvents; }; b3PluginManager::b3PluginManager(class PhysicsCommandProcessorInterface* physSdk) @@ -89,8 +92,8 @@ b3PluginManager::~b3PluginManager() { while (m_data->m_pluginMap.size()) { - b3PluginHandle* plugin = m_data->m_pluginMap.getAtIndex(0); - unloadPlugin(plugin->m_pluginUniqueId); + b3PluginHandle** plugin = m_data->m_pluginMap.getAtIndex(0); + unloadPlugin((*plugin)->m_pluginUniqueId); } delete m_data->m_physicsDirect; m_data->m_pluginMap.clear(); @@ -98,16 +101,40 @@ b3PluginManager::~b3PluginManager() delete m_data; } +void b3PluginManager::addEvents(const struct b3VRControllerEvent* vrControllerEvents, int numVRControllerEvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents, const struct b3MouseEvent* mouseEvents, int numMouseEvents) +{ -int b3PluginManager::loadPlugin(const char* pluginPath) + for (int i = 0; i < numKeyEvents; i++) + { + m_data->m_keyEvents.push_back(keyEvents[i]); + } + + for (int i = 0; i < numVRControllerEvents; i++) + { + m_data->m_vrEvents.push_back(vrControllerEvents[i]); + } + for (int i = 0; i < numMouseEvents; i++) + { + m_data->m_mouseEvents.push_back(mouseEvents[i]); + } +} + +void b3PluginManager::clearEvents() +{ + m_data->m_keyEvents.resize(0); + m_data->m_vrEvents.resize(0); + m_data->m_mouseEvents.resize(0); +} + +int b3PluginManager::loadPlugin(const char* pluginPath, const char* postFixStr) { int pluginUniqueId = -1; - b3Plugin* pluginOrg = m_data->m_pluginMap.find(pluginPath); - if (pluginOrg) + b3PluginHandle** pluginOrgPtr = m_data->m_pluginMap.find(pluginPath); + if (pluginOrgPtr) { //already loaded - pluginUniqueId = pluginOrg->m_pluginUniqueId; + pluginUniqueId = (*pluginOrgPtr)->m_pluginUniqueId; } else { @@ -118,12 +145,18 @@ int b3PluginManager::loadPlugin(const char* pluginPath) bool ok = false; if (pluginHandle) { + std::string postFix = postFixStr; + std::string initStr = std::string("initPlugin") + postFix; + std::string exitStr = std::string("exitPlugin") + postFix; + std::string executePluginCommandStr = std::string("executePluginCommand") + postFix; + std::string preTickPluginCallbackStr = std::string("preTickPluginCallback") + postFix; + std::string postTickPluginCallback = std::string("postTickPluginCallback") + postFix; - plugin->m_initFunc = (PFN_INIT)B3_DYNLIB_IMPORT(pluginHandle, "initPlugin"); - plugin->m_exitFunc = (PFN_EXIT)B3_DYNLIB_IMPORT(pluginHandle, "exitPlugin"); - plugin->m_executeCommandFunc = (PFN_EXECUTE)B3_DYNLIB_IMPORT(pluginHandle, "executePluginCommand"); - plugin->m_preTickFunc = (PFN_TICK)B3_DYNLIB_IMPORT(pluginHandle, "preTickPluginCallback"); - plugin->m_postTickFunc = (PFN_TICK)B3_DYNLIB_IMPORT(pluginHandle, "postTickPluginCallback"); + plugin->m_initFunc = (PFN_INIT)B3_DYNLIB_IMPORT(pluginHandle, initStr.c_str()); + plugin->m_exitFunc = (PFN_EXIT)B3_DYNLIB_IMPORT(pluginHandle, exitStr.c_str()); + plugin->m_executeCommandFunc = (PFN_EXECUTE)B3_DYNLIB_IMPORT(pluginHandle, executePluginCommandStr.c_str()); + plugin->m_preTickFunc = (PFN_TICK)B3_DYNLIB_IMPORT(pluginHandle, preTickPluginCallbackStr.c_str()); + plugin->m_postTickFunc = (PFN_TICK)B3_DYNLIB_IMPORT(pluginHandle, postTickPluginCallback.c_str()); if (plugin->m_initFunc && plugin->m_exitFunc && plugin->m_executeCommandFunc) { @@ -140,7 +173,7 @@ int b3PluginManager::loadPlugin(const char* pluginPath) plugin->m_ownsPluginHandle = true; plugin->m_pluginHandle = pluginHandle; plugin->m_pluginPath = pluginPath; - m_data->m_pluginMap.insert(pluginPath, *plugin); + m_data->m_pluginMap.insert(pluginPath, plugin); } else { @@ -190,14 +223,28 @@ void b3PluginManager::tickPlugins(double timeStep, bool isPreTick) { for (int i=0;im_pluginMap.size();i++) { - b3PluginHandle* plugin = m_data->m_pluginMap.getAtIndex(i); - + b3PluginHandle** pluginPtr = m_data->m_pluginMap.getAtIndex(i); + b3PluginHandle* plugin = 0; + if (pluginPtr && *pluginPtr) + { + plugin = *pluginPtr; + } + else + { + continue; + } PFN_TICK tick = isPreTick? plugin->m_preTickFunc : plugin->m_postTickFunc; if (tick) { b3PluginContext context; context.m_userPointer = plugin->m_userPointer; context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect; + context.m_numMouseEvents = m_data->m_mouseEvents.size(); + context.m_mouseEvents = m_data->m_mouseEvents.size() ? &m_data->m_mouseEvents[0] : 0; + context.m_numKeyEvents = m_data->m_keyEvents.size(); + context.m_keyEvents = m_data->m_keyEvents.size() ? &m_data->m_keyEvents[0] : 0; + context.m_numVRControllerEvents = m_data->m_vrEvents.size(); + context.m_vrControllerEvents = m_data->m_vrEvents.size()? &m_data->m_vrEvents[0]:0; int result = tick(&context); plugin->m_userPointer = context.m_userPointer; } @@ -214,7 +261,13 @@ int b3PluginManager::executePluginCommand(int pluginUniqueId, const b3PluginArgu b3PluginContext context; context.m_userPointer = plugin->m_userPointer; context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect; - + context.m_numMouseEvents = 0; + context.m_mouseEvents = 0; + context.m_numKeyEvents = 0; + context.m_keyEvents = 0; + context.m_numVRControllerEvents = 0; + context.m_vrControllerEvents = 0; + result = plugin->m_executeCommandFunc(&context, arguments); plugin->m_userPointer = context.m_userPointer; } @@ -242,12 +295,19 @@ int b3PluginManager::registerStaticLinkedPlugin(const char* pluginPath, PFN_INIT pluginHandle->m_pluginPath = pluginPath; pluginHandle->m_userPointer = 0; - m_data->m_pluginMap.insert(pluginPath, *pluginHandle); + m_data->m_pluginMap.insert(pluginPath, pluginHandle); { b3PluginContext context; context.m_userPointer = 0; context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect; + context.m_numMouseEvents = 0; + context.m_mouseEvents = 0; + context.m_numKeyEvents = 0; + context.m_keyEvents = 0; + context.m_numVRControllerEvents = 0; + context.m_vrControllerEvents = 0; + int result = pluginHandle->m_initFunc(&context); pluginHandle->m_userPointer = context.m_userPointer; } diff --git a/examples/SharedMemory/b3PluginManager.h b/examples/SharedMemory/b3PluginManager.h index 6d4fa7ef1..d9466b071 100644 --- a/examples/SharedMemory/b3PluginManager.h +++ b/examples/SharedMemory/b3PluginManager.h @@ -12,10 +12,14 @@ class b3PluginManager b3PluginManager(class PhysicsCommandProcessorInterface* physSdk); virtual ~b3PluginManager(); - int loadPlugin(const char* pluginPath); + int loadPlugin(const char* pluginPath, const char* postFixStr =""); void unloadPlugin(int pluginUniqueId); int executePluginCommand(int pluginUniqueId, const struct b3PluginArguments* arguments); + void addEvents(const struct b3VRControllerEvent* vrControllerEvents, int numVRControllerEvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents, const struct b3MouseEvent* mouseEvents, int numMouseEvents); + void clearEvents(); + void tickPlugins(double timeStep, bool isPreTick); + int registerStaticLinkedPlugin(const char* pluginPath, PFN_INIT initFunc,PFN_EXIT exitFunc, PFN_EXECUTE executeCommandFunc, PFN_TICK preTickFunc, PFN_TICK postTickFunc); }; diff --git a/examples/SharedMemory/plugins/b3PluginContext.h b/examples/SharedMemory/plugins/b3PluginContext.h index cb8bd754d..8d042a3b8 100644 --- a/examples/SharedMemory/plugins/b3PluginContext.h +++ b/examples/SharedMemory/plugins/b3PluginContext.h @@ -5,12 +5,17 @@ struct b3PluginContext { - b3PhysicsClientHandle m_physClient; //plugin can modify the m_userPointer to store persistent object pointer (class or struct instance etc) void* m_userPointer; + const struct b3VRControllerEvent* m_vrControllerEvents; + int m_numVRControllerEvents; + const struct b3KeyboardEvent* m_keyEvents; + int m_numKeyEvents; + const struct b3MouseEvent* m_mouseEvents; + int m_numMouseEvents; }; diff --git a/examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.cpp b/examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.cpp index 474e944df..78dc439f1 100644 --- a/examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.cpp +++ b/examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.cpp @@ -37,7 +37,7 @@ struct MyClass } }; -B3_SHARED_API int initPlugin(struct b3PluginContext* context) +B3_SHARED_API int initPlugin_vrSyncPlugin(struct b3PluginContext* context) { MyClass* obj = new MyClass(); context->m_userPointer = obj; @@ -47,28 +47,17 @@ B3_SHARED_API int initPlugin(struct b3PluginContext* context) } -B3_SHARED_API int preTickPluginCallback(struct b3PluginContext* context) +B3_SHARED_API int preTickPluginCallback_vrSyncPlugin(struct b3PluginContext* context) { MyClass* obj = (MyClass* )context->m_userPointer; - if (obj->m_controllerId>=0) + if (obj && obj->m_controllerId>=0) { - b3SharedMemoryCommandHandle commandHandle = b3RequestVREventsCommandInit(context->m_physClient); - int deviceTypeFilter = VR_DEVICE_CONTROLLER; - b3VREventsSetDeviceTypeFilter(commandHandle, deviceTypeFilter); - - b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle); - int statusType = b3GetStatusType(statusHandle); - if (statusType == CMD_REQUEST_VR_EVENTS_DATA_COMPLETED) { - struct b3VREventsData vrEvents; - int i = 0; - b3GetVREventsData(context->m_physClient, &vrEvents); - if (vrEvents.m_numControllerEvents) { - for (int n=0;nm_numVRControllerEvents;n++) { - b3VRControllerEvent& event = vrEvents.m_controllerEvents[n]; + const b3VRControllerEvent& event = context->m_vrControllerEvents[n]; if (event.m_controllerId ==obj->m_controllerId) { if (obj->m_constraintId>=0) @@ -171,7 +160,7 @@ B3_SHARED_API int preTickPluginCallback(struct b3PluginContext* context) -B3_SHARED_API int executePluginCommand(struct b3PluginContext* context, const struct b3PluginArguments* arguments) +B3_SHARED_API int executePluginCommand_vrSyncPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments) { MyClass* obj = (MyClass*) context->m_userPointer; if (arguments->m_numInts>=4 && arguments->m_numFloats >= 2) @@ -199,7 +188,7 @@ B3_SHARED_API int executePluginCommand(struct b3PluginContext* context, const st } -B3_SHARED_API void exitPlugin(struct b3PluginContext* context) +B3_SHARED_API void exitPlugin_vrSyncPlugin(struct b3PluginContext* context) { MyClass* obj = (MyClass*) context->m_userPointer; delete obj; diff --git a/examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.h b/examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.h index fcc60a610..9b4cebd1a 100644 --- a/examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.h +++ b/examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.h @@ -9,12 +9,12 @@ extern "C" #endif //initPlugin, exitPlugin and executePluginCommand are required, otherwise plugin won't load -B3_SHARED_API int initPlugin(struct b3PluginContext* context); -B3_SHARED_API void exitPlugin(struct b3PluginContext* context); -B3_SHARED_API int executePluginCommand(struct b3PluginContext* context, const struct b3PluginArguments* arguments); +B3_SHARED_API int initPlugin_vrSyncPlugin(struct b3PluginContext* context); +B3_SHARED_API void exitPlugin_vrSyncPlugin(struct b3PluginContext* context); +B3_SHARED_API int executePluginCommand_vrSyncPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments); //preTickPluginCallback and postTickPluginCallback are optional. -B3_SHARED_API int preTickPluginCallback(struct b3PluginContext* context); +B3_SHARED_API int preTickPluginCallback_vrSyncPlugin(struct b3PluginContext* context); diff --git a/examples/pybullet/examples/vr_kuka_setup_vrSyncPlugin.py b/examples/pybullet/examples/vr_kuka_setup_vrSyncPlugin.py index 7297bdd73..0085e52e4 100644 --- a/examples/pybullet/examples/vr_kuka_setup_vrSyncPlugin.py +++ b/examples/pybullet/examples/vr_kuka_setup_vrSyncPlugin.py @@ -95,8 +95,32 @@ p.setGravity(0,0,-10) p.setRealTimeSimulation(1) -plugin = p.loadPlugin("e:/develop/bullet3/bin/pybullet_vrSyncPlugin_vs2010_x64_release.dll") -controllerId = 3 + + + +CONTROLLER_ID = 0 +POSITION = 1 +ORIENTATION = 2 +BUTTONS = 6 + +controllerId = -1 + +print("waiting for VR controller trigger") +while (controllerId<0): + events = p.getVREvents() + for e in (events): + if (e[BUTTONS][33]==p.VR_BUTTON_IS_DOWN): + controllerId = e[CONTROLLER_ID] + if (e[BUTTONS][32]==p.VR_BUTTON_IS_DOWN): + controllerId = e[CONTROLLER_ID] + +print("Using controllerId="+str(controllerId)) + + +plugin = p.loadPlugin("d:/develop/bullet3/bin/pybullet_vrSyncPlugin_vs2010_x64_release.dll","_vrSyncPlugin") +#plugin = p.loadPlugin("vrSyncPlugin") +print("PluginId="+str(plugin)) + p.executePluginCommand(plugin ,"bla", [controllerId,pr2_cid, pr2_cid2,pr2_gripper],[50,3]) while (1): diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index b332ad650..b8a8c30d4 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -6681,13 +6681,15 @@ static PyObject* pybullet_loadPlugin(PyObject* self, int physicsClientId = 0; char* pluginPath = 0; + char* postFix = 0; + b3SharedMemoryCommandHandle command = 0; b3SharedMemoryStatusHandle statusHandle = 0; int statusType = -1; b3PhysicsClientHandle sm = 0; - static char* kwlist[] = { "pluginPath", "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &pluginPath, &physicsClientId)) + static char* kwlist[] = { "pluginPath", "postFix", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|si", kwlist, &pluginPath, &postFix, &physicsClientId)) { return NULL; } @@ -6701,6 +6703,10 @@ static PyObject* pybullet_loadPlugin(PyObject* self, command = b3CreateCustomCommand(sm); b3CustomCommandLoadPlugin(command, pluginPath); + if (postFix) + { + b3CustomCommandLoadPluginSetPostFix(command, postFix); + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusPluginUniqueId(statusHandle); return PyInt_FromLong(statusType); diff --git a/examples/pybullet/unity3d/autogen/NativeMethods.cs b/examples/pybullet/unity3d/autogen/NativeMethods.cs index 29ec9f4f0..60511833b 100644 --- a/examples/pybullet/unity3d/autogen/NativeMethods.cs +++ b/examples/pybullet/unity3d/autogen/NativeMethods.cs @@ -425,13 +425,13 @@ public enum b3JointInfoFlags { [System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential)] public struct b3JointInfo { - - /// char* - [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)] + + /// char[1024] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValTStr, SizeConst = 1024)] public string m_linkName; - - /// char* - [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)] + + /// char[1024] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValTStr, SizeConst = 1024)] public string m_jointName; /// int @@ -531,13 +531,13 @@ public struct b3UserConstraint { [System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential)] public struct b3BodyInfo { - - /// char* - [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)] + + /// char[1024] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValTStr, SizeConst = 1024)] public string m_baseName; - - /// char* - [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)] + + /// char[1024] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValTStr, SizeConst = 1024)] public string m_bodyName; } @@ -624,8 +624,7 @@ public struct b3CameraImageData { public int m_pixelHeight; /// char* - [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)] - public string m_rgbColorData; + public System.IntPtr m_rgbColorData; /// float* public System.IntPtr m_depthValues; @@ -1444,7 +1443,7 @@ public static extern int b3GetBodyUniqueId(IntPtr physClient, int serialIndex) ///bodyUniqueId: int ///info: b3BodyInfo* [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010_x64_release.dll", EntryPoint="b3GetBodyInfo")] -public static extern int b3GetBodyInfo(IntPtr physClient, int bodyUniqueId, ref b3BodyInfo info) ; + public static extern int b3GetBodyInfo(IntPtr physClient, int bodyUniqueId, ref b3BodyInfo info) ; /// Return Type: int diff --git a/examples/pybullet/unity3d/examples/NewBehaviourScript.cs b/examples/pybullet/unity3d/examples/NewBehaviourScript.cs index 42bcfddf0..6007203da 100644 --- a/examples/pybullet/unity3d/examples/NewBehaviourScript.cs +++ b/examples/pybullet/unity3d/examples/NewBehaviourScript.cs @@ -10,29 +10,7 @@ using System; public class NewBehaviourScript : MonoBehaviour { - - - - [DllImport("TestCppPlug.dll")] - static extern int Add(int a, int b); - - [DllImport("TestCppPlug.dll")] - static extern int CallMe(Action action); - - [DllImport("TestCppPlug.dll")] - static extern IntPtr CreateSharedAPI(int id); - - [DllImport("TestCppPlug.dll")] - static extern int GetMyIdPlusTen(IntPtr api); - - [DllImport("TestCppPlug.dll")] - static extern void DeleteSharedAPI(IntPtr api); - - private void IWasCalled(int value) - { - text.text = value.ToString(); - } - + Text text; IntPtr sharedAPI; IntPtr pybullet; @@ -40,48 +18,76 @@ public class NewBehaviourScript : MonoBehaviour { // Use this for initialization void Start () { text = GetComponent(); - CallMe(IWasCalled); - sharedAPI = CreateSharedAPI(30); - pybullet = NativeMethods.b3ConnectPhysicsDirect();// SharedMemory(12347); + + + pybullet = NativeMethods.b3ConnectPhysicsDirect();// NativeMethods.b3ConnectSharedMemory(NativeConstants.SHARED_MEMORY_KEY); IntPtr cmd = NativeMethods.b3InitResetSimulationCommand(pybullet); IntPtr status = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd); int numBodies = NativeMethods.b3GetNumBodies(pybullet); cmd = NativeMethods.b3LoadUrdfCommandInit(pybullet, "plane.urdf"); status = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd); EnumSharedMemoryServerStatus statusType = (EnumSharedMemoryServerStatus)NativeMethods.b3GetStatusType(status); - if (statusType == EnumSharedMemoryServerStatus.CMD_URDF_LOADING_COMPLETED) + if (statusType == (EnumSharedMemoryServerStatus)EnumSharedMemoryServerStatus.CMD_URDF_LOADING_COMPLETED) { numBodies = NativeMethods.b3GetNumBodies(pybullet); text.text = numBodies.ToString(); + cmd = NativeMethods.b3InitRequestVisualShapeInformation(pybullet, 0); + status = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd); + statusType = (EnumSharedMemoryServerStatus) NativeMethods.b3GetStatusType(status); + + if (statusType == (EnumSharedMemoryServerStatus)EnumSharedMemoryServerStatus.CMD_VISUAL_SHAPE_INFO_COMPLETED) + { + b3VisualShapeInformation visuals = new b3VisualShapeInformation(); + NativeMethods.b3GetVisualShapeInformation(pybullet, ref visuals); + System.Console.WriteLine("visuals.m_numVisualShapes=" + visuals.m_numVisualShapes); + System.IntPtr visualPtr = visuals.m_visualShapeData; + + for (int s = 0; s < visuals.m_numVisualShapes; s++) + { + b3VisualShapeData visual = (b3VisualShapeData)Marshal.PtrToStructure(visualPtr, typeof(b3VisualShapeData)); + visualPtr = new IntPtr(visualPtr.ToInt64()+(Marshal.SizeOf(typeof(b3VisualShapeData)))); + double x = visual.m_dimensions[0]; + double y = visual.m_dimensions[1]; + double z = visual.m_dimensions[2]; + System.Console.WriteLine("visual.m_visualGeometryType = " + visual.m_visualGeometryType); + System.Console.WriteLine("visual.m_dimensions" + x + "," + y + "," + z); + if (visual.m_visualGeometryType == (int)eUrdfGeomTypes.GEOM_MESH) + { + System.Console.WriteLine("visual.m_meshAssetFileName=" + visual.m_meshAssetFileName); + text.text = visual.m_meshAssetFileName; + } + } + } if (numBodies > 0) { - //b3BodyInfo info=new b3BodyInfo(); - //NativeMethods.b3GetBodyInfo(pybullet, 0, ref info ); - //text.text = info.m_baseName; + b3BodyInfo info=new b3BodyInfo(); + NativeMethods.b3GetBodyInfo(pybullet, 0, ref info ); + + text.text = info.m_baseName; } + + } - //IntPtr clientPtr = b3CreateInProcessPhysicsServerAndConnect(0, ref ptr); } // Update is called once per frame void Update () { - //if (pybullet != IntPtr.Zero) + if (pybullet != IntPtr.Zero) { IntPtr cmd = NativeMethods.b3InitStepSimulationCommand(pybullet); IntPtr status = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd); - - //text.text = System.IO.Directory.GetCurrentDirectory().ToString(); - //text.text = Add(4, 5).ToString(); - //text.text = UnityEngine.Random.Range(0f, 1f).ToString();// GetMyIdPlusTen(sharedAPI).ToString(); } } void OnDestroy() { - NativeMethods.b3DisconnectSharedMemory(pybullet); + if (pybullet != IntPtr.Zero) + { + NativeMethods.b3DisconnectSharedMemory(pybullet); + } } }