diff --git a/AUTHORS.txt b/AUTHORS.txt index 556e6f641..1bff63207 100644 --- a/AUTHORS.txt +++ b/AUTHORS.txt @@ -2,17 +2,21 @@ Bullet Physics is created by Erwin Coumans with contributions from the following AMD Apple +Yunfei Bai Steve Baker Gino van den Bergen +Jeff Bingham Nicola Candussi Erin Catto Lawrence Chai Erwin Coumans -Christer Ericson Disney Animation +Benjamin Ellenberger +Christer Ericson Google Dirk Gregorius Marcus Hennix +Jasmine Hsu MBSim Development Team Takahiro Harada Simon Hobbs @@ -20,6 +24,7 @@ John Hsu Ole Kniemeyer Jay Lee Francisco Leon +lunkhound Vsevolod Klementjev Phil Knight John McCutchan @@ -32,9 +37,9 @@ Russel Smith Sony Jakub Stephien Marten Svanfeldt +Jie Tan Pierre Terdiman Steven Thompson Tamas Umenhoffer -Yunfei Bai If your name is missing, please send an email to erwin.coumans@gmail.com or file an issue at http://github.com/bulletphysics/bullet3 diff --git a/CMakeLists.txt b/CMakeLists.txt index 46cd4ff15..f0e62d396 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -466,7 +466,11 @@ configure_file ( ${CMAKE_CURRENT_SOURCE_DIR}/BulletConfig.cmake.in ${CMAKE_CURRENT_BINARY_DIR}/BulletConfig.cmake @ONLY ESCAPE_QUOTES ) -install ( FILES ${CMAKE_CURRENT_SOURCE_DIR}/UseBullet.cmake +OPTION(INSTALL_CMAKE_FILES "Install generated CMake files" ON) + +IF (INSTALL_CMAKE_FILES) + install ( FILES ${CMAKE_CURRENT_SOURCE_DIR}/UseBullet.cmake ${CMAKE_CURRENT_BINARY_DIR}/BulletConfig.cmake DESTINATION ${BULLET_CONFIG_CMAKE_PATH} ) +ENDIF (INSTALL_CMAKE_FILES) diff --git a/Extras/BulletRobotics/CMakeLists.txt b/Extras/BulletRobotics/CMakeLists.txt index a8d238197..3c1b66f53 100644 --- a/Extras/BulletRobotics/CMakeLists.txt +++ b/Extras/BulletRobotics/CMakeLists.txt @@ -32,6 +32,9 @@ SET(BulletRobotics_SRCS ../../examples/SharedMemory/PhysicsDirectC_API.h ../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp ../../examples/SharedMemory/PhysicsServerCommandProcessor.h + ../../examples/SharedMemory/b3PluginManager.cpp + ../../examples/SharedMemory/b3PluginManager.h + ../../examples/SharedMemory/PhysicsClientSharedMemory.cpp ../../examples/SharedMemory/PhysicsClientSharedMemory.h ../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp diff --git a/data/pr2_gripper.urdf b/data/pr2_gripper.urdf index 7d16d986a..4913718ce 100644 --- a/data/pr2_gripper.urdf +++ b/data/pr2_gripper.urdf @@ -24,7 +24,7 @@ - + @@ -83,7 +83,7 @@ - + diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index e21d33d40..01d0b5c18 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -171,6 +171,7 @@ SET(BulletExampleBrowser_SRCS ../SharedMemory/PhysicsServerCommandProcessor.h ../SharedMemory/SharedMemoryCommands.h ../SharedMemory/SharedMemoryPublic.h + ../SharedMemory/b3PluginManager.cpp ../RobotSimulator/b3RobotSimulatorClientAPI.cpp ../RobotSimulator/b3RobotSimulatorClientAPI.h ../BasicDemo/BasicExample.cpp diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index 0ae38bc0f..5aa3b15b5 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -109,6 +109,7 @@ project "App_BulletExampleBrowser" "../SharedMemory/PhysicsLoopBackC_API.h", "../SharedMemory/PhysicsServerCommandProcessor.cpp", "../SharedMemory/PhysicsServerCommandProcessor.h", + "../SharedMemory/b3PluginManager.cpp", "../SharedMemory/TinyRendererVisualShapeConverter.cpp", "../SharedMemory/TinyRendererVisualShapeConverter.h", "../SharedMemory/SharedMemoryCommands.h", diff --git a/examples/OpenGLWindow/GLInstancingRenderer.cpp b/examples/OpenGLWindow/GLInstancingRenderer.cpp index b89309aab..a92500af7 100644 --- a/examples/OpenGLWindow/GLInstancingRenderer.cpp +++ b/examples/OpenGLWindow/GLInstancingRenderer.cpp @@ -971,10 +971,10 @@ int GLInstancingRenderer::registerTexture(const unsigned char* texels, int width void GLInstancingRenderer::replaceTexture(int shapeIndex, int textureId) { - if (shapeIndex >=0 && shapeIndex < m_data->m_textureHandles.size()) + if ((shapeIndex >=0) && (shapeIndex < m_graphicsInstances.size())) { b3GraphicsInstance* gfxObj = m_graphicsInstances[shapeIndex]; - if (textureId>=0) + if (textureId>=0 && textureId < m_data->m_textureHandles.size()) { gfxObj->m_textureIndex = textureId; gfxObj->m_flags |= eGfxHasTexture; @@ -985,7 +985,7 @@ void GLInstancingRenderer::replaceTexture(int shapeIndex, int textureId) void GLInstancingRenderer::updateTexture(int textureIndex, const unsigned char* texels, bool flipPixelsY) { - if (textureIndex>=0) + if ((textureIndex>=0) && (textureIndex < m_data->m_textureHandles.size())) { glActiveTexture(GL_TEXTURE0); b3Assert(glGetError() ==GL_NO_ERROR); @@ -1027,7 +1027,7 @@ void GLInstancingRenderer::activateTexture(int textureIndex) { glActiveTexture(GL_TEXTURE0); - if (textureIndex>=0) + if (textureIndex>=0 && textureIndex < m_data->m_textureHandles.size()) { glBindTexture(GL_TEXTURE_2D,m_data->m_textureHandles[textureIndex].m_glTexture); } else diff --git a/examples/RobotSimulator/CMakeLists.txt b/examples/RobotSimulator/CMakeLists.txt index 7d35a445e..9549709b0 100644 --- a/examples/RobotSimulator/CMakeLists.txt +++ b/examples/RobotSimulator/CMakeLists.txt @@ -42,6 +42,8 @@ SET(RobotSimulator_SRCS ../../examples/SharedMemory/PhysicsDirectC_API.h ../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp ../../examples/SharedMemory/PhysicsServerCommandProcessor.h + ../../examples/SharedMemory/b3PluginManager.cpp + ../../examples/SharedMemory/PhysicsClientSharedMemory.cpp ../../examples/SharedMemory/PhysicsClientSharedMemory.h ../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp index 172a8dfb9..f5556bb60 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp @@ -988,7 +988,8 @@ bool b3RobotSimulatorClientAPI::getBodyJacobian(int bodyUniqueId, int linkIndex, if (b3GetStatusType(statusHandle) == CMD_CALCULATED_JACOBIAN_COMPLETED) { - b3GetStatusJacobian(statusHandle, linearJacobian, angularJacobian); + int dofCount; + b3GetStatusJacobian(statusHandle, &dofCount, linearJacobian, angularJacobian); return true; } return false; diff --git a/examples/RobotSimulator/premake4.lua b/examples/RobotSimulator/premake4.lua index faa6fb281..368fefd1f 100644 --- a/examples/RobotSimulator/premake4.lua +++ b/examples/RobotSimulator/premake4.lua @@ -20,6 +20,8 @@ myfiles = "../../examples/SharedMemory/PhysicsDirectC_API.h", "../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp", "../../examples/SharedMemory/PhysicsServerCommandProcessor.h", + "../../examples/SharedMemory/b3PluginManager.cpp", + "../../examples/SharedMemory/PhysicsClientSharedMemory.cpp", "../../examples/SharedMemory/PhysicsClientSharedMemory.h", "../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp", diff --git a/examples/SharedMemory/CMakeLists.txt b/examples/SharedMemory/CMakeLists.txt index a369eafa9..c479f7db3 100644 --- a/examples/SharedMemory/CMakeLists.txt +++ b/examples/SharedMemory/CMakeLists.txt @@ -1,5 +1,4 @@ - SET(SharedMemory_SRCS IKTrajectoryHelper.cpp IKTrajectoryHelper.h @@ -41,6 +40,7 @@ SET(SharedMemory_SRCS TinyRendererVisualShapeConverter.h SharedMemoryCommands.h SharedMemoryPublic.h + b3PluginManager.cpp ../TinyRenderer/geometry.cpp ../TinyRenderer/model.cpp ../TinyRenderer/tgaimage.cpp diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 46c4ae0be..d8c2fff7f 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -9,7 +9,7 @@ #include "SharedMemoryCommands.h" -b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName) +B3_SHARED_API b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -31,7 +31,7 @@ b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClien return (b3SharedMemoryCommandHandle) command; } -b3SharedMemoryCommandHandle b3SaveWorldCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName) +B3_SHARED_API b3SharedMemoryCommandHandle b3SaveWorldCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -53,7 +53,7 @@ b3SharedMemoryCommandHandle b3SaveWorldCommandInit(b3PhysicsClientHandle physCli return (b3SharedMemoryCommandHandle) command; } -b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName) +B3_SHARED_API b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -80,7 +80,7 @@ b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClie return 0; } -b3SharedMemoryCommandHandle b3LoadBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName) +B3_SHARED_API b3SharedMemoryCommandHandle b3LoadBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName) { PhysicsClient* cl = (PhysicsClient*)physClient; b3Assert(cl); @@ -107,7 +107,7 @@ b3SharedMemoryCommandHandle b3LoadBulletCommandInit(b3PhysicsClientHandle physCl return 0; } -b3SharedMemoryCommandHandle b3SaveBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName) +B3_SHARED_API b3SharedMemoryCommandHandle b3SaveBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName) { PhysicsClient* cl = (PhysicsClient*)physClient; b3Assert(cl); @@ -133,7 +133,7 @@ b3SharedMemoryCommandHandle b3SaveBulletCommandInit(b3PhysicsClientHandle physCl } return 0; } -b3SharedMemoryCommandHandle b3LoadMJCFCommandInit(b3PhysicsClientHandle physClient, const char* fileName) +B3_SHARED_API b3SharedMemoryCommandHandle b3LoadMJCFCommandInit(b3PhysicsClientHandle physClient, const char* fileName) { PhysicsClient* cl = (PhysicsClient*)physClient; b3Assert(cl); @@ -160,7 +160,7 @@ b3SharedMemoryCommandHandle b3LoadMJCFCommandInit(b3PhysicsClientHandle physClie return 0; } -void b3LoadMJCFCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags) +B3_SHARED_API void b3LoadMJCFCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command->m_type == CMD_LOAD_MJCF); @@ -171,7 +171,7 @@ void b3LoadMJCFCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int fl } } -b3SharedMemoryCommandHandle b3LoadBunnyCommandInit(b3PhysicsClientHandle physClient) +B3_SHARED_API b3SharedMemoryCommandHandle b3LoadBunnyCommandInit(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -185,7 +185,7 @@ b3SharedMemoryCommandHandle b3LoadBunnyCommandInit(b3PhysicsClientHandle physCli return (b3SharedMemoryCommandHandle) command; } -int b3LoadBunnySetScale(b3SharedMemoryCommandHandle commandHandle, double scale) +B3_SHARED_API int b3LoadBunnySetScale(b3SharedMemoryCommandHandle commandHandle, double scale) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command->m_type == CMD_LOAD_BUNNY); @@ -194,7 +194,7 @@ int b3LoadBunnySetScale(b3SharedMemoryCommandHandle commandHandle, double scale) return 0; } -int b3LoadBunnySetMass(b3SharedMemoryCommandHandle commandHandle, double mass) +B3_SHARED_API int b3LoadBunnySetMass(b3SharedMemoryCommandHandle commandHandle, double mass) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command->m_type == CMD_LOAD_BUNNY); @@ -203,7 +203,7 @@ int b3LoadBunnySetMass(b3SharedMemoryCommandHandle commandHandle, double mass) return 0; } -int b3LoadBunnySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, double collisionMargin) +B3_SHARED_API int b3LoadBunnySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, double collisionMargin) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command->m_type == CMD_LOAD_BUNNY); @@ -212,7 +212,7 @@ int b3LoadBunnySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, dou return 0; } -int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody) +B3_SHARED_API int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -223,7 +223,7 @@ int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, return 0; } -int b3LoadUrdfCommandSetGlobalScaling(b3SharedMemoryCommandHandle commandHandle, double globalScaling) +B3_SHARED_API int b3LoadUrdfCommandSetGlobalScaling(b3SharedMemoryCommandHandle commandHandle, double globalScaling) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -235,7 +235,7 @@ int b3LoadUrdfCommandSetGlobalScaling(b3SharedMemoryCommandHandle commandHandle, -int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody) +B3_SHARED_API int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -246,7 +246,7 @@ int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, i return 0; } -int b3LoadSdfCommandSetUseGlobalScaling(b3SharedMemoryCommandHandle commandHandle, double globalScaling) +B3_SHARED_API int b3LoadSdfCommandSetUseGlobalScaling(b3SharedMemoryCommandHandle commandHandle, double globalScaling) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -259,7 +259,7 @@ int b3LoadSdfCommandSetUseGlobalScaling(b3SharedMemoryCommandHandle commandHandl -int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase) +B3_SHARED_API int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -273,7 +273,7 @@ int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, return -1; } -int b3LoadUrdfCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags) +B3_SHARED_API int b3LoadUrdfCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -286,7 +286,7 @@ int b3LoadUrdfCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int fla return 0; } -int b3LoadUrdfCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ) +B3_SHARED_API int b3LoadUrdfCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -305,7 +305,7 @@ int b3LoadUrdfCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, return -1; } -int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW) +B3_SHARED_API int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -325,7 +325,7 @@ int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHand return -1; } -b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient) +B3_SHARED_API b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -337,7 +337,7 @@ b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle return (b3SharedMemoryCommandHandle) command; } -int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz) +B3_SHARED_API int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); @@ -348,7 +348,7 @@ int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, doub return 0; } -int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation) +B3_SHARED_API int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); @@ -357,7 +357,7 @@ int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandH return 0; } -int b3PhysicsParamSetInternalSimFlags(b3SharedMemoryCommandHandle commandHandle, int flags) +B3_SHARED_API int b3PhysicsParamSetInternalSimFlags(b3SharedMemoryCommandHandle commandHandle, int flags) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); @@ -366,7 +366,7 @@ int b3PhysicsParamSetInternalSimFlags(b3SharedMemoryCommandHandle commandHan return 0; } -int b3PhysicsParamSetUseSplitImpulse(b3SharedMemoryCommandHandle commandHandle, int useSplitImpulse) +B3_SHARED_API int b3PhysicsParamSetUseSplitImpulse(b3SharedMemoryCommandHandle commandHandle, int useSplitImpulse) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); @@ -376,7 +376,7 @@ int b3PhysicsParamSetUseSplitImpulse(b3SharedMemoryCommandHandle commandHandle, return 0; } -int b3PhysicsParamSetSplitImpulsePenetrationThreshold(b3SharedMemoryCommandHandle commandHandle, double splitImpulsePenetrationThreshold) +B3_SHARED_API int b3PhysicsParamSetSplitImpulsePenetrationThreshold(b3SharedMemoryCommandHandle commandHandle, double splitImpulsePenetrationThreshold) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); @@ -386,7 +386,7 @@ int b3PhysicsParamSetSplitImpulsePenetrationThreshold(b3SharedMemoryCommandHandl return 0; } -int b3PhysicsParamSetContactBreakingThreshold(b3SharedMemoryCommandHandle commandHandle, double contactBreakingThreshold) +B3_SHARED_API int b3PhysicsParamSetContactBreakingThreshold(b3SharedMemoryCommandHandle commandHandle, double contactBreakingThreshold) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); @@ -395,7 +395,7 @@ int b3PhysicsParamSetContactBreakingThreshold(b3SharedMemoryCommandHandle comman command->m_updateFlags |= SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD; return 0; } -int b3PhysicsParamSetMaxNumCommandsPer1ms(b3SharedMemoryCommandHandle commandHandle, int maxNumCmdPer1ms) +B3_SHARED_API int b3PhysicsParamSetMaxNumCommandsPer1ms(b3SharedMemoryCommandHandle commandHandle, int maxNumCmdPer1ms) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); @@ -406,7 +406,7 @@ int b3PhysicsParamSetMaxNumCommandsPer1ms(b3SharedMemoryCommandHandle commandHan } -int b3PhysicsParamSetEnableFileCaching(b3SharedMemoryCommandHandle commandHandle, int enableFileCaching) +B3_SHARED_API int b3PhysicsParamSetEnableFileCaching(b3SharedMemoryCommandHandle commandHandle, int enableFileCaching) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); @@ -417,7 +417,7 @@ int b3PhysicsParamSetEnableFileCaching(b3SharedMemoryCommandHandle commandHandle } -int b3PhysicsParamSetRestitutionVelocityThreshold(b3SharedMemoryCommandHandle commandHandle, double restitutionVelocityThreshold) +B3_SHARED_API int b3PhysicsParamSetRestitutionVelocityThreshold(b3SharedMemoryCommandHandle commandHandle, double restitutionVelocityThreshold) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); @@ -429,7 +429,7 @@ int b3PhysicsParamSetRestitutionVelocityThreshold(b3SharedMemoryCommandHandle co } -int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations) +B3_SHARED_API int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); @@ -439,7 +439,7 @@ int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHand } -int b3PhysicsParamSetCollisionFilterMode(b3SharedMemoryCommandHandle commandHandle, int filterMode) +B3_SHARED_API int b3PhysicsParamSetCollisionFilterMode(b3SharedMemoryCommandHandle commandHandle, int filterMode) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); @@ -450,7 +450,7 @@ int b3PhysicsParamSetCollisionFilterMode(b3SharedMemoryCommandHandle commandHand -int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep) +B3_SHARED_API int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); @@ -459,7 +459,7 @@ int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double return 0; } -int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps) +B3_SHARED_API int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); @@ -469,7 +469,7 @@ int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int } -int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP) +B3_SHARED_API int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); @@ -477,7 +477,7 @@ int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle command->m_physSimParamArgs.m_defaultContactERP = defaultContactERP; return 0; } -int b3PhysicsParamSetDefaultNonContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultNonContactERP) +B3_SHARED_API int b3PhysicsParamSetDefaultNonContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultNonContactERP) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); @@ -485,7 +485,7 @@ int b3PhysicsParamSetDefaultNonContactERP(b3SharedMemoryCommandHandle commandHan command->m_physSimParamArgs.m_defaultNonContactERP = defaultNonContactERP; return 0; } -int b3PhysicsParamSetDefaultFrictionERP(b3SharedMemoryCommandHandle commandHandle, double frictionERP) +B3_SHARED_API int b3PhysicsParamSetDefaultFrictionERP(b3SharedMemoryCommandHandle commandHandle, double frictionERP) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); @@ -495,7 +495,7 @@ int b3PhysicsParamSetDefaultFrictionERP(b3SharedMemoryCommandHandle commandHandl } -b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient) +B3_SHARED_API b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -507,7 +507,7 @@ b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle ph return (b3SharedMemoryCommandHandle) command; } -b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHandle physClient) +B3_SHARED_API b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -522,12 +522,12 @@ b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHand } -b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode) +B3_SHARED_API b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode) { return b3JointControlCommandInit2(physClient,0,controlMode); } -b3SharedMemoryCommandHandle b3JointControlCommandInit2( b3PhysicsClientHandle physClient, int bodyUniqueId, int controlMode) +B3_SHARED_API b3SharedMemoryCommandHandle b3JointControlCommandInit2( b3PhysicsClientHandle physClient, int bodyUniqueId, int controlMode) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -545,7 +545,7 @@ b3SharedMemoryCommandHandle b3JointControlCommandInit2( b3PhysicsClientHandle ph return (b3SharedMemoryCommandHandle) command; } -int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int qIndex, double value) +B3_SHARED_API int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int qIndex, double value) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -558,7 +558,7 @@ int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, return 0; } -int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value) +B3_SHARED_API int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -571,7 +571,7 @@ int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, return 0; } -int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value) +B3_SHARED_API int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -584,7 +584,7 @@ int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, return 0; } -int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value) +B3_SHARED_API int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -598,7 +598,7 @@ int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, } -int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value) +B3_SHARED_API int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -611,7 +611,7 @@ int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int return 0; } -int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value) +B3_SHARED_API int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -624,7 +624,7 @@ int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandl return 0; } -b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId) +B3_SHARED_API b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -637,7 +637,7 @@ b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandl return (b3SharedMemoryCommandHandle) command; } -int b3RequestActualStateCommandComputeLinkVelocity(b3SharedMemoryCommandHandle commandHandle, int computeLinkVelocity) +B3_SHARED_API int b3RequestActualStateCommandComputeLinkVelocity(b3SharedMemoryCommandHandle commandHandle, int computeLinkVelocity) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -649,8 +649,20 @@ int b3RequestActualStateCommandComputeLinkVelocity(b3SharedMemoryCommandHandle c return 0; } +B3_SHARED_API int b3RequestActualStateCommandComputeForwardKinematics(b3SharedMemoryCommandHandle commandHandle, int computeForwardKinematics) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + btAssert(command->m_type == CMD_REQUEST_ACTUAL_STATE); + if (computeForwardKinematics && command->m_type == CMD_REQUEST_ACTUAL_STATE) + { + command->m_updateFlags |= ACTUAL_STATE_COMPUTE_FORWARD_KINEMATICS; + } + return 0; +} -int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, b3JointSensorState *state) + +B3_SHARED_API int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, b3JointSensorState *state) { const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; b3Assert(status); @@ -683,7 +695,7 @@ int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle return 0; } -int b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int linkIndex, b3LinkState *state) +B3_SHARED_API int b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int linkIndex, b3LinkState *state) { const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; b3Assert(status); @@ -730,7 +742,7 @@ int b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle return 0; } -b3SharedMemoryCommandHandle b3CreateCollisionShapeCommandInit(b3PhysicsClientHandle physClient) +B3_SHARED_API b3SharedMemoryCommandHandle b3CreateCollisionShapeCommandInit(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -747,7 +759,7 @@ b3SharedMemoryCommandHandle b3CreateCollisionShapeCommandInit(b3PhysicsClientHan return 0; } -int b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,double radius) +B3_SHARED_API int b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,double radius) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -768,7 +780,7 @@ int b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,do return -1; } -int b3CreateCollisionShapeAddBox(b3SharedMemoryCommandHandle commandHandle,double halfExtents[3]) +B3_SHARED_API int b3CreateCollisionShapeAddBox(b3SharedMemoryCommandHandle commandHandle,double halfExtents[3]) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -791,7 +803,7 @@ int b3CreateCollisionShapeAddBox(b3SharedMemoryCommandHandle commandHandle,doubl return -1; } -int b3CreateCollisionShapeAddCapsule(b3SharedMemoryCommandHandle commandHandle,double radius, double height) +B3_SHARED_API int b3CreateCollisionShapeAddCapsule(b3SharedMemoryCommandHandle commandHandle,double radius, double height) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -813,7 +825,7 @@ int b3CreateCollisionShapeAddCapsule(b3SharedMemoryCommandHandle commandHandle,d return -1; } -int b3CreateCollisionShapeAddCylinder(b3SharedMemoryCommandHandle commandHandle,double radius, double height) +B3_SHARED_API int b3CreateCollisionShapeAddCylinder(b3SharedMemoryCommandHandle commandHandle,double radius, double height) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -836,7 +848,7 @@ int b3CreateCollisionShapeAddCylinder(b3SharedMemoryCommandHandle commandHandle, } -int b3CreateCollisionShapeAddPlane(b3SharedMemoryCommandHandle commandHandle, double planeNormal[3], double planeConstant) +B3_SHARED_API int b3CreateCollisionShapeAddPlane(b3SharedMemoryCommandHandle commandHandle, double planeNormal[3], double planeConstant) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -860,7 +872,7 @@ int b3CreateCollisionShapeAddPlane(b3SharedMemoryCommandHandle commandHandle, do return -1; } -int b3CreateCollisionShapeAddMesh(b3SharedMemoryCommandHandle commandHandle,const char* fileName, double meshScale[3]) +B3_SHARED_API int b3CreateCollisionShapeAddMesh(b3SharedMemoryCommandHandle commandHandle,const char* fileName, double meshScale[3]) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -885,7 +897,7 @@ int b3CreateCollisionShapeAddMesh(b3SharedMemoryCommandHandle commandHandle,cons return -1; } -void b3CreateCollisionSetFlag(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, int flags) +B3_SHARED_API void b3CreateCollisionSetFlag(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, int flags) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; @@ -901,7 +913,7 @@ void b3CreateCollisionSetFlag(b3SharedMemoryCommandHandle commandHandle,int shap } -void b3CreateCollisionShapeSetChildTransform(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, double childPosition[3], double childOrientation[4]) +B3_SHARED_API void b3CreateCollisionShapeSetChildTransform(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, double childPosition[3], double childOrientation[4]) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -923,7 +935,7 @@ void b3CreateCollisionShapeSetChildTransform(b3SharedMemoryCommandHandle command } -int b3GetStatusCollisionShapeUniqueId(b3SharedMemoryStatusHandle statusHandle) +B3_SHARED_API int b3GetStatusCollisionShapeUniqueId(b3SharedMemoryStatusHandle statusHandle) { const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; b3Assert(status); @@ -936,7 +948,7 @@ int b3GetStatusCollisionShapeUniqueId(b3SharedMemoryStatusHandle statusHandle) } -b3SharedMemoryCommandHandle b3CreateVisualShapeCommandInit(b3PhysicsClientHandle physClient) +B3_SHARED_API b3SharedMemoryCommandHandle b3CreateVisualShapeCommandInit(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -952,7 +964,7 @@ b3SharedMemoryCommandHandle b3CreateVisualShapeCommandInit(b3PhysicsClientHandle return 0; } -int b3GetStatusVisualShapeUniqueId(b3SharedMemoryStatusHandle statusHandle) +B3_SHARED_API int b3GetStatusVisualShapeUniqueId(b3SharedMemoryStatusHandle statusHandle) { const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; b3Assert(status); @@ -964,7 +976,7 @@ int b3GetStatusVisualShapeUniqueId(b3SharedMemoryStatusHandle statusHandle) return -1; } -b3SharedMemoryCommandHandle b3CreateMultiBodyCommandInit(b3PhysicsClientHandle physClient) +B3_SHARED_API b3SharedMemoryCommandHandle b3CreateMultiBodyCommandInit(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -983,7 +995,7 @@ b3SharedMemoryCommandHandle b3CreateMultiBodyCommandInit(b3PhysicsClientHandle p return 0; } -int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass, int collisionShapeUnique, int visualShapeUniqueId, double basePosition[3], double baseOrientation[4] , double baseInertialFramePosition[3], double baseInertialFrameOrientation[4]) +B3_SHARED_API int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass, int collisionShapeUnique, int visualShapeUniqueId, double basePosition[3], double baseOrientation[4] , double baseInertialFramePosition[3], double baseInertialFrameOrientation[4]) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -1036,7 +1048,7 @@ int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass return -2; } -int b3CreateMultiBodyLink(b3SharedMemoryCommandHandle commandHandle, double linkMass, double linkCollisionShapeIndex, +B3_SHARED_API int b3CreateMultiBodyLink(b3SharedMemoryCommandHandle commandHandle, double linkMass, double linkCollisionShapeIndex, double linkVisualShapeIndex, double linkPosition[3], double linkOrientation[4], @@ -1102,7 +1114,7 @@ int b3CreateMultiBodyLink(b3SharedMemoryCommandHandle commandHandle, double link //useMaximalCoordinates are disabled by default, enabling them is experimental and not fully supported yet -void b3CreateMultiBodyUseMaximalCoordinates(b3SharedMemoryCommandHandle commandHandle) +B3_SHARED_API void b3CreateMultiBodyUseMaximalCoordinates(b3SharedMemoryCommandHandle commandHandle) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -1113,7 +1125,7 @@ void b3CreateMultiBodyUseMaximalCoordinates(b3SharedMemoryCommandHandle commandH } } -int b3GetStatusMultiBodyUniqueId(b3SharedMemoryStatusHandle statusHandle) +B3_SHARED_API int b3GetStatusMultiBodyUniqueId(b3SharedMemoryStatusHandle statusHandle) { const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; b3Assert(status); @@ -1125,7 +1137,7 @@ int b3GetStatusMultiBodyUniqueId(b3SharedMemoryStatusHandle statusHandle) return -1; } -b3SharedMemoryCommandHandle b3CreateBoxShapeCommandInit(b3PhysicsClientHandle physClient) +B3_SHARED_API b3SharedMemoryCommandHandle b3CreateBoxShapeCommandInit(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -1137,7 +1149,7 @@ b3SharedMemoryCommandHandle b3CreateBoxShapeCommandInit(b3PhysicsClientHandle ph return (b3SharedMemoryCommandHandle) command; } -int b3CreateBoxCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ) +B3_SHARED_API int b3CreateBoxCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -1151,7 +1163,7 @@ int b3CreateBoxCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle } -int b3CreateBoxCommandSetHalfExtents(b3SharedMemoryCommandHandle commandHandle, double halfExtentsX,double halfExtentsY,double halfExtentsZ) +B3_SHARED_API int b3CreateBoxCommandSetHalfExtents(b3SharedMemoryCommandHandle commandHandle, double halfExtentsX,double halfExtentsY,double halfExtentsZ) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -1166,7 +1178,7 @@ int b3CreateBoxCommandSetHalfExtents(b3SharedMemoryCommandHandle commandHandle, } -int b3CreateBoxCommandSetMass(b3SharedMemoryCommandHandle commandHandle, double mass) +B3_SHARED_API int b3CreateBoxCommandSetMass(b3SharedMemoryCommandHandle commandHandle, double mass) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -1177,7 +1189,7 @@ int b3CreateBoxCommandSetMass(b3SharedMemoryCommandHandle commandHandle, double } -int b3CreateBoxCommandSetCollisionShapeType(b3SharedMemoryCommandHandle commandHandle, int collisionShapeType) +B3_SHARED_API int b3CreateBoxCommandSetCollisionShapeType(b3SharedMemoryCommandHandle commandHandle, int collisionShapeType) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -1188,7 +1200,7 @@ int b3CreateBoxCommandSetCollisionShapeType(b3SharedMemoryCommandHandle commandH return 0; } -int b3CreateBoxCommandSetColorRGBA(b3SharedMemoryCommandHandle commandHandle, double red,double green,double blue, double alpha) +B3_SHARED_API int b3CreateBoxCommandSetColorRGBA(b3SharedMemoryCommandHandle commandHandle, double red,double green,double blue, double alpha) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -1201,7 +1213,7 @@ int b3CreateBoxCommandSetColorRGBA(b3SharedMemoryCommandHandle commandHandle, do return 0; } -int b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW) +B3_SHARED_API int b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -1215,7 +1227,7 @@ int b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHan return 0; } -b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex) +B3_SHARED_API b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex) { PhysicsClient* cl = (PhysicsClient* ) physClient; @@ -1235,7 +1247,7 @@ b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physCl return (b3SharedMemoryCommandHandle) command; } -int b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ) +B3_SHARED_API int b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -1252,7 +1264,7 @@ int b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle return 0; } -int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW) +B3_SHARED_API int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -1271,7 +1283,7 @@ int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHan return 0; } -int b3CreatePoseCommandSetBaseLinearVelocity(b3SharedMemoryCommandHandle commandHandle, double linVel[3]) +B3_SHARED_API int b3CreatePoseCommandSetBaseLinearVelocity(b3SharedMemoryCommandHandle commandHandle, double linVel[3]) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -1288,7 +1300,7 @@ int b3CreatePoseCommandSetBaseLinearVelocity(b3SharedMemoryCommandHandle command return 0; } -int b3CreatePoseCommandSetBaseAngularVelocity(b3SharedMemoryCommandHandle commandHandle, double angVel[3]) +B3_SHARED_API int b3CreatePoseCommandSetBaseAngularVelocity(b3SharedMemoryCommandHandle commandHandle, double angVel[3]) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -1305,7 +1317,7 @@ int b3CreatePoseCommandSetBaseAngularVelocity(b3SharedMemoryCommandHandle comman return 0; } -int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* jointPositions) +B3_SHARED_API int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* jointPositions) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -1324,7 +1336,7 @@ int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHand -int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition) +B3_SHARED_API int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -1332,7 +1344,7 @@ int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3Shar command->m_updateFlags |=INIT_POSE_HAS_JOINT_STATE; b3JointInfo info; b3GetJointInfo(physClient, command->m_initPoseArgs.m_bodyUniqueId,jointIndex, &info); - btAssert((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && info.m_qIndex >=0); + //btAssert((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && info.m_qIndex >=0); if ((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && info.m_qIndex >=0) { command->m_initPoseArgs.m_initialStateQ[info.m_qIndex] = jointPosition; @@ -1341,7 +1353,7 @@ int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3Shar return 0; } -int b3CreatePoseCommandSetJointVelocities(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int numJointVelocities, const double* jointVelocities) +B3_SHARED_API int b3CreatePoseCommandSetJointVelocities(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int numJointVelocities, const double* jointVelocities) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -1359,7 +1371,7 @@ int b3CreatePoseCommandSetJointVelocities(b3PhysicsClientHandle physClient, b3Sh return 0; } -int b3CreatePoseCommandSetJointVelocity(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointVelocity) +B3_SHARED_API int b3CreatePoseCommandSetJointVelocity(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointVelocity) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -1367,7 +1379,7 @@ int b3CreatePoseCommandSetJointVelocity(b3PhysicsClientHandle physClient, b3Shar command->m_updateFlags |=INIT_POSE_HAS_JOINT_VELOCITY; b3JointInfo info; b3GetJointInfo(physClient, command->m_initPoseArgs.m_bodyUniqueId,jointIndex, &info); - btAssert((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && info.m_uIndex >=0); + //btAssert((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && info.m_uIndex >=0); if ((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && (info.m_uIndex >=0) && (info.m_uIndexm_initPoseArgs.m_initialStateQdot[info.m_uIndex] = jointVelocity; @@ -1378,7 +1390,7 @@ int b3CreatePoseCommandSetJointVelocity(b3PhysicsClientHandle physClient, b3Shar -b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId) +B3_SHARED_API b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -1394,7 +1406,7 @@ b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle phys } -int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle commandHandle, int jointIndex, int enable) +B3_SHARED_API int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle commandHandle, int jointIndex, int enable) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -1408,7 +1420,7 @@ int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle c return 0; } -int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, int linkIndex, int enable) +B3_SHARED_API int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, int linkIndex, int enable) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -1423,14 +1435,14 @@ int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, in -void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient) +B3_SHARED_API void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; cl->disconnectSharedMemory(); delete cl; } -b3SharedMemoryStatusHandle b3ProcessServerStatus(b3PhysicsClientHandle physClient) +B3_SHARED_API b3SharedMemoryStatusHandle b3ProcessServerStatus(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; if (cl && cl->isConnected()) @@ -1444,7 +1456,7 @@ b3SharedMemoryStatusHandle b3ProcessServerStatus(b3PhysicsClientHandle physClien -int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle) +B3_SHARED_API int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle) { const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; //b3Assert(status); @@ -1455,7 +1467,7 @@ int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle) return CMD_INVALID_STATUS; } -int b3GetStatusBodyIndices(b3SharedMemoryStatusHandle statusHandle, int* bodyIndicesOut, int bodyIndicesCapacity) +B3_SHARED_API int b3GetStatusBodyIndices(b3SharedMemoryStatusHandle statusHandle, int* bodyIndicesOut, int bodyIndicesCapacity) { int numBodies = 0; const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; @@ -1484,7 +1496,7 @@ int b3GetStatusBodyIndices(b3SharedMemoryStatusHandle statusHandle, int* bodyInd return numBodies; } -int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle) +B3_SHARED_API int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle) { const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; b3Assert(status); @@ -1518,7 +1530,7 @@ int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle) return bodyId; } -b3SharedMemoryCommandHandle b3RequestCollisionInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId) +B3_SHARED_API b3SharedMemoryCommandHandle b3RequestCollisionInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -1531,7 +1543,7 @@ b3SharedMemoryCommandHandle b3RequestCollisionInfoCommandInit(b3PhysicsClientHan return (b3SharedMemoryCommandHandle) command; } -int b3GetStatusAABB(b3SharedMemoryStatusHandle statusHandle, int linkIndex, double aabbMin[3], double aabbMax[3]) +B3_SHARED_API int b3GetStatusAABB(b3SharedMemoryStatusHandle statusHandle, int linkIndex, double aabbMin[3], double aabbMax[3]) { const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; const b3SendCollisionInfoArgs &args = status->m_sendCollisionInfoArgs; @@ -1566,7 +1578,7 @@ int b3GetStatusAABB(b3SharedMemoryStatusHandle statusHandle, int linkIndex, doub return 0; } -int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle, +B3_SHARED_API int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle, int* bodyUniqueId, int* numDegreeOfFreedomQ, int* numDegreeOfFreedomU, @@ -1604,7 +1616,7 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle, return true; } -int b3CanSubmitCommand(b3PhysicsClientHandle physClient) +B3_SHARED_API int b3CanSubmitCommand(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; if (cl) @@ -1614,7 +1626,7 @@ int b3CanSubmitCommand(b3PhysicsClientHandle physClient) return false; } -int b3SubmitClientCommand(b3PhysicsClientHandle physClient, const b3SharedMemoryCommandHandle commandHandle) +B3_SHARED_API int b3SubmitClientCommand(b3PhysicsClientHandle physClient, const b3SharedMemoryCommandHandle commandHandle) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; PhysicsClient* cl = (PhysicsClient* ) physClient; @@ -1631,7 +1643,8 @@ int b3SubmitClientCommand(b3PhysicsClientHandle physClient, const b3SharedMemory #include "../Utils/b3Clock.h" -b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHandle physClient, const b3SharedMemoryCommandHandle commandHandle) + +B3_SHARED_API b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHandle physClient, const b3SharedMemoryCommandHandle commandHandle) { B3_PROFILE("b3SubmitClientCommandAndWaitStatus"); b3Clock clock; @@ -1667,53 +1680,53 @@ b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHan ///return the total number of bodies in the simulation -int b3GetNumBodies(b3PhysicsClientHandle physClient) +B3_SHARED_API int b3GetNumBodies(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; return cl->getNumBodies(); } -int b3GetNumUserConstraints(b3PhysicsClientHandle physClient) +B3_SHARED_API int b3GetNumUserConstraints(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; return cl->getNumUserConstraints(); } -int b3GetUserConstraintInfo(b3PhysicsClientHandle physClient, int constraintUniqueId, struct b3UserConstraint* infoPtr) +B3_SHARED_API int b3GetUserConstraintInfo(b3PhysicsClientHandle physClient, int constraintUniqueId, struct b3UserConstraint* info) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3UserConstraint constraintInfo1; b3Assert(physClient); - b3Assert(infoPtr); + b3Assert(info); b3Assert(constraintUniqueId>=0); - if (infoPtr==0) + if (info==0) return 0; if (cl->getUserConstraintInfo(constraintUniqueId, constraintInfo1)) { - *infoPtr = constraintInfo1; + *info = constraintInfo1; return 1; } return 0; } /// return the user constraint id, given the index in range [0 , b3GetNumUserConstraints() ) -int b3GetUserConstraintId(b3PhysicsClientHandle physClient, int serialIndex) +B3_SHARED_API int b3GetUserConstraintId(b3PhysicsClientHandle physClient, int serialIndex) { PhysicsClient* cl = (PhysicsClient* ) physClient; return cl->getUserConstraintId(serialIndex); } /// return the body unique id, given the index in range [0 , b3GetNumBodies() ) -int b3GetBodyUniqueId(b3PhysicsClientHandle physClient, int serialIndex) +B3_SHARED_API int b3GetBodyUniqueId(b3PhysicsClientHandle physClient, int serialIndex) { PhysicsClient* cl = (PhysicsClient* ) physClient; return cl->getBodyUniqueId(serialIndex); } ///given a body unique id, return the body information. See b3BodyInfo in SharedMemoryPublic.h -int b3GetBodyInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, struct b3BodyInfo* info) +B3_SHARED_API int b3GetBodyInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, struct b3BodyInfo* info) { PhysicsClient* cl = (PhysicsClient* ) physClient; return cl->getBodyInfo(bodyUniqueId,*info); @@ -1721,19 +1734,150 @@ int b3GetBodyInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, struct b3B -int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyId) +B3_SHARED_API int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyId) { PhysicsClient* cl = (PhysicsClient* ) physClient; return cl->getNumJoints(bodyId); } -int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info) +B3_SHARED_API int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info) { PhysicsClient* cl = (PhysicsClient* ) physClient; return cl->getJointInfo(bodyIndex, jointIndex, *info); } -b3SharedMemoryCommandHandle b3GetDynamicsInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex) + + +B3_SHARED_API b3SharedMemoryCommandHandle b3CreateCustomCommand(b3PhysicsClientHandle physClient) +{ + PhysicsClient* cl = (PhysicsClient*)physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type = CMD_CUSTOM_COMMAND; + command->m_updateFlags = 0; + return (b3SharedMemoryCommandHandle)command; +} + +B3_SHARED_API void b3CustomCommandLoadPlugin(b3SharedMemoryCommandHandle commandHandle, const char* pluginPath) +{ + 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; + command->m_customCommandArgs.m_pluginPath[0] = 0; + + int len = strlen(pluginPath); + if (lenm_customCommandArgs.m_pluginPath, pluginPath); + } + } +} + + + +B3_SHARED_API int b3GetStatusPluginCommandResult(b3SharedMemoryStatusHandle statusHandle) +{ + int statusUniqueId = -1; + + const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; + b3Assert(status); + b3Assert(status->m_type == CMD_CUSTOM_COMMAND_COMPLETED); + if (status->m_type == CMD_CUSTOM_COMMAND_COMPLETED) + { + statusUniqueId = status->m_customCommandResultArgs.m_executeCommandResult; + } + return statusUniqueId; +} + +B3_SHARED_API int b3GetStatusPluginUniqueId(b3SharedMemoryStatusHandle statusHandle) +{ + int statusUniqueId = -1; + + const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; + b3Assert(status); + b3Assert(status->m_type == CMD_CUSTOM_COMMAND_COMPLETED); + if (status->m_type == CMD_CUSTOM_COMMAND_COMPLETED) + { + statusUniqueId = status->m_customCommandResultArgs.m_pluginUniqueId; + } + return statusUniqueId; +} + +B3_SHARED_API void b3CustomCommandUnloadPlugin(b3SharedMemoryCommandHandle commandHandle, int pluginUniqueId) +{ + + 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_UNLOAD_PLUGIN; + command->m_customCommandArgs.m_pluginUniqueId = pluginUniqueId; + } +} +B3_SHARED_API void b3CustomCommandExecutePluginCommand(b3SharedMemoryCommandHandle commandHandle, int pluginUniqueId, const char* textArguments) +{ + + 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_EXECUTE_PLUGIN_COMMAND; + command->m_customCommandArgs.m_pluginUniqueId = pluginUniqueId; + + 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; + + int len = strlen(textArguments); + + if (lenm_customCommandArgs.m_arguments.m_text, textArguments); + } + } +} + +B3_SHARED_API void b3CustomCommandExecuteAddIntArgument(b3SharedMemoryCommandHandle commandHandle, int intVal) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_CUSTOM_COMMAND); + b3Assert(command->m_updateFlags & CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND); + if (command->m_type == CMD_CUSTOM_COMMAND && (command->m_updateFlags & CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND)) + { + int numInts = command->m_customCommandArgs.m_arguments.m_numInts; + if (numIntsm_customCommandArgs.m_arguments.m_ints[numInts]=intVal; + command->m_customCommandArgs.m_arguments.m_numInts++; + } + } +} + +B3_SHARED_API void b3CustomCommandExecuteAddFloatArgument(b3SharedMemoryCommandHandle commandHandle, float floatVal) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_CUSTOM_COMMAND); + b3Assert(command->m_updateFlags & CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND); + if (command->m_type == CMD_CUSTOM_COMMAND && (command->m_updateFlags & CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND)) + { + int numFloats = command->m_customCommandArgs.m_arguments.m_numFloats; + if (numFloatsm_customCommandArgs.m_arguments.m_floats[numFloats]=floatVal; + command->m_customCommandArgs.m_arguments.m_numFloats++; + } + } +} + + + + + +B3_SHARED_API b3SharedMemoryCommandHandle b3GetDynamicsInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -1746,7 +1890,7 @@ b3SharedMemoryCommandHandle b3GetDynamicsInfoCommandInit(b3PhysicsClientHandle p return (b3SharedMemoryCommandHandle) command; } -int b3GetDynamicsInfo(b3SharedMemoryStatusHandle statusHandle, struct b3DynamicsInfo* info) +B3_SHARED_API int b3GetDynamicsInfo(b3SharedMemoryStatusHandle statusHandle, struct b3DynamicsInfo* info) { const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; const b3DynamicsInfo &dynamicsInfo = status->m_dynamicsInfo; @@ -1759,7 +1903,7 @@ int b3GetDynamicsInfo(b3SharedMemoryStatusHandle statusHandle, struct b3Dynamics return true; } -b3SharedMemoryCommandHandle b3InitChangeDynamicsInfo(b3PhysicsClientHandle physClient) +B3_SHARED_API b3SharedMemoryCommandHandle b3InitChangeDynamicsInfo(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -1774,7 +1918,7 @@ b3SharedMemoryCommandHandle b3InitChangeDynamicsInfo(b3PhysicsClientHandle physC return (b3SharedMemoryCommandHandle) command; } -int b3ChangeDynamicsInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass) +B3_SHARED_API int b3ChangeDynamicsInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO); @@ -1786,7 +1930,7 @@ int b3ChangeDynamicsInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int b return 0; } -int b3ChangeDynamicsInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction) +B3_SHARED_API int b3ChangeDynamicsInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO); @@ -1797,7 +1941,7 @@ int b3ChangeDynamicsInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHa return 0; } -int b3ChangeDynamicsInfoSetSpinningFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction) +B3_SHARED_API int b3ChangeDynamicsInfoSetSpinningFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO); @@ -1808,7 +1952,7 @@ int b3ChangeDynamicsInfoSetSpinningFriction(b3SharedMemoryCommandHandle commandH return 0; } -int b3ChangeDynamicsInfoSetRollingFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction) +B3_SHARED_API int b3ChangeDynamicsInfoSetRollingFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO); @@ -1821,7 +1965,7 @@ int b3ChangeDynamicsInfoSetRollingFriction(b3SharedMemoryCommandHandle commandHa } -int b3ChangeDynamicsInfoSetRestitution(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double restitution) +B3_SHARED_API int b3ChangeDynamicsInfoSetRestitution(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double restitution) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO); @@ -1831,7 +1975,7 @@ int b3ChangeDynamicsInfoSetRestitution(b3SharedMemoryCommandHandle commandHandle command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_RESTITUTION; return 0; } -int b3ChangeDynamicsInfoSetLinearDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId,double linearDamping) +B3_SHARED_API int b3ChangeDynamicsInfoSetLinearDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId,double linearDamping) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO); @@ -1843,7 +1987,7 @@ int b3ChangeDynamicsInfoSetLinearDamping(b3SharedMemoryCommandHandle commandHand } -int b3ChangeDynamicsInfoSetAngularDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId,double angularDamping) +B3_SHARED_API int b3ChangeDynamicsInfoSetAngularDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId,double angularDamping) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO); @@ -1853,7 +1997,7 @@ int b3ChangeDynamicsInfoSetAngularDamping(b3SharedMemoryCommandHandle commandHan return 0; } -int b3ChangeDynamicsInfoSetContactStiffnessAndDamping(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex,double contactStiffness, double contactDamping) +B3_SHARED_API int b3ChangeDynamicsInfoSetContactStiffnessAndDamping(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex,double contactStiffness, double contactDamping) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO); @@ -1865,7 +2009,7 @@ int b3ChangeDynamicsInfoSetContactStiffnessAndDamping(b3SharedMemoryCommandHandl return 0; } -int b3ChangeDynamicsInfoSetFrictionAnchor(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex, int frictionAnchor) +B3_SHARED_API int b3ChangeDynamicsInfoSetFrictionAnchor(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex, int frictionAnchor) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO); @@ -1878,7 +2022,7 @@ int b3ChangeDynamicsInfoSetFrictionAnchor(b3SharedMemoryCommandHandle commandHan -b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info) +B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -1904,7 +2048,7 @@ b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHan return (b3SharedMemoryCommandHandle)command; } -b3SharedMemoryCommandHandle b3InitChangeUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId) +B3_SHARED_API b3SharedMemoryCommandHandle b3InitChangeUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -1917,7 +2061,7 @@ b3SharedMemoryCommandHandle b3InitChangeUserConstraintCommand(b3PhysicsClientHa return (b3SharedMemoryCommandHandle)command; } -int b3InitChangeUserConstraintSetPivotInB(b3SharedMemoryCommandHandle commandHandle, double pivotInB[3]) +B3_SHARED_API int b3InitChangeUserConstraintSetPivotInB(b3SharedMemoryCommandHandle commandHandle, double jointChildPivot[]) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -1927,12 +2071,12 @@ int b3InitChangeUserConstraintSetPivotInB(b3SharedMemoryCommandHandle commandHan command->m_updateFlags |= USER_CONSTRAINT_CHANGE_PIVOT_IN_B; - command->m_userConstraintArguments.m_childFrame[0] = pivotInB[0]; - command->m_userConstraintArguments.m_childFrame[1] = pivotInB[1]; - command->m_userConstraintArguments.m_childFrame[2] = pivotInB[2]; + command->m_userConstraintArguments.m_childFrame[0] = jointChildPivot[0]; + command->m_userConstraintArguments.m_childFrame[1] = jointChildPivot[1]; + command->m_userConstraintArguments.m_childFrame[2] = jointChildPivot[2]; return 0; } -int b3InitChangeUserConstraintSetFrameInB(b3SharedMemoryCommandHandle commandHandle, double frameOrnInB[4]) +B3_SHARED_API int b3InitChangeUserConstraintSetFrameInB(b3SharedMemoryCommandHandle commandHandle, double jointChildFrameOrn[]) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -1941,15 +2085,15 @@ int b3InitChangeUserConstraintSetFrameInB(b3SharedMemoryCommandHandle commandHan command->m_updateFlags |= USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B; - command->m_userConstraintArguments.m_childFrame[3] = frameOrnInB[0]; - command->m_userConstraintArguments.m_childFrame[4] = frameOrnInB[1]; - command->m_userConstraintArguments.m_childFrame[5] = frameOrnInB[2]; - command->m_userConstraintArguments.m_childFrame[6] = frameOrnInB[3]; + command->m_userConstraintArguments.m_childFrame[3] = jointChildFrameOrn[0]; + command->m_userConstraintArguments.m_childFrame[4] = jointChildFrameOrn[1]; + command->m_userConstraintArguments.m_childFrame[5] = jointChildFrameOrn[2]; + command->m_userConstraintArguments.m_childFrame[6] = jointChildFrameOrn[3]; return 0; } -int b3InitChangeUserConstraintSetMaxForce(b3SharedMemoryCommandHandle commandHandle, double maxAppliedForce) +B3_SHARED_API int b3InitChangeUserConstraintSetMaxForce(b3SharedMemoryCommandHandle commandHandle, double maxAppliedForce) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -1961,7 +2105,7 @@ int b3InitChangeUserConstraintSetMaxForce(b3SharedMemoryCommandHandle commandHan return 0; } -int b3InitChangeUserConstraintSetGearRatio(b3SharedMemoryCommandHandle commandHandle, double gearRatio) +B3_SHARED_API int b3InitChangeUserConstraintSetGearRatio(b3SharedMemoryCommandHandle commandHandle, double gearRatio) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -1974,7 +2118,7 @@ int b3InitChangeUserConstraintSetGearRatio(b3SharedMemoryCommandHandle commandHa return 0; } -int b3InitChangeUserConstraintSetGearAuxLink(b3SharedMemoryCommandHandle commandHandle, int gearAuxLink) +B3_SHARED_API int b3InitChangeUserConstraintSetGearAuxLink(b3SharedMemoryCommandHandle commandHandle, int gearAuxLink) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -1987,8 +2131,33 @@ int b3InitChangeUserConstraintSetGearAuxLink(b3SharedMemoryCommandHandle command return 0; } +B3_SHARED_API int b3InitChangeUserConstraintSetRelativePositionTarget(b3SharedMemoryCommandHandle commandHandle, double relativePositionTarget) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_USER_CONSTRAINT); + b3Assert(command->m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT); + command->m_updateFlags |=USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET; + command->m_userConstraintArguments.m_relativePositionTarget = relativePositionTarget; -b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId) + return 0; +} +B3_SHARED_API int b3InitChangeUserConstraintSetERP(b3SharedMemoryCommandHandle commandHandle, double erp) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_USER_CONSTRAINT); + b3Assert(command->m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT); + + command->m_updateFlags |=USER_CONSTRAINT_CHANGE_ERP; + command->m_userConstraintArguments.m_erp = erp; + + return 0; +} + + + +B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -2002,7 +2171,7 @@ b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHa return (b3SharedMemoryCommandHandle)command; } -b3SharedMemoryCommandHandle b3InitRemoveBodyCommand(b3PhysicsClientHandle physClient, int bodyUniqueId) +B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveBodyCommand(b3PhysicsClientHandle physClient, int bodyUniqueId) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -2019,7 +2188,7 @@ b3SharedMemoryCommandHandle b3InitRemoveBodyCommand(b3PhysicsClientHandle physCl return (b3SharedMemoryCommandHandle)command; } -int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle) +B3_SHARED_API int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle) { const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; b3Assert(status); @@ -2034,7 +2203,7 @@ int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle) } -b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX, +B3_SHARED_API b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX, double rayFromWorldY, double rayFromWorldZ, double rayToWorldX, double rayToWorldY, double rayToWorldZ) { @@ -2053,7 +2222,7 @@ b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double return (b3SharedMemoryCommandHandle)command; } -b3SharedMemoryCommandHandle b3MovePickedBody(b3PhysicsClientHandle physClient, double rayFromWorldX, +B3_SHARED_API b3SharedMemoryCommandHandle b3MovePickedBody(b3PhysicsClientHandle physClient, double rayFromWorldX, double rayFromWorldY, double rayFromWorldZ, double rayToWorldX, double rayToWorldY, double rayToWorldZ) @@ -2073,7 +2242,7 @@ b3SharedMemoryCommandHandle b3MovePickedBody(b3PhysicsClientHandle physClient, d return (b3SharedMemoryCommandHandle)command; } -b3SharedMemoryCommandHandle b3RemovePickingConstraint(b3PhysicsClientHandle physClient) +B3_SHARED_API b3SharedMemoryCommandHandle b3RemovePickingConstraint(b3PhysicsClientHandle physClient) { PhysicsClient *cl = (PhysicsClient *)physClient; b3Assert(cl); @@ -2084,7 +2253,7 @@ b3SharedMemoryCommandHandle b3RemovePickingConstraint(b3PhysicsClientHandle phys return (b3SharedMemoryCommandHandle)command; } -b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsClientHandle physClient, double rayFromWorldX, +B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsClientHandle physClient, double rayFromWorldX, double rayFromWorldY, double rayFromWorldZ, double rayToWorldX, double rayToWorldY, double rayToWorldZ) { @@ -2105,7 +2274,7 @@ b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsClientHandle phy return (b3SharedMemoryCommandHandle)command; } -b3SharedMemoryCommandHandle b3CreateRaycastBatchCommandInit(b3PhysicsClientHandle physClient) +B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastBatchCommandInit(b3PhysicsClientHandle physClient) { PhysicsClient *cl = (PhysicsClient *)physClient; b3Assert(cl); @@ -2118,7 +2287,7 @@ b3SharedMemoryCommandHandle b3CreateRaycastBatchCommandInit(b3PhysicsClientHandl return (b3SharedMemoryCommandHandle)command; } -void b3RaycastBatchAddRay(b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[3], const double rayToWorld[3]) +B3_SHARED_API void b3RaycastBatchAddRay(b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[3], const double rayToWorld[3]) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -2140,7 +2309,7 @@ void b3RaycastBatchAddRay(b3SharedMemoryCommandHandle commandHandle, const doubl } -void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo) +B3_SHARED_API void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo) { PhysicsClient* cl = (PhysicsClient* ) physClient; if (cl) @@ -2151,7 +2320,7 @@ void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastI ///If you re-connected to an existing server, or server changed otherwise, sync the body info -b3SharedMemoryCommandHandle b3InitSyncBodyInfoCommand(b3PhysicsClientHandle physClient) +B3_SHARED_API b3SharedMemoryCommandHandle b3InitSyncBodyInfoCommand(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -2163,7 +2332,7 @@ b3SharedMemoryCommandHandle b3InitSyncBodyInfoCommand(b3PhysicsClientHandle phys return (b3SharedMemoryCommandHandle) command; } -b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode) +B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -2176,7 +2345,7 @@ b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle command->m_requestDebugLinesArguments.m_startingLineIndex = 0; return (b3SharedMemoryCommandHandle) command; } -void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* lines) +B3_SHARED_API void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* lines) { PhysicsClient* cl = (PhysicsClient* ) physClient; @@ -2194,7 +2363,7 @@ void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* l /// Add/remove user-specific debug lines and debug text messages -b3SharedMemoryCommandHandle b3InitUserDebugDrawAddLine3D(b3PhysicsClientHandle physClient, double fromXYZ[3], double toXYZ[3], double colorRGB[3], double lineWidth, double lifeTime) +B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugDrawAddLine3D(b3PhysicsClientHandle physClient, double fromXYZ[3], double toXYZ[3], double colorRGB[3], double lineWidth, double lifeTime) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -2225,7 +2394,7 @@ b3SharedMemoryCommandHandle b3InitUserDebugDrawAddLine3D(b3PhysicsClientHandle p return (b3SharedMemoryCommandHandle) command; } -b3SharedMemoryCommandHandle b3InitUserDebugDrawAddText3D(b3PhysicsClientHandle physClient, const char* txt, double positionXYZ[3], double colorRGB[3], double textSize, double lifeTime) +B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugDrawAddText3D(b3PhysicsClientHandle physClient, const char* txt, double positionXYZ[3], double colorRGB[3], double textSize, double lifeTime) { PhysicsClient* cl = (PhysicsClient* ) physClient; @@ -2263,7 +2432,7 @@ b3SharedMemoryCommandHandle b3InitUserDebugDrawAddText3D(b3PhysicsClientHandle p return (b3SharedMemoryCommandHandle) command; } -void b3UserDebugTextSetOptionFlags(b3SharedMemoryCommandHandle commandHandle, int optionFlags) +B3_SHARED_API void b3UserDebugTextSetOptionFlags(b3SharedMemoryCommandHandle commandHandle, int optionFlags) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -2273,7 +2442,7 @@ void b3UserDebugTextSetOptionFlags(b3SharedMemoryCommandHandle commandHandle, in command->m_updateFlags |= USER_DEBUG_HAS_OPTION_FLAGS; } -void b3UserDebugTextSetOrientation(b3SharedMemoryCommandHandle commandHandle, double orientation[4]) +B3_SHARED_API void b3UserDebugTextSetOrientation(b3SharedMemoryCommandHandle commandHandle, double orientation[4]) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -2288,7 +2457,7 @@ void b3UserDebugTextSetOrientation(b3SharedMemoryCommandHandle commandHandle, do } -void b3UserDebugItemSetParentObject(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex) +B3_SHARED_API void b3UserDebugItemSetParentObject(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -2300,7 +2469,7 @@ void b3UserDebugItemSetParentObject(b3SharedMemoryCommandHandle commandHandle, i } -b3SharedMemoryCommandHandle b3InitUserDebugAddParameter(b3PhysicsClientHandle physClient, const char* txt, double rangeMin, double rangeMax, double startValue) +B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugAddParameter(b3PhysicsClientHandle physClient, const char* txt, double rangeMin, double rangeMax, double startValue) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -2325,7 +2494,7 @@ b3SharedMemoryCommandHandle b3InitUserDebugAddParameter(b3PhysicsClientHandle ph return (b3SharedMemoryCommandHandle)command; } -b3SharedMemoryCommandHandle b3InitUserDebugReadParameter(b3PhysicsClientHandle physClient, int debugItemUniqueId) +B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugReadParameter(b3PhysicsClientHandle physClient, int debugItemUniqueId) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -2338,7 +2507,7 @@ b3SharedMemoryCommandHandle b3InitUserDebugReadParameter(b3PhysicsClientHandle p return (b3SharedMemoryCommandHandle) command; } -int b3GetStatusDebugParameterValue(b3SharedMemoryStatusHandle statusHandle, double* paramValue) +B3_SHARED_API int b3GetStatusDebugParameterValue(b3SharedMemoryStatusHandle statusHandle, double* paramValue) { const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; btAssert(status->m_type == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED); @@ -2351,7 +2520,7 @@ int b3GetStatusDebugParameterValue(b3SharedMemoryStatusHandle statusHandle, doub } -b3SharedMemoryCommandHandle b3InitUserDebugDrawRemove(b3PhysicsClientHandle physClient, int debugItemUniqueId) +B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugDrawRemove(b3PhysicsClientHandle physClient, int debugItemUniqueId) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -2365,7 +2534,7 @@ b3SharedMemoryCommandHandle b3InitUserDebugDrawRemove(b3PhysicsClientHandle phys } -b3SharedMemoryCommandHandle b3InitUserDebugDrawRemoveAll(b3PhysicsClientHandle physClient) +B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugDrawRemoveAll(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -2377,7 +2546,7 @@ b3SharedMemoryCommandHandle b3InitUserDebugDrawRemoveAll(b3PhysicsClientHandle p return (b3SharedMemoryCommandHandle) command; } -int b3GetDebugItemUniqueId(b3SharedMemoryStatusHandle statusHandle) +B3_SHARED_API int b3GetDebugItemUniqueId(b3SharedMemoryStatusHandle statusHandle) { const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; btAssert(status->m_type == CMD_USER_DEBUG_DRAW_COMPLETED); @@ -2387,7 +2556,7 @@ int b3GetDebugItemUniqueId(b3SharedMemoryStatusHandle statusHandle) return status->m_userDebugDrawArgs.m_debugItemUniqueId; } -b3SharedMemoryCommandHandle b3InitDebugDrawingCommand(b3PhysicsClientHandle physClient) +B3_SHARED_API b3SharedMemoryCommandHandle b3InitDebugDrawingCommand(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient*)physClient; b3Assert(cl); @@ -2401,7 +2570,7 @@ b3SharedMemoryCommandHandle b3InitDebugDrawingCommand(b3PhysicsClientHandle phys -void b3SetDebugObjectColor(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex, double objectColorRGB[3]) +B3_SHARED_API void b3SetDebugObjectColor(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex, double objectColorRGB[3]) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -2414,7 +2583,7 @@ void b3SetDebugObjectColor(b3SharedMemoryCommandHandle commandHandle, int object command->m_userDebugDrawArgs.m_objectDebugColorRGB[2] = objectColorRGB[2]; } -void b3RemoveDebugObjectColor(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex) +B3_SHARED_API void b3RemoveDebugObjectColor(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -2429,7 +2598,7 @@ void b3RemoveDebugObjectColor(b3SharedMemoryCommandHandle commandHandle, int obj ///request an image from a simulated camera, using a software renderer. -b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient) +B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -2442,7 +2611,7 @@ b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physC return (b3SharedMemoryCommandHandle) command; } -void b3RequestCameraImageSelectRenderer(b3SharedMemoryCommandHandle commandHandle, int renderer) +B3_SHARED_API void b3RequestCameraImageSelectRenderer(b3SharedMemoryCommandHandle commandHandle, int renderer) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -2454,7 +2623,7 @@ void b3RequestCameraImageSelectRenderer(b3SharedMemoryCommandHandle commandHandl } } -void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle commandHandle, float viewMatrix[16], float projectionMatrix[16]) +B3_SHARED_API void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle commandHandle, float viewMatrix[16], float projectionMatrix[16]) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -2467,7 +2636,7 @@ void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle commandHa command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES; } -void b3RequestCameraImageSetLightDirection(b3SharedMemoryCommandHandle commandHandle, const float lightDirection[3]) +B3_SHARED_API void b3RequestCameraImageSetLightDirection(b3SharedMemoryCommandHandle commandHandle, const float lightDirection[3]) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -2479,7 +2648,7 @@ void b3RequestCameraImageSetLightDirection(b3SharedMemoryCommandHandle commandHa command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_LIGHT_DIRECTION; } -void b3RequestCameraImageSetLightColor(b3SharedMemoryCommandHandle commandHandle, const float lightColor[3]) +B3_SHARED_API void b3RequestCameraImageSetLightColor(b3SharedMemoryCommandHandle commandHandle, const float lightColor[3]) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -2491,7 +2660,7 @@ void b3RequestCameraImageSetLightColor(b3SharedMemoryCommandHandle commandHandle command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_LIGHT_COLOR; } -void b3RequestCameraImageSetLightDistance(b3SharedMemoryCommandHandle commandHandle, float lightDistance) +B3_SHARED_API void b3RequestCameraImageSetLightDistance(b3SharedMemoryCommandHandle commandHandle, float lightDistance) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -2500,7 +2669,7 @@ void b3RequestCameraImageSetLightDistance(b3SharedMemoryCommandHandle commandHan command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_LIGHT_DISTANCE; } -void b3RequestCameraImageSetLightAmbientCoeff(b3SharedMemoryCommandHandle commandHandle, float lightAmbientCoeff) +B3_SHARED_API void b3RequestCameraImageSetLightAmbientCoeff(b3SharedMemoryCommandHandle commandHandle, float lightAmbientCoeff) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -2509,7 +2678,7 @@ void b3RequestCameraImageSetLightAmbientCoeff(b3SharedMemoryCommandHandle comman command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_AMBIENT_COEFF; } -void b3RequestCameraImageSetLightDiffuseCoeff(b3SharedMemoryCommandHandle commandHandle, float lightDiffuseCoeff) +B3_SHARED_API void b3RequestCameraImageSetLightDiffuseCoeff(b3SharedMemoryCommandHandle commandHandle, float lightDiffuseCoeff) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -2518,7 +2687,7 @@ void b3RequestCameraImageSetLightDiffuseCoeff(b3SharedMemoryCommandHandle comman command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_DIFFUSE_COEFF; } -void b3RequestCameraImageSetLightSpecularCoeff(b3SharedMemoryCommandHandle commandHandle, float lightSpecularCoeff) +B3_SHARED_API void b3RequestCameraImageSetLightSpecularCoeff(b3SharedMemoryCommandHandle commandHandle, float lightSpecularCoeff) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -2527,7 +2696,7 @@ void b3RequestCameraImageSetLightSpecularCoeff(b3SharedMemoryCommandHandle comma command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_SPECULAR_COEFF; } -void b3RequestCameraImageSetShadow(b3SharedMemoryCommandHandle commandHandle, int hasShadow) +B3_SHARED_API void b3RequestCameraImageSetShadow(b3SharedMemoryCommandHandle commandHandle, int hasShadow) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -2536,7 +2705,7 @@ void b3RequestCameraImageSetShadow(b3SharedMemoryCommandHandle commandHandle, in command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_SHADOW; } -void b3ComputePositionFromViewMatrix(const float viewMatrix[16], float cameraPosition[3], float cameraTargetPosition[3], float cameraUp[3]) +B3_SHARED_API void b3ComputePositionFromViewMatrix(const float viewMatrix[16], float cameraPosition[3], float cameraTargetPosition[3], float cameraUp[3]) { b3Matrix3x3 r(viewMatrix[0], viewMatrix[4], viewMatrix[8], viewMatrix[1], viewMatrix[5], viewMatrix[9], viewMatrix[2], viewMatrix[6], viewMatrix[10]); b3Vector3 p = b3MakeVector3(viewMatrix[12], viewMatrix[13], viewMatrix[14]); @@ -2560,7 +2729,7 @@ void b3ComputePositionFromViewMatrix(const float viewMatrix[16], float cameraPos cameraUp[2] = u[2]; } -void b3ComputeViewMatrixFromPositions(const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3], float viewMatrix[16]) +B3_SHARED_API void b3ComputeViewMatrixFromPositions(const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3], float viewMatrix[16]) { b3Vector3 eye = b3MakeVector3(cameraPosition[0], cameraPosition[1], cameraPosition[2]); b3Vector3 center = b3MakeVector3(cameraTargetPosition[0], cameraTargetPosition[1], cameraTargetPosition[2]); @@ -2593,7 +2762,7 @@ void b3ComputeViewMatrixFromPositions(const float cameraPosition[3], const float } -void b3ComputeViewMatrixFromYawPitchRoll(const float cameraTargetPosition[3], float distance, float yaw, float pitch, float roll, int upAxis, float viewMatrix[16]) +B3_SHARED_API void b3ComputeViewMatrixFromYawPitchRoll(const float cameraTargetPosition[3], float distance, float yaw, float pitch, float roll, int upAxis, float viewMatrix[16]) { b3Vector3 camUpVector; b3Vector3 camForward; @@ -2649,7 +2818,7 @@ void b3ComputeViewMatrixFromYawPitchRoll(const float cameraTargetPosition[3], fl } -void b3ComputeProjectionMatrix(float left, float right, float bottom, float top, float nearVal, float farVal, float projectionMatrix[16]) +B3_SHARED_API void b3ComputeProjectionMatrix(float left, float right, float bottom, float top, float nearVal, float farVal, float projectionMatrix[16]) { projectionMatrix[0 * 4 + 0] = (float(2) * nearVal) / (right - left); projectionMatrix[0 * 4 + 1] = float(0); @@ -2673,7 +2842,7 @@ void b3ComputeProjectionMatrix(float left, float right, float bottom, float top, } -void b3ComputeProjectionMatrixFOV(float fov, float aspect, float nearVal, float farVal, float projectionMatrix[16]) +B3_SHARED_API void b3ComputeProjectionMatrixFOV(float fov, float aspect, float nearVal, float farVal, float projectionMatrix[16]) { float yScale = 1.0 / tan((3.141592538 / 180.0) * fov / 2); float xScale = yScale / aspect; @@ -2700,7 +2869,7 @@ void b3ComputeProjectionMatrixFOV(float fov, float aspect, float nearVal, float } -void b3RequestCameraImageSetViewMatrix2(b3SharedMemoryCommandHandle commandHandle, const float cameraTargetPosition[3], float distance, float yaw, float pitch, float roll, int upAxis) +B3_SHARED_API void b3RequestCameraImageSetViewMatrix2(b3SharedMemoryCommandHandle commandHandle, const float cameraTargetPosition[3], float distance, float yaw, float pitch, float roll, int upAxis) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -2714,7 +2883,7 @@ void b3RequestCameraImageSetViewMatrix2(b3SharedMemoryCommandHandle commandHandl -void b3RequestCameraImageSetViewMatrix(b3SharedMemoryCommandHandle commandHandle, const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3]) +B3_SHARED_API void b3RequestCameraImageSetViewMatrix(b3SharedMemoryCommandHandle commandHandle, const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3]) { float viewMatrix[16]; b3ComputeViewMatrixFromPositions(cameraPosition, cameraTargetPosition, cameraUp, viewMatrix); @@ -2729,7 +2898,7 @@ void b3RequestCameraImageSetViewMatrix(b3SharedMemoryCommandHandle commandHandle } -void b3RequestCameraImageSetProjectionMatrix(b3SharedMemoryCommandHandle commandHandle, float left, float right, float bottom, float top, float nearVal, float farVal) +B3_SHARED_API void b3RequestCameraImageSetProjectionMatrix(b3SharedMemoryCommandHandle commandHandle, float left, float right, float bottom, float top, float nearVal, float farVal) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; @@ -2741,7 +2910,7 @@ void b3RequestCameraImageSetProjectionMatrix(b3SharedMemoryCommandHandle command command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES; } -void b3RequestCameraImageSetFOVProjectionMatrix(b3SharedMemoryCommandHandle commandHandle, float fov, float aspect, float nearVal, float farVal) +B3_SHARED_API void b3RequestCameraImageSetFOVProjectionMatrix(b3SharedMemoryCommandHandle commandHandle, float fov, float aspect, float nearVal, float farVal) { @@ -2754,7 +2923,7 @@ void b3RequestCameraImageSetFOVProjectionMatrix(b3SharedMemoryCommandHandle comm command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES; } -void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle commandHandle, int width, int height ) +B3_SHARED_API void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle commandHandle, int width, int height ) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -2764,7 +2933,7 @@ void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle commandH command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT; } -void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData) +B3_SHARED_API void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData) { PhysicsClient* cl = (PhysicsClient* ) physClient; if (cl) @@ -2774,7 +2943,7 @@ void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImage } ///request an contact point information -b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClientHandle physClient) +B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -2791,7 +2960,7 @@ b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClient return (b3SharedMemoryCommandHandle) command; } -void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA) +B3_SHARED_API void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -2799,7 +2968,7 @@ void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int body command->m_requestContactPointArguments.m_objectAIndexFilter = bodyUniqueIdA; } -void b3SetContactFilterLinkA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA) +B3_SHARED_API void b3SetContactFilterLinkA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -2808,7 +2977,7 @@ void b3SetContactFilterLinkA(b3SharedMemoryCommandHandle commandHandle, int link command->m_requestContactPointArguments.m_linkIndexAIndexFilter= linkIndexA; } -void b3SetContactFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB) +B3_SHARED_API void b3SetContactFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -2817,12 +2986,12 @@ void b3SetContactFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int link command->m_requestContactPointArguments.m_linkIndexBIndexFilter = linkIndexB; } -void b3SetClosestDistanceFilterLinkA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA) +B3_SHARED_API void b3SetClosestDistanceFilterLinkA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA) { b3SetContactFilterLinkA(commandHandle, linkIndexA); } -void b3SetClosestDistanceFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB) +B3_SHARED_API void b3SetClosestDistanceFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB) { b3SetContactFilterLinkB(commandHandle, linkIndexB); } @@ -2830,7 +2999,7 @@ void b3SetClosestDistanceFilterLinkB(b3SharedMemoryCommandHandle commandHandle, -void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB) +B3_SHARED_API void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -2840,7 +3009,7 @@ void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int body ///compute the closest points between two bodies -b3SharedMemoryCommandHandle b3InitClosestDistanceQuery(b3PhysicsClientHandle physClient) +B3_SHARED_API b3SharedMemoryCommandHandle b3InitClosestDistanceQuery(b3PhysicsClientHandle physClient) { b3SharedMemoryCommandHandle commandHandle =b3InitRequestContactPointInformation(physClient); struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; @@ -2851,17 +3020,17 @@ b3SharedMemoryCommandHandle b3InitClosestDistanceQuery(b3PhysicsClientHandle phy return commandHandle; } -void b3SetClosestDistanceFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA) +B3_SHARED_API void b3SetClosestDistanceFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA) { b3SetContactFilterBodyA(commandHandle,bodyUniqueIdA); } -void b3SetClosestDistanceFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB) +B3_SHARED_API void b3SetClosestDistanceFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB) { b3SetContactFilterBodyB(commandHandle,bodyUniqueIdB); } -void b3SetClosestDistanceThreshold(b3SharedMemoryCommandHandle commandHandle, double distance) +B3_SHARED_API void b3SetClosestDistanceThreshold(b3SharedMemoryCommandHandle commandHandle, double distance) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -2872,7 +3041,7 @@ void b3SetClosestDistanceThreshold(b3SharedMemoryCommandHandle commandHandle, do ///get all the bodies that touch a given axis aligned bounding box specified in world space (min and max coordinates) -b3SharedMemoryCommandHandle b3InitAABBOverlapQuery(b3PhysicsClientHandle physClient, const double aabbMin[3], const double aabbMax[3]) +B3_SHARED_API b3SharedMemoryCommandHandle b3InitAABBOverlapQuery(b3PhysicsClientHandle physClient, const double aabbMin[3], const double aabbMax[3]) { PhysicsClient* cl = (PhysicsClient*)physClient; b3Assert(cl); @@ -2892,7 +3061,7 @@ b3SharedMemoryCommandHandle b3InitAABBOverlapQuery(b3PhysicsClientHandle physCli return (b3SharedMemoryCommandHandle)command; } -void b3GetAABBOverlapResults(b3PhysicsClientHandle physClient, struct b3AABBOverlapData* data) +B3_SHARED_API void b3GetAABBOverlapResults(b3PhysicsClientHandle physClient, struct b3AABBOverlapData* data) { PhysicsClient* cl = (PhysicsClient*)physClient; if (cl) @@ -2903,7 +3072,7 @@ void b3GetAABBOverlapResults(b3PhysicsClientHandle physClient, struct b3AABBOver -void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointData) +B3_SHARED_API void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointData) { PhysicsClient* cl = (PhysicsClient* ) physClient; if (cl) @@ -2912,7 +3081,7 @@ void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3Con } } -void b3GetClosestPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointInfo) +B3_SHARED_API void b3GetClosestPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointInfo) { b3GetContactPointInformation(physClient,contactPointInfo); } @@ -2920,7 +3089,7 @@ void b3GetClosestPointInformation(b3PhysicsClientHandle physClient, struct b3Con //request visual shape information -b3SharedMemoryCommandHandle b3InitRequestVisualShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueIdA) +B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestVisualShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueIdA) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -2934,7 +3103,7 @@ b3SharedMemoryCommandHandle b3InitRequestVisualShapeInformation(b3PhysicsClientH return (b3SharedMemoryCommandHandle) command; } -void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3VisualShapeInformation* visualShapeInfo) +B3_SHARED_API void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3VisualShapeInformation* visualShapeInfo) { PhysicsClient* cl = (PhysicsClient*)physClient; if (cl) @@ -2943,7 +3112,7 @@ void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3Visu } } -b3SharedMemoryCommandHandle b3CreateChangeTextureCommandInit(b3PhysicsClientHandle physClient, int textureUniqueId, int width, int height, const char* rgbPixels) +B3_SHARED_API b3SharedMemoryCommandHandle b3CreateChangeTextureCommandInit(b3PhysicsClientHandle physClient, int textureUniqueId, int width, int height, const char* rgbPixels) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -2962,7 +3131,7 @@ b3SharedMemoryCommandHandle b3CreateChangeTextureCommandInit(b3PhysicsClientHand } -b3SharedMemoryCommandHandle b3InitLoadTexture(b3PhysicsClientHandle physClient, const char* filename) +B3_SHARED_API b3SharedMemoryCommandHandle b3InitLoadTexture(b3PhysicsClientHandle physClient, const char* filename) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -2982,7 +3151,7 @@ b3SharedMemoryCommandHandle b3InitLoadTexture(b3PhysicsClientHandle physClient, return (b3SharedMemoryCommandHandle) command; } -int b3GetStatusTextureUniqueId(b3SharedMemoryStatusHandle statusHandle) +B3_SHARED_API int b3GetStatusTextureUniqueId(b3SharedMemoryStatusHandle statusHandle) { int uid = -1; const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; @@ -2994,7 +3163,7 @@ int b3GetStatusTextureUniqueId(b3SharedMemoryStatusHandle statusHandle) return uid; } -b3SharedMemoryCommandHandle b3InitUpdateVisualShape(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, int shapeIndex, int textureUniqueId) +B3_SHARED_API b3SharedMemoryCommandHandle b3InitUpdateVisualShape(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, int shapeIndex, int textureUniqueId) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -3015,7 +3184,7 @@ b3SharedMemoryCommandHandle b3InitUpdateVisualShape(b3PhysicsClientHandle physCl return (b3SharedMemoryCommandHandle) command; } -void b3UpdateVisualShapeRGBAColor(b3SharedMemoryCommandHandle commandHandle, double rgbaColor[4]) +B3_SHARED_API void b3UpdateVisualShapeRGBAColor(b3SharedMemoryCommandHandle commandHandle, double rgbaColor[4]) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -3031,7 +3200,7 @@ void b3UpdateVisualShapeRGBAColor(b3SharedMemoryCommandHandle commandHandle, dou } } -void b3UpdateVisualShapeSpecularColor(b3SharedMemoryCommandHandle commandHandle, double specularColor[3]) +B3_SHARED_API void b3UpdateVisualShapeSpecularColor(b3SharedMemoryCommandHandle commandHandle, double specularColor[3]) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -3046,7 +3215,7 @@ void b3UpdateVisualShapeSpecularColor(b3SharedMemoryCommandHandle commandHandle, } } -b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient) +B3_SHARED_API b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -3060,7 +3229,7 @@ b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandl return (b3SharedMemoryCommandHandle) command; } -void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flag) +B3_SHARED_API void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flag) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -3077,7 +3246,7 @@ void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUni command->m_externalForceArguments.m_numForcesAndTorques++; } -void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flag) +B3_SHARED_API void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flag) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -3097,7 +3266,7 @@ void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUn ///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics -b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, +B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations) { PhysicsClient* cl = (PhysicsClient*)physClient; @@ -3120,7 +3289,7 @@ b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClien return (b3SharedMemoryCommandHandle)command; } -int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandle, +B3_SHARED_API int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandle, int* bodyUniqueId, int* dofCount, double* jointForces) @@ -3151,7 +3320,7 @@ int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandl return true; } -b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, const double* localPosition, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations) +B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, const double* localPosition, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations) { PhysicsClient* cl = (PhysicsClient*)physClient; b3Assert(cl); @@ -3178,13 +3347,17 @@ b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle } -int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJacobian, double* angularJacobian) +B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, int* dofCount, double* linearJacobian, double* angularJacobian) { const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; btAssert(status->m_type == CMD_CALCULATED_JACOBIAN_COMPLETED); if (status->m_type != CMD_CALCULATED_JACOBIAN_COMPLETED) return false; + if (dofCount) + { + *dofCount = status->m_jacobianResultArgs.m_dofCount; + } if (linearJacobian) { for (int i = 0; i < status->m_jacobianResultArgs.m_dofCount*3; i++) @@ -3205,7 +3378,7 @@ int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJ } ///compute the joint positions to move the end effector to a desired target using inverse kinematics -b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex) +B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex) { PhysicsClient* cl = (PhysicsClient*)physClient; b3Assert(cl); @@ -3221,7 +3394,7 @@ b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsCli } -void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3]) +B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3]) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -3235,7 +3408,7 @@ void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMemoryCommandHand } -void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3], const double targetOrientation[4]) +B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3], const double targetOrientation[4]) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -3254,7 +3427,7 @@ void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemory } -void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[3], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose) +B3_SHARED_API void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[3], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -3275,7 +3448,7 @@ void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemoryCommandHandle } } -void b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[3], const double targetOrientation[4], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose) +B3_SHARED_API void b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[3], const double targetOrientation[4], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -3302,7 +3475,7 @@ void b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(b3SharedMemoryCommandHan } -void b3CalculateInverseKinematicsSetJointDamping(b3SharedMemoryCommandHandle commandHandle, int numDof, const double* jointDampingCoeff) +B3_SHARED_API void b3CalculateInverseKinematicsSetJointDamping(b3SharedMemoryCommandHandle commandHandle, int numDof, const double* jointDampingCoeff) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -3315,7 +3488,7 @@ void b3CalculateInverseKinematicsSetJointDamping(b3SharedMemoryCommandHandle com } } -int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle, +B3_SHARED_API int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle, int* bodyUniqueId, int* dofCount, double* jointPositions) @@ -3345,7 +3518,7 @@ int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle status return true; } -b3SharedMemoryCommandHandle b3RequestVREventsCommandInit(b3PhysicsClientHandle physClient) +B3_SHARED_API b3SharedMemoryCommandHandle b3RequestVREventsCommandInit(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient*)physClient; b3Assert(cl); @@ -3358,7 +3531,7 @@ b3SharedMemoryCommandHandle b3RequestVREventsCommandInit(b3PhysicsClientHandle p return (b3SharedMemoryCommandHandle)command; } -void b3VREventsSetDeviceTypeFilter(b3SharedMemoryCommandHandle commandHandle, int deviceTypeFilter) +B3_SHARED_API void b3VREventsSetDeviceTypeFilter(b3SharedMemoryCommandHandle commandHandle, int deviceTypeFilter) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -3368,7 +3541,7 @@ void b3VREventsSetDeviceTypeFilter(b3SharedMemoryCommandHandle commandHandle, in } } -void b3GetVREventsData(b3PhysicsClientHandle physClient, struct b3VREventsData* vrEventsData) +B3_SHARED_API void b3GetVREventsData(b3PhysicsClientHandle physClient, struct b3VREventsData* vrEventsData) { PhysicsClient* cl = (PhysicsClient* ) physClient; if (cl) @@ -3377,7 +3550,7 @@ void b3GetVREventsData(b3PhysicsClientHandle physClient, struct b3VREventsData* } } -b3SharedMemoryCommandHandle b3SetVRCameraStateCommandInit(b3PhysicsClientHandle physClient) +B3_SHARED_API b3SharedMemoryCommandHandle b3SetVRCameraStateCommandInit(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient*)physClient; b3Assert(cl); @@ -3392,7 +3565,7 @@ b3SharedMemoryCommandHandle b3SetVRCameraStateCommandInit(b3PhysicsClientHandle } -int b3SetVRCameraRootPosition(b3SharedMemoryCommandHandle commandHandle, double rootPos[3]) +B3_SHARED_API int b3SetVRCameraRootPosition(b3SharedMemoryCommandHandle commandHandle, double rootPos[3]) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -3405,7 +3578,7 @@ int b3SetVRCameraRootPosition(b3SharedMemoryCommandHandle commandHandle, double } -int b3SetVRCameraRootOrientation(b3SharedMemoryCommandHandle commandHandle, double rootOrn[4]) +B3_SHARED_API int b3SetVRCameraRootOrientation(b3SharedMemoryCommandHandle commandHandle, double rootOrn[4]) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -3419,7 +3592,7 @@ int b3SetVRCameraRootOrientation(b3SharedMemoryCommandHandle commandHandle, doub return 0; } -int b3SetVRCameraTrackingObject(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId) +B3_SHARED_API int b3SetVRCameraTrackingObject(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -3429,7 +3602,7 @@ int b3SetVRCameraTrackingObject(b3SharedMemoryCommandHandle commandHandle, int o return 0; } -int b3SetVRCameraTrackingObjectFlag(b3SharedMemoryCommandHandle commandHandle, int flag) +B3_SHARED_API int b3SetVRCameraTrackingObjectFlag(b3SharedMemoryCommandHandle commandHandle, int flag) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -3440,7 +3613,7 @@ int b3SetVRCameraTrackingObjectFlag(b3SharedMemoryCommandHandle commandHandle, i } -b3SharedMemoryCommandHandle b3RequestKeyboardEventsCommandInit(b3PhysicsClientHandle physClient) +B3_SHARED_API b3SharedMemoryCommandHandle b3RequestKeyboardEventsCommandInit(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient*)physClient; b3Assert(cl); @@ -3454,7 +3627,7 @@ b3SharedMemoryCommandHandle b3RequestKeyboardEventsCommandInit(b3PhysicsClientHa return (b3SharedMemoryCommandHandle)command; } -void b3GetKeyboardEventsData(b3PhysicsClientHandle physClient, struct b3KeyboardEventsData* keyboardEventsData) +B3_SHARED_API void b3GetKeyboardEventsData(b3PhysicsClientHandle physClient, struct b3KeyboardEventsData* keyboardEventsData) { PhysicsClient* cl = (PhysicsClient* ) physClient; if (cl) @@ -3463,7 +3636,7 @@ void b3GetKeyboardEventsData(b3PhysicsClientHandle physClient, struct b3Keyboard } } -b3SharedMemoryCommandHandle b3RequestMouseEventsCommandInit(b3PhysicsClientHandle physClient) +B3_SHARED_API b3SharedMemoryCommandHandle b3RequestMouseEventsCommandInit(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient*)physClient; b3Assert(cl); @@ -3477,7 +3650,7 @@ b3SharedMemoryCommandHandle b3RequestMouseEventsCommandInit(b3PhysicsClientHandl return (b3SharedMemoryCommandHandle)command; } -void b3GetMouseEventsData(b3PhysicsClientHandle physClient, struct b3MouseEventsData* mouseEventsData) +B3_SHARED_API void b3GetMouseEventsData(b3PhysicsClientHandle physClient, struct b3MouseEventsData* mouseEventsData) { PhysicsClient* cl = (PhysicsClient* ) physClient; if (cl) @@ -3489,7 +3662,7 @@ void b3GetMouseEventsData(b3PhysicsClientHandle physClient, struct b3MouseEvents -b3SharedMemoryCommandHandle b3ProfileTimingCommandInit(b3PhysicsClientHandle physClient, const char* name) +B3_SHARED_API b3SharedMemoryCommandHandle b3ProfileTimingCommandInit(b3PhysicsClientHandle physClient, const char* name) { PhysicsClient* cl = (PhysicsClient*)physClient; b3Assert(cl); @@ -3514,7 +3687,7 @@ b3SharedMemoryCommandHandle b3ProfileTimingCommandInit(b3PhysicsClientHandle phy return (b3SharedMemoryCommandHandle)command; } -void b3SetProfileTimingDuractionInMicroSeconds(b3SharedMemoryCommandHandle commandHandle, int duration) +B3_SHARED_API void b3SetProfileTimingDuractionInMicroSeconds(b3SharedMemoryCommandHandle commandHandle, int duration) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -3525,7 +3698,7 @@ void b3SetProfileTimingDuractionInMicroSeconds(b3SharedMemoryCommandHandle comma } } -b3SharedMemoryCommandHandle b3StateLoggingCommandInit(b3PhysicsClientHandle physClient) +B3_SHARED_API b3SharedMemoryCommandHandle b3StateLoggingCommandInit(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient*)physClient; b3Assert(cl); @@ -3542,7 +3715,7 @@ b3SharedMemoryCommandHandle b3StateLoggingCommandInit(b3PhysicsClientHandle phys } -int b3StateLoggingStart(b3SharedMemoryCommandHandle commandHandle, int loggingType, const char* fileName) +B3_SHARED_API int b3StateLoggingStart(b3SharedMemoryCommandHandle commandHandle, int loggingType, const char* fileName) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -3564,7 +3737,7 @@ int b3StateLoggingStart(b3SharedMemoryCommandHandle commandHandle, int loggingTy return 0; } -int b3GetStatusLoggingUniqueId(b3SharedMemoryStatusHandle statusHandle) +B3_SHARED_API int b3GetStatusLoggingUniqueId(b3SharedMemoryStatusHandle statusHandle) { const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; b3Assert(status); @@ -3576,7 +3749,7 @@ int b3GetStatusLoggingUniqueId(b3SharedMemoryStatusHandle statusHandle) return -1; } -int b3StateLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId) +B3_SHARED_API int b3StateLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -3592,7 +3765,7 @@ int b3StateLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHa return 0; } -int b3StateLoggingSetLinkIndexA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA) +B3_SHARED_API int b3StateLoggingSetLinkIndexA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -3605,7 +3778,7 @@ int b3StateLoggingSetLinkIndexA(b3SharedMemoryCommandHandle commandHandle, int l return 0; } -int b3StateLoggingSetLinkIndexB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB) +B3_SHARED_API int b3StateLoggingSetLinkIndexB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -3618,7 +3791,7 @@ int b3StateLoggingSetLinkIndexB(b3SharedMemoryCommandHandle commandHandle, int l return 0; } -int b3StateLoggingSetBodyAUniqueId(b3SharedMemoryCommandHandle commandHandle, int bodyAUniqueId) +B3_SHARED_API int b3StateLoggingSetBodyAUniqueId(b3SharedMemoryCommandHandle commandHandle, int bodyAUniqueId) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -3631,7 +3804,7 @@ int b3StateLoggingSetBodyAUniqueId(b3SharedMemoryCommandHandle commandHandle, in return 0; } -int b3StateLoggingSetBodyBUniqueId(b3SharedMemoryCommandHandle commandHandle, int bodyBUniqueId) +B3_SHARED_API int b3StateLoggingSetBodyBUniqueId(b3SharedMemoryCommandHandle commandHandle, int bodyBUniqueId) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -3644,7 +3817,7 @@ int b3StateLoggingSetBodyBUniqueId(b3SharedMemoryCommandHandle commandHandle, in return 0; } -int b3StateLoggingSetMaxLogDof(b3SharedMemoryCommandHandle commandHandle, int maxLogDof) +B3_SHARED_API int b3StateLoggingSetMaxLogDof(b3SharedMemoryCommandHandle commandHandle, int maxLogDof) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -3657,7 +3830,7 @@ int b3StateLoggingSetMaxLogDof(b3SharedMemoryCommandHandle commandHandle, int ma return 0; } -int b3StateLoggingSetLogFlags(b3SharedMemoryCommandHandle commandHandle, int logFlags) +B3_SHARED_API int b3StateLoggingSetLogFlags(b3SharedMemoryCommandHandle commandHandle, int logFlags) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -3673,7 +3846,7 @@ int b3StateLoggingSetLogFlags(b3SharedMemoryCommandHandle commandHandle, int log -int b3StateLoggingSetDeviceTypeFilter(b3SharedMemoryCommandHandle commandHandle, int deviceTypeFilter) +B3_SHARED_API int b3StateLoggingSetDeviceTypeFilter(b3SharedMemoryCommandHandle commandHandle, int deviceTypeFilter) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -3686,7 +3859,7 @@ int b3StateLoggingSetDeviceTypeFilter(b3SharedMemoryCommandHandle commandHandle, return 0; } -int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUid) +B3_SHARED_API int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUid) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -3701,7 +3874,7 @@ int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUid ///configure the 3D OpenGL debug visualizer (enable/disable GUI widgets, shadows, position camera etc) -b3SharedMemoryCommandHandle b3InitConfigureOpenGLVisualizer(b3PhysicsClientHandle physClient) +B3_SHARED_API b3SharedMemoryCommandHandle b3InitConfigureOpenGLVisualizer(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient*)physClient; b3Assert(cl); @@ -3715,7 +3888,7 @@ b3SharedMemoryCommandHandle b3InitConfigureOpenGLVisualizer(b3PhysicsClientHandl return (b3SharedMemoryCommandHandle)command; } -void b3ConfigureOpenGLVisualizerSetVisualizationFlags(b3SharedMemoryCommandHandle commandHandle, int flag, int enabled) +B3_SHARED_API void b3ConfigureOpenGLVisualizerSetVisualizationFlags(b3SharedMemoryCommandHandle commandHandle, int flag, int enabled) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -3728,7 +3901,7 @@ void b3ConfigureOpenGLVisualizerSetVisualizationFlags(b3SharedMemoryCommandHandl } } -void b3ConfigureOpenGLVisualizerSetViewMatrix(b3SharedMemoryCommandHandle commandHandle, float cameraDistance, float cameraPitch, float cameraYaw, const float cameraTargetPosition[3]) +B3_SHARED_API void b3ConfigureOpenGLVisualizerSetViewMatrix(b3SharedMemoryCommandHandle commandHandle, float cameraDistance, float cameraPitch, float cameraYaw, const float cameraTargetPosition[3]) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -3746,7 +3919,7 @@ void b3ConfigureOpenGLVisualizerSetViewMatrix(b3SharedMemoryCommandHandle comman } } -b3SharedMemoryCommandHandle b3InitRequestOpenGLVisualizerCameraCommand(b3PhysicsClientHandle physClient) +B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestOpenGLVisualizerCameraCommand(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient*)physClient; b3Assert(cl); @@ -3761,7 +3934,7 @@ b3SharedMemoryCommandHandle b3InitRequestOpenGLVisualizerCameraCommand(b3Physics } -int b3GetStatusOpenGLVisualizerCamera(b3SharedMemoryStatusHandle statusHandle, b3OpenGLVisualizerCameraInfo* camera) +B3_SHARED_API int b3GetStatusOpenGLVisualizerCamera(b3SharedMemoryStatusHandle statusHandle, b3OpenGLVisualizerCameraInfo* camera) { const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; //b3Assert(status); @@ -3773,7 +3946,7 @@ int b3GetStatusOpenGLVisualizerCamera(b3SharedMemoryStatusHandle statusHandle, b return 0; } -void b3SetTimeOut(b3PhysicsClientHandle physClient, double timeOutInSeconds) +B3_SHARED_API void b3SetTimeOut(b3PhysicsClientHandle physClient, double timeOutInSeconds) { PhysicsClient* cl = (PhysicsClient*)physClient; b3Assert(cl); @@ -3785,7 +3958,7 @@ void b3SetTimeOut(b3PhysicsClientHandle physClient, double timeOutInSeconds) } -double b3GetTimeOut(b3PhysicsClientHandle physClient) +B3_SHARED_API double b3GetTimeOut(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient*)physClient; b3Assert(cl); @@ -3796,7 +3969,7 @@ double b3GetTimeOut(b3PhysicsClientHandle physClient) return -1; } -b3SharedMemoryCommandHandle b3SetAdditionalSearchPath(b3PhysicsClientHandle physClient, char* path) +B3_SHARED_API b3SharedMemoryCommandHandle b3SetAdditionalSearchPath(b3PhysicsClientHandle physClient, char* path) { PhysicsClient* cl = (PhysicsClient*)physClient; b3Assert(cl); @@ -3815,7 +3988,7 @@ b3SharedMemoryCommandHandle b3SetAdditionalSearchPath(b3PhysicsClientHandle phys } -void b3MultiplyTransforms(const double posA[3], const double ornA[4], const double posB[3], const double ornB[4], double outPos[3], double outOrn[4]) +B3_SHARED_API void b3MultiplyTransforms(const double posA[3], const double ornA[4], const double posB[3], const double ornB[4], double outPos[3], double outOrn[4]) { b3Transform trA; b3Transform trB; @@ -3834,7 +4007,7 @@ void b3MultiplyTransforms(const double posA[3], const double ornA[4], const doub outOrn[3] = orn[3]; } -void b3InvertTransform(const double pos[3], const double orn[4], double outPos[3], double outOrn[4]) +B3_SHARED_API void b3InvertTransform(const double pos[3], const double orn[4], double outPos[3], double outOrn[4]) { b3Transform tr; tr.setOrigin(b3MakeVector3(pos[0],pos[1],pos[2])); diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 04105e701..d18a547fe 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -10,6 +10,14 @@ B3_DECLARE_HANDLE(b3PhysicsClientHandle); B3_DECLARE_HANDLE(b3SharedMemoryCommandHandle); B3_DECLARE_HANDLE(b3SharedMemoryStatusHandle); +#ifdef _WIN32 + #define B3_SHARED_API __declspec(dllexport) +#elif defined (__GNUC__) + #define B3_SHARED_API __attribute__((visibility("default"))) +#else + #define B3_SHARED_API +#endif + ///There are several connection methods, see following header files: #include "PhysicsClientSharedMemory_C_API.h" @@ -33,30 +41,42 @@ extern "C" { ///b3DisconnectSharedMemory will disconnect the client from the server and cleanup memory. -void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient); +B3_SHARED_API void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient); ///There can only be 1 outstanding command. Check if a command can be send. -int b3CanSubmitCommand(b3PhysicsClientHandle physClient); +B3_SHARED_API int b3CanSubmitCommand(b3PhysicsClientHandle physClient); ///blocking submit command and wait for status -b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle); +B3_SHARED_API b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle); ///In general it is better to use b3SubmitClientCommandAndWaitStatus. b3SubmitClientCommand is a non-blocking submit ///command, which requires checking for the status manually, using b3ProcessServerStatus. Also, before sending the ///next command, make sure to check if you can send a command using 'b3CanSubmitCommand'. -int b3SubmitClientCommand(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle); +B3_SHARED_API int b3SubmitClientCommand(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle); ///non-blocking check status -b3SharedMemoryStatusHandle b3ProcessServerStatus(b3PhysicsClientHandle physClient); +B3_SHARED_API b3SharedMemoryStatusHandle b3ProcessServerStatus(b3PhysicsClientHandle physClient); /// Get the physics server return status type. See EnumSharedMemoryServerStatus in SharedMemoryPublic.h for error codes. -int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle); +B3_SHARED_API int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle); -int b3GetStatusBodyIndices(b3SharedMemoryStatusHandle statusHandle, int* bodyIndicesOut, int bodyIndicesCapacity); +///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 int b3GetStatusPluginUniqueId(b3SharedMemoryStatusHandle statusHandle); +B3_SHARED_API int b3GetStatusPluginCommandResult(b3SharedMemoryStatusHandle statusHandle); -int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle); +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); +B3_SHARED_API void b3CustomCommandExecuteAddFloatArgument(b3SharedMemoryCommandHandle commandHandle, float floatVal); -int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle, + +B3_SHARED_API int b3GetStatusBodyIndices(b3SharedMemoryStatusHandle statusHandle, int* bodyIndicesOut, int bodyIndicesCapacity); + +B3_SHARED_API int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle); + +B3_SHARED_API int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle, int* bodyUniqueId, int* numDegreeOfFreedomQ, int* numDegreeOfFreedomU, @@ -66,315 +86,320 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle, const double* jointReactionForces[]); -b3SharedMemoryCommandHandle b3RequestCollisionInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId); -int b3GetStatusAABB(b3SharedMemoryStatusHandle statusHandle, int linkIndex, double aabbMin[3], double aabbMax[3]); +B3_SHARED_API b3SharedMemoryCommandHandle b3RequestCollisionInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId); +B3_SHARED_API int b3GetStatusAABB(b3SharedMemoryStatusHandle statusHandle, int linkIndex, double aabbMin[/*3*/], double aabbMax[/*3*/]); ///If you re-connected to an existing server, or server changed otherwise, sync the body info and user constraints etc. -b3SharedMemoryCommandHandle b3InitSyncBodyInfoCommand(b3PhysicsClientHandle physClient); +B3_SHARED_API b3SharedMemoryCommandHandle b3InitSyncBodyInfoCommand(b3PhysicsClientHandle physClient); -b3SharedMemoryCommandHandle b3InitRemoveBodyCommand(b3PhysicsClientHandle physClient, int bodyUniqueId); +B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveBodyCommand(b3PhysicsClientHandle physClient, int bodyUniqueId); ///return the total number of bodies in the simulation -int b3GetNumBodies(b3PhysicsClientHandle physClient); +B3_SHARED_API int b3GetNumBodies(b3PhysicsClientHandle physClient); /// return the body unique id, given the index in range [0 , b3GetNumBodies() ) -int b3GetBodyUniqueId(b3PhysicsClientHandle physClient, int serialIndex); +B3_SHARED_API int b3GetBodyUniqueId(b3PhysicsClientHandle physClient, int serialIndex); ///given a body unique id, return the body information. See b3BodyInfo in SharedMemoryPublic.h -int b3GetBodyInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, struct b3BodyInfo* info); +B3_SHARED_API int b3GetBodyInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, struct b3BodyInfo* info); ///give a unique body index (after loading the body) return the number of joints. -int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex); +B3_SHARED_API int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyId); ///given a body and joint index, return the joint information. See b3JointInfo in SharedMemoryPublic.h -int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info); +B3_SHARED_API int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info); -b3SharedMemoryCommandHandle b3GetDynamicsInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex); +B3_SHARED_API b3SharedMemoryCommandHandle b3GetDynamicsInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex); ///given a body unique id and link index, return the dynamics information. See b3DynamicsInfo in SharedMemoryPublic.h -int b3GetDynamicsInfo(b3SharedMemoryStatusHandle statusHandle, struct b3DynamicsInfo* info); +B3_SHARED_API int b3GetDynamicsInfo(b3SharedMemoryStatusHandle statusHandle, struct b3DynamicsInfo* info); -b3SharedMemoryCommandHandle b3InitChangeDynamicsInfo(b3PhysicsClientHandle physClient); -int b3ChangeDynamicsInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass); -int b3ChangeDynamicsInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction); -int b3ChangeDynamicsInfoSetSpinningFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction); -int b3ChangeDynamicsInfoSetRollingFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction); -int b3ChangeDynamicsInfoSetRestitution(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double restitution); -int b3ChangeDynamicsInfoSetLinearDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId,double linearDamping); -int b3ChangeDynamicsInfoSetAngularDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId,double angularDamping); -int b3ChangeDynamicsInfoSetContactStiffnessAndDamping(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex,double contactStiffness, double contactDamping); -int b3ChangeDynamicsInfoSetFrictionAnchor(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex, int frictionAnchor); +B3_SHARED_API b3SharedMemoryCommandHandle b3InitChangeDynamicsInfo(b3PhysicsClientHandle physClient); +B3_SHARED_API int b3ChangeDynamicsInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass); +B3_SHARED_API int b3ChangeDynamicsInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction); +B3_SHARED_API int b3ChangeDynamicsInfoSetSpinningFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction); +B3_SHARED_API int b3ChangeDynamicsInfoSetRollingFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction); +B3_SHARED_API int b3ChangeDynamicsInfoSetRestitution(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double restitution); +B3_SHARED_API int b3ChangeDynamicsInfoSetLinearDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId,double linearDamping); +B3_SHARED_API int b3ChangeDynamicsInfoSetAngularDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId,double angularDamping); +B3_SHARED_API int b3ChangeDynamicsInfoSetContactStiffnessAndDamping(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex,double contactStiffness, double contactDamping); +B3_SHARED_API int b3ChangeDynamicsInfoSetFrictionAnchor(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex, int frictionAnchor); -b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info); +B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info); ///return a unique id for the user constraint, after successful creation, or -1 for an invalid constraint id -int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle); +B3_SHARED_API int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle); ///change parameters of an existing user constraint -b3SharedMemoryCommandHandle b3InitChangeUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId); -int b3InitChangeUserConstraintSetPivotInB(b3SharedMemoryCommandHandle commandHandle, double jointChildPivot[3]); -int b3InitChangeUserConstraintSetFrameInB(b3SharedMemoryCommandHandle commandHandle, double jointChildFrameOrn[4]); -int b3InitChangeUserConstraintSetMaxForce(b3SharedMemoryCommandHandle commandHandle, double maxAppliedForce); -int b3InitChangeUserConstraintSetGearRatio(b3SharedMemoryCommandHandle commandHandle, double gearRatio); -int b3InitChangeUserConstraintSetGearAuxLink(b3SharedMemoryCommandHandle commandHandle, int gearAuxLink); +B3_SHARED_API b3SharedMemoryCommandHandle b3InitChangeUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId); +B3_SHARED_API int b3InitChangeUserConstraintSetPivotInB(b3SharedMemoryCommandHandle commandHandle, double jointChildPivot[/*3*/]); +B3_SHARED_API int b3InitChangeUserConstraintSetFrameInB(b3SharedMemoryCommandHandle commandHandle, double jointChildFrameOrn[/*4*/]); +B3_SHARED_API int b3InitChangeUserConstraintSetMaxForce(b3SharedMemoryCommandHandle commandHandle, double maxAppliedForce); +B3_SHARED_API int b3InitChangeUserConstraintSetGearRatio(b3SharedMemoryCommandHandle commandHandle, double gearRatio); +B3_SHARED_API int b3InitChangeUserConstraintSetGearAuxLink(b3SharedMemoryCommandHandle commandHandle, int gearAuxLink); +B3_SHARED_API int b3InitChangeUserConstraintSetRelativePositionTarget(b3SharedMemoryCommandHandle commandHandle, double relativePositionTarget); +B3_SHARED_API int b3InitChangeUserConstraintSetERP(b3SharedMemoryCommandHandle commandHandle, double erp); -b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId); +B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId); -int b3GetNumUserConstraints(b3PhysicsClientHandle physClient); -int b3GetUserConstraintInfo(b3PhysicsClientHandle physClient, int constraintUniqueId, struct b3UserConstraint* info); +B3_SHARED_API int b3GetNumUserConstraints(b3PhysicsClientHandle physClient); +B3_SHARED_API int b3GetUserConstraintInfo(b3PhysicsClientHandle physClient, int constraintUniqueId, struct b3UserConstraint* info); /// return the user constraint id, given the index in range [0 , b3GetNumUserConstraints() ) -int b3GetUserConstraintId(b3PhysicsClientHandle physClient, int serialIndex); +B3_SHARED_API int b3GetUserConstraintId(b3PhysicsClientHandle physClient, int serialIndex); ///Request physics debug lines for debug visualization. The flags in debugMode are the same as used in Bullet ///See btIDebugDraw::DebugDrawModes in Bullet/src/LinearMath/btIDebugDraw.h -b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode); +B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode); ///Get the pointers to the physics debug line information, after b3InitRequestDebugLinesCommand returns ///status CMD_DEBUG_LINES_COMPLETED -void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* lines); +B3_SHARED_API void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* lines); ///configure the 3D OpenGL debug visualizer (enable/disable GUI widgets, shadows, position camera etc) -b3SharedMemoryCommandHandle b3InitConfigureOpenGLVisualizer(b3PhysicsClientHandle physClient); -void b3ConfigureOpenGLVisualizerSetVisualizationFlags(b3SharedMemoryCommandHandle commandHandle, int flag, int enabled); -void b3ConfigureOpenGLVisualizerSetViewMatrix(b3SharedMemoryCommandHandle commandHandle, float cameraDistance, float cameraPitch, float cameraYaw, const float cameraTargetPosition[3]); +B3_SHARED_API b3SharedMemoryCommandHandle b3InitConfigureOpenGLVisualizer(b3PhysicsClientHandle physClient); +B3_SHARED_API void b3ConfigureOpenGLVisualizerSetVisualizationFlags(b3SharedMemoryCommandHandle commandHandle, int flag, int enabled); +B3_SHARED_API void b3ConfigureOpenGLVisualizerSetViewMatrix(b3SharedMemoryCommandHandle commandHandle, float cameraDistance, float cameraPitch, float cameraYaw, const float cameraTargetPosition[/*3*/]); -b3SharedMemoryCommandHandle b3InitRequestOpenGLVisualizerCameraCommand(b3PhysicsClientHandle physClient); -int b3GetStatusOpenGLVisualizerCamera(b3SharedMemoryStatusHandle statusHandle, struct b3OpenGLVisualizerCameraInfo* camera); +B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestOpenGLVisualizerCameraCommand(b3PhysicsClientHandle physClient); +B3_SHARED_API int b3GetStatusOpenGLVisualizerCamera(b3SharedMemoryStatusHandle statusHandle, struct b3OpenGLVisualizerCameraInfo* camera); /// Add/remove user-specific debug lines and debug text messages -b3SharedMemoryCommandHandle b3InitUserDebugDrawAddLine3D(b3PhysicsClientHandle physClient, double fromXYZ[3], double toXYZ[3], double colorRGB[3], double lineWidth, double lifeTime); +B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugDrawAddLine3D(b3PhysicsClientHandle physClient, double fromXYZ[/*3*/], double toXYZ[/*3*/], double colorRGB[/*3*/], double lineWidth, double lifeTime); -b3SharedMemoryCommandHandle b3InitUserDebugDrawAddText3D(b3PhysicsClientHandle physClient, const char* txt, double positionXYZ[3], double colorRGB[3], double textSize, double lifeTime); -void b3UserDebugTextSetOptionFlags(b3SharedMemoryCommandHandle commandHandle, int optionFlags); -void b3UserDebugTextSetOrientation(b3SharedMemoryCommandHandle commandHandle, double orientation[4]); +B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugDrawAddText3D(b3PhysicsClientHandle physClient, const char* txt, double positionXYZ[/*3*/], double colorRGB[/*3*/], double textSize, double lifeTime); +B3_SHARED_API void b3UserDebugTextSetOptionFlags(b3SharedMemoryCommandHandle commandHandle, int optionFlags); +B3_SHARED_API void b3UserDebugTextSetOrientation(b3SharedMemoryCommandHandle commandHandle, double orientation[/*4*/]); -void b3UserDebugItemSetParentObject(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex); +B3_SHARED_API void b3UserDebugItemSetParentObject(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex); -b3SharedMemoryCommandHandle b3InitUserDebugAddParameter(b3PhysicsClientHandle physClient, const char* txt, double rangeMin, double rangeMax, double startValue); -b3SharedMemoryCommandHandle b3InitUserDebugReadParameter(b3PhysicsClientHandle physClient, int debugItemUniqueId); -int b3GetStatusDebugParameterValue(b3SharedMemoryStatusHandle statusHandle, double* paramValue); +B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugAddParameter(b3PhysicsClientHandle physClient, const char* txt, double rangeMin, double rangeMax, double startValue); +B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugReadParameter(b3PhysicsClientHandle physClient, int debugItemUniqueId); +B3_SHARED_API int b3GetStatusDebugParameterValue(b3SharedMemoryStatusHandle statusHandle, double* paramValue); -b3SharedMemoryCommandHandle b3InitUserDebugDrawRemove(b3PhysicsClientHandle physClient, int debugItemUniqueId); -b3SharedMemoryCommandHandle b3InitUserDebugDrawRemoveAll(b3PhysicsClientHandle physClient); +B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugDrawRemove(b3PhysicsClientHandle physClient, int debugItemUniqueId); +B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugDrawRemoveAll(b3PhysicsClientHandle physClient); -b3SharedMemoryCommandHandle b3InitDebugDrawingCommand(b3PhysicsClientHandle physClient); -void b3SetDebugObjectColor(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex, double objectColorRGB[3]); -void b3RemoveDebugObjectColor(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex); +B3_SHARED_API b3SharedMemoryCommandHandle b3InitDebugDrawingCommand(b3PhysicsClientHandle physClient); +B3_SHARED_API void b3SetDebugObjectColor(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex, double objectColorRGB[/*3*/]); +B3_SHARED_API void b3RemoveDebugObjectColor(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex); ///All debug items unique Ids are positive: a negative unique Id means failure. -int b3GetDebugItemUniqueId(b3SharedMemoryStatusHandle statusHandle); +B3_SHARED_API int b3GetDebugItemUniqueId(b3SharedMemoryStatusHandle statusHandle); ///request an image from a simulated camera, using a software renderer. -b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient); -void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle command, float viewMatrix[16], float projectionMatrix[16]); -void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle command, int width, int height ); -void b3RequestCameraImageSetLightDirection(b3SharedMemoryCommandHandle commandHandle, const float lightDirection[3]); -void b3RequestCameraImageSetLightColor(b3SharedMemoryCommandHandle commandHandle, const float lightColor[3]); -void b3RequestCameraImageSetLightDistance(b3SharedMemoryCommandHandle commandHandle, float lightDistance); -void b3RequestCameraImageSetLightAmbientCoeff(b3SharedMemoryCommandHandle commandHandle, float lightAmbientCoeff); -void b3RequestCameraImageSetLightDiffuseCoeff(b3SharedMemoryCommandHandle commandHandle, float lightDiffuseCoeff); -void b3RequestCameraImageSetLightSpecularCoeff(b3SharedMemoryCommandHandle commandHandle, float lightSpecularCoeff); -void b3RequestCameraImageSetShadow(b3SharedMemoryCommandHandle commandHandle, int hasShadow); -void b3RequestCameraImageSelectRenderer(b3SharedMemoryCommandHandle commandHandle, int renderer); -void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData); +B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient); +B3_SHARED_API void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle commandHandle, float viewMatrix[/*16*/], float projectionMatrix[/*16*/]); +B3_SHARED_API void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle commandHandle, int width, int height ); +B3_SHARED_API void b3RequestCameraImageSetLightDirection(b3SharedMemoryCommandHandle commandHandle, const float lightDirection[/*3*/]); +B3_SHARED_API void b3RequestCameraImageSetLightColor(b3SharedMemoryCommandHandle commandHandle, const float lightColor[/*3*/]); +B3_SHARED_API void b3RequestCameraImageSetLightDistance(b3SharedMemoryCommandHandle commandHandle, float lightDistance); +B3_SHARED_API void b3RequestCameraImageSetLightAmbientCoeff(b3SharedMemoryCommandHandle commandHandle, float lightAmbientCoeff); +B3_SHARED_API void b3RequestCameraImageSetLightDiffuseCoeff(b3SharedMemoryCommandHandle commandHandle, float lightDiffuseCoeff); +B3_SHARED_API void b3RequestCameraImageSetLightSpecularCoeff(b3SharedMemoryCommandHandle commandHandle, float lightSpecularCoeff); +B3_SHARED_API void b3RequestCameraImageSetShadow(b3SharedMemoryCommandHandle commandHandle, int hasShadow); +B3_SHARED_API void b3RequestCameraImageSelectRenderer(b3SharedMemoryCommandHandle commandHandle, int renderer); +B3_SHARED_API void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData); ///compute a view matrix, helper function for b3RequestCameraImageSetCameraMatrices -void b3ComputeViewMatrixFromPositions(const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3], float viewMatrix[16]); -void b3ComputeViewMatrixFromYawPitchRoll(const float cameraTargetPosition[3], float distance, float yaw, float pitch, float roll, int upAxis, float viewMatrix[16]); -void b3ComputePositionFromViewMatrix(const float viewMatrix[16], float cameraPosition[3], float cameraTargetPosition[3], float cameraUp[3]); +B3_SHARED_API void b3ComputeViewMatrixFromPositions(const float cameraPosition[/*3*/], const float cameraTargetPosition[/*3*/], const float cameraUp[/*3*/], float viewMatrix[/*16*/]); +B3_SHARED_API void b3ComputeViewMatrixFromYawPitchRoll(const float cameraTargetPosition[/*3*/], float distance, float yaw, float pitch, float roll, int upAxis, float viewMatrix[/*16*/]); +B3_SHARED_API void b3ComputePositionFromViewMatrix(const float viewMatrix[/*16*/], float cameraPosition[/*3*/], float cameraTargetPosition[/*3*/], float cameraUp[/*3*/]); ///compute a projection matrix, helper function for b3RequestCameraImageSetCameraMatrices -void b3ComputeProjectionMatrix(float left, float right, float bottom, float top, float nearVal, float farVal, float projectionMatrix[16]); -void b3ComputeProjectionMatrixFOV(float fov, float aspect, float nearVal, float farVal, float projectionMatrix[16]); +B3_SHARED_API void b3ComputeProjectionMatrix(float left, float right, float bottom, float top, float nearVal, float farVal, float projectionMatrix[/*16*/]); +B3_SHARED_API void b3ComputeProjectionMatrixFOV(float fov, float aspect, float nearVal, float farVal, float projectionMatrix[/*16*/]); /* obsolete, please use b3ComputeViewProjectionMatrices */ -void b3RequestCameraImageSetViewMatrix(b3SharedMemoryCommandHandle command, const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3]); +B3_SHARED_API void b3RequestCameraImageSetViewMatrix(b3SharedMemoryCommandHandle commandHandle, const float cameraPosition[/*3*/], const float cameraTargetPosition[/*3*/], const float cameraUp[/*3*/]); /* obsolete, please use b3ComputeViewProjectionMatrices */ -void b3RequestCameraImageSetViewMatrix2(b3SharedMemoryCommandHandle commandHandle, const float cameraTargetPosition[3], float distance, float yaw, float pitch, float roll, int upAxis); +B3_SHARED_API void b3RequestCameraImageSetViewMatrix2(b3SharedMemoryCommandHandle commandHandle, const float cameraTargetPosition[/*3*/], float distance, float yaw, float pitch, float roll, int upAxis); /* obsolete, please use b3ComputeViewProjectionMatrices */ -void b3RequestCameraImageSetProjectionMatrix(b3SharedMemoryCommandHandle command, float left, float right, float bottom, float top, float nearVal, float farVal); +B3_SHARED_API void b3RequestCameraImageSetProjectionMatrix(b3SharedMemoryCommandHandle commandHandle, float left, float right, float bottom, float top, float nearVal, float farVal); /* obsolete, please use b3ComputeViewProjectionMatrices */ -void b3RequestCameraImageSetFOVProjectionMatrix(b3SharedMemoryCommandHandle command, float fov, float aspect, float nearVal, float farVal); +B3_SHARED_API void b3RequestCameraImageSetFOVProjectionMatrix(b3SharedMemoryCommandHandle commandHandle, float fov, float aspect, float nearVal, float farVal); ///request an contact point information -b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClientHandle physClient); -void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA); -void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB); -void b3SetContactFilterLinkA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA); -void b3SetContactFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB); -void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointInfo); +B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClientHandle physClient); +B3_SHARED_API void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA); +B3_SHARED_API void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB); +B3_SHARED_API void b3SetContactFilterLinkA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA); +B3_SHARED_API void b3SetContactFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB); +B3_SHARED_API void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointData); ///compute the closest points between two bodies -b3SharedMemoryCommandHandle b3InitClosestDistanceQuery(b3PhysicsClientHandle physClient); -void b3SetClosestDistanceFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA); -void b3SetClosestDistanceFilterLinkA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA); -void b3SetClosestDistanceFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB); -void b3SetClosestDistanceFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB); -void b3SetClosestDistanceThreshold(b3SharedMemoryCommandHandle commandHandle, double distance); -void b3GetClosestPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointInfo); +B3_SHARED_API b3SharedMemoryCommandHandle b3InitClosestDistanceQuery(b3PhysicsClientHandle physClient); +B3_SHARED_API void b3SetClosestDistanceFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA); +B3_SHARED_API void b3SetClosestDistanceFilterLinkA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA); +B3_SHARED_API void b3SetClosestDistanceFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB); +B3_SHARED_API void b3SetClosestDistanceFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB); +B3_SHARED_API void b3SetClosestDistanceThreshold(b3SharedMemoryCommandHandle commandHandle, double distance); +B3_SHARED_API void b3GetClosestPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointInfo); ///get all the bodies that touch a given axis aligned bounding box specified in world space (min and max coordinates) -b3SharedMemoryCommandHandle b3InitAABBOverlapQuery(b3PhysicsClientHandle physClient, const double aabbMin[3],const double aabbMax[3]); -void b3GetAABBOverlapResults(b3PhysicsClientHandle physClient, struct b3AABBOverlapData* data); +B3_SHARED_API b3SharedMemoryCommandHandle b3InitAABBOverlapQuery(b3PhysicsClientHandle physClient, const double aabbMin[/*3*/],const double aabbMax[/*3*/]); +B3_SHARED_API void b3GetAABBOverlapResults(b3PhysicsClientHandle physClient, struct b3AABBOverlapData* data); //request visual shape information -b3SharedMemoryCommandHandle b3InitRequestVisualShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueIdA); -void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3VisualShapeInformation* visualShapeInfo); +B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestVisualShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueIdA); +B3_SHARED_API void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3VisualShapeInformation* visualShapeInfo); -b3SharedMemoryCommandHandle b3InitLoadTexture(b3PhysicsClientHandle physClient, const char* filename); -int b3GetStatusTextureUniqueId(b3SharedMemoryStatusHandle statusHandle); +B3_SHARED_API b3SharedMemoryCommandHandle b3InitLoadTexture(b3PhysicsClientHandle physClient, const char* filename); +B3_SHARED_API int b3GetStatusTextureUniqueId(b3SharedMemoryStatusHandle statusHandle); -b3SharedMemoryCommandHandle b3CreateChangeTextureCommandInit(b3PhysicsClientHandle physClient, int textureUniqueId, int width, int height, const char* rgbPixels); +B3_SHARED_API b3SharedMemoryCommandHandle b3CreateChangeTextureCommandInit(b3PhysicsClientHandle physClient, int textureUniqueId, int width, int height, const char* rgbPixels); -b3SharedMemoryCommandHandle b3InitUpdateVisualShape(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, int shapeIndex, int textureUniqueId); -void b3UpdateVisualShapeRGBAColor(b3SharedMemoryCommandHandle commandHandle, double rgbaColor[4]); -void b3UpdateVisualShapeSpecularColor(b3SharedMemoryCommandHandle commandHandle, double specularColor[3]); +B3_SHARED_API b3SharedMemoryCommandHandle b3InitUpdateVisualShape(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, int shapeIndex, int textureUniqueId); +B3_SHARED_API void b3UpdateVisualShapeRGBAColor(b3SharedMemoryCommandHandle commandHandle, double rgbaColor[/*4*/]); +B3_SHARED_API void b3UpdateVisualShapeSpecularColor(b3SharedMemoryCommandHandle commandHandle, double specularColor[/*3*/]); -b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient); -int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz); -int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep); -int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP); -int b3PhysicsParamSetDefaultNonContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultNonContactERP); -int b3PhysicsParamSetDefaultFrictionERP(b3SharedMemoryCommandHandle commandHandle, double frictionERP); +B3_SHARED_API b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient); +B3_SHARED_API int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz); +B3_SHARED_API int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep); +B3_SHARED_API int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP); +B3_SHARED_API int b3PhysicsParamSetDefaultNonContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultNonContactERP); +B3_SHARED_API int b3PhysicsParamSetDefaultFrictionERP(b3SharedMemoryCommandHandle commandHandle, double frictionERP); -int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps); -int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation); -int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations); -int b3PhysicsParamSetCollisionFilterMode(b3SharedMemoryCommandHandle commandHandle, int filterMode); +B3_SHARED_API int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps); +B3_SHARED_API int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation); +B3_SHARED_API int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations); +B3_SHARED_API int b3PhysicsParamSetCollisionFilterMode(b3SharedMemoryCommandHandle commandHandle, int filterMode); -int b3PhysicsParamSetUseSplitImpulse(b3SharedMemoryCommandHandle commandHandle, int useSplitImpulse); -int b3PhysicsParamSetSplitImpulsePenetrationThreshold(b3SharedMemoryCommandHandle commandHandle, double splitImpulsePenetrationThreshold); -int b3PhysicsParamSetContactBreakingThreshold(b3SharedMemoryCommandHandle commandHandle, double contactBreakingThreshold); -int b3PhysicsParamSetMaxNumCommandsPer1ms(b3SharedMemoryCommandHandle commandHandle, int maxNumCmdPer1ms); -int b3PhysicsParamSetEnableFileCaching(b3SharedMemoryCommandHandle commandHandle, int enableFileCaching); -int b3PhysicsParamSetRestitutionVelocityThreshold(b3SharedMemoryCommandHandle commandHandle, double restitutionVelocityThreshold); +B3_SHARED_API int b3PhysicsParamSetUseSplitImpulse(b3SharedMemoryCommandHandle commandHandle, int useSplitImpulse); +B3_SHARED_API int b3PhysicsParamSetSplitImpulsePenetrationThreshold(b3SharedMemoryCommandHandle commandHandle, double splitImpulsePenetrationThreshold); +B3_SHARED_API int b3PhysicsParamSetContactBreakingThreshold(b3SharedMemoryCommandHandle commandHandle, double contactBreakingThreshold); +B3_SHARED_API int b3PhysicsParamSetMaxNumCommandsPer1ms(b3SharedMemoryCommandHandle commandHandle, int maxNumCmdPer1ms); +B3_SHARED_API int b3PhysicsParamSetEnableFileCaching(b3SharedMemoryCommandHandle commandHandle, int enableFileCaching); +B3_SHARED_API int b3PhysicsParamSetRestitutionVelocityThreshold(b3SharedMemoryCommandHandle commandHandle, double restitutionVelocityThreshold); //b3PhysicsParamSetInternalSimFlags is for internal/temporary/easter-egg/experimental demo purposes //Use at own risk: magic things may or my not happen when calling this API -int b3PhysicsParamSetInternalSimFlags(b3SharedMemoryCommandHandle commandHandle, int flags); +B3_SHARED_API int b3PhysicsParamSetInternalSimFlags(b3SharedMemoryCommandHandle commandHandle, int flags); -b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient); +B3_SHARED_API b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient); -b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHandle physClient); +B3_SHARED_API b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHandle physClient); ///Load a robot from a URDF file. Status type will CMD_URDF_LOADING_COMPLETED. ///Access the robot from the unique body index, through b3GetStatusBodyIndex(statusHandle); -b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName); +B3_SHARED_API b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName); -int b3LoadUrdfCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ); -int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW); -int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody); -int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase); -int b3LoadUrdfCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags); -int b3LoadUrdfCommandSetGlobalScaling(b3SharedMemoryCommandHandle commandHandle, double globalScaling); +B3_SHARED_API int b3LoadUrdfCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ); +B3_SHARED_API int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW); +B3_SHARED_API int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody); +B3_SHARED_API int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase); +B3_SHARED_API int b3LoadUrdfCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags); +B3_SHARED_API int b3LoadUrdfCommandSetGlobalScaling(b3SharedMemoryCommandHandle commandHandle, double globalScaling); -b3SharedMemoryCommandHandle b3LoadBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName); -b3SharedMemoryCommandHandle b3SaveBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName); -b3SharedMemoryCommandHandle b3LoadMJCFCommandInit(b3PhysicsClientHandle physClient, const char* fileName); -void b3LoadMJCFCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags); +B3_SHARED_API b3SharedMemoryCommandHandle b3LoadBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName); +B3_SHARED_API b3SharedMemoryCommandHandle b3SaveBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName); +B3_SHARED_API b3SharedMemoryCommandHandle b3LoadMJCFCommandInit(b3PhysicsClientHandle physClient, const char* fileName); +B3_SHARED_API void b3LoadMJCFCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags); ///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics -b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, +B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations); -int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandle, +B3_SHARED_API int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandle, int* bodyUniqueId, int* dofCount, double* jointForces); -b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, const double* localPosition, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations); +B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, const double* localPosition, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations); -int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJacobian, double* angularJacobian); +B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, + int* dofCount, + double* linearJacobian, + double* angularJacobian); ///compute the joint positions to move the end effector to a desired target using inverse kinematics -b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex); -void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3]); -void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3], const double targetOrientation[4]); -void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[3], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose); -void b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[3], const double targetOrientation[4], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose); -void b3CalculateInverseKinematicsSetJointDamping(b3SharedMemoryCommandHandle commandHandle, int numDof, const double* jointDampingCoeff); -int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle, +B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex); +B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[/*3*/]); +B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[/*3*/], const double targetOrientation[/*4*/]); +B3_SHARED_API void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[/*3*/], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose); +B3_SHARED_API void b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[/*3*/], const double targetOrientation[/*4*/], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose); +B3_SHARED_API void b3CalculateInverseKinematicsSetJointDamping(b3SharedMemoryCommandHandle commandHandle, int numDof, const double* jointDampingCoeff); +B3_SHARED_API int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle, int* bodyUniqueId, int* dofCount, double* jointPositions); -b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName); -int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody); -int b3LoadSdfCommandSetUseGlobalScaling(b3SharedMemoryCommandHandle commandHandle, double globalScaling); +B3_SHARED_API b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName); +B3_SHARED_API int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody); +B3_SHARED_API int b3LoadSdfCommandSetUseGlobalScaling(b3SharedMemoryCommandHandle commandHandle, double globalScaling); -b3SharedMemoryCommandHandle b3SaveWorldCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName); +B3_SHARED_API b3SharedMemoryCommandHandle b3SaveWorldCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName); ///The b3JointControlCommandInit method is obsolete, use b3JointControlCommandInit2 instead -b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode); +B3_SHARED_API b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode); ///Set joint motor control variables such as desired position/angle, desired velocity, ///applied joint forces, dependent on the control mode (CONTROL_MODE_VELOCITY or CONTROL_MODE_TORQUE) -b3SharedMemoryCommandHandle b3JointControlCommandInit2(b3PhysicsClientHandle physClient, int bodyUniqueId, int controlMode); +B3_SHARED_API b3SharedMemoryCommandHandle b3JointControlCommandInit2(b3PhysicsClientHandle physClient, int bodyUniqueId, int controlMode); ///Only use when controlMode is CONTROL_MODE_POSITION_VELOCITY_PD -int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int qIndex, double value); -int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); -int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); +B3_SHARED_API int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int qIndex, double value); +B3_SHARED_API int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); +B3_SHARED_API int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); ///Only use when controlMode is CONTROL_MODE_VELOCITY -int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); /* find a better name for dof/q/u indices, point to b3JointInfo */ -int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); +B3_SHARED_API int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); /* find a better name for dof/q/u indices, point to b3JointInfo */ +B3_SHARED_API int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); ///Only use if when controlMode is CONTROL_MODE_TORQUE, -int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); +B3_SHARED_API int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); ///the creation of collision shapes and rigid bodies etc is likely going to change, ///but good to have a b3CreateBoxShapeCommandInit for now -b3SharedMemoryCommandHandle b3CreateCollisionShapeCommandInit(b3PhysicsClientHandle physClient); -int b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,double radius); -int b3CreateCollisionShapeAddBox(b3SharedMemoryCommandHandle commandHandle,double halfExtents[3]); -int b3CreateCollisionShapeAddCapsule(b3SharedMemoryCommandHandle commandHandle,double radius, double height); -int b3CreateCollisionShapeAddCylinder(b3SharedMemoryCommandHandle commandHandle,double radius, double height); -int b3CreateCollisionShapeAddPlane(b3SharedMemoryCommandHandle commandHandle, double planeNormal[3], double planeConstant); -int b3CreateCollisionShapeAddMesh(b3SharedMemoryCommandHandle commandHandle,const char* fileName, double meshScale[3]); -void b3CreateCollisionSetFlag(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, int flags); +B3_SHARED_API b3SharedMemoryCommandHandle b3CreateCollisionShapeCommandInit(b3PhysicsClientHandle physClient); +B3_SHARED_API int b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,double radius); +B3_SHARED_API int b3CreateCollisionShapeAddBox(b3SharedMemoryCommandHandle commandHandle,double halfExtents[/*3*/]); +B3_SHARED_API int b3CreateCollisionShapeAddCapsule(b3SharedMemoryCommandHandle commandHandle,double radius, double height); +B3_SHARED_API int b3CreateCollisionShapeAddCylinder(b3SharedMemoryCommandHandle commandHandle,double radius, double height); +B3_SHARED_API int b3CreateCollisionShapeAddPlane(b3SharedMemoryCommandHandle commandHandle, double planeNormal[/*3*/], double planeConstant); +B3_SHARED_API int b3CreateCollisionShapeAddMesh(b3SharedMemoryCommandHandle commandHandle,const char* fileName, double meshScale[/*3*/]); +B3_SHARED_API void b3CreateCollisionSetFlag(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, int flags); -void b3CreateCollisionShapeSetChildTransform(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, double childPosition[3], double childOrientation[4]); +B3_SHARED_API void b3CreateCollisionShapeSetChildTransform(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, double childPosition[/*3*/], double childOrientation[/*4*/]); -int b3GetStatusCollisionShapeUniqueId(b3SharedMemoryStatusHandle statusHandle); +B3_SHARED_API int b3GetStatusCollisionShapeUniqueId(b3SharedMemoryStatusHandle statusHandle); -b3SharedMemoryCommandHandle b3CreateVisualShapeCommandInit(b3PhysicsClientHandle physClient); -int b3GetStatusVisualShapeUniqueId(b3SharedMemoryStatusHandle statusHandle); +B3_SHARED_API b3SharedMemoryCommandHandle b3CreateVisualShapeCommandInit(b3PhysicsClientHandle physClient); +B3_SHARED_API int b3GetStatusVisualShapeUniqueId(b3SharedMemoryStatusHandle statusHandle); -b3SharedMemoryCommandHandle b3CreateMultiBodyCommandInit(b3PhysicsClientHandle physClient); -int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass, int collisionShapeUnique, int visualShapeUniqueId, double basePosition[3], double baseOrientation[4] , double baseInertialFramePosition[3], double baseInertialFrameOrientation[4]); +B3_SHARED_API b3SharedMemoryCommandHandle b3CreateMultiBodyCommandInit(b3PhysicsClientHandle physClient); +B3_SHARED_API int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass, int collisionShapeUnique, int visualShapeUniqueId, double basePosition[/*3*/], double baseOrientation[/*4*/] , double baseInertialFramePosition[/*3*/], double baseInertialFrameOrientation[/*4*/]); -int b3CreateMultiBodyLink(b3SharedMemoryCommandHandle commandHandle, double linkMass, double linkCollisionShapeIndex, +B3_SHARED_API int b3CreateMultiBodyLink(b3SharedMemoryCommandHandle commandHandle, double linkMass, double linkCollisionShapeIndex, double linkVisualShapeIndex, - double linkPosition[3], - double linkOrientation[4], - double linkInertialFramePosition[3], - double linkInertialFrameOrientation[4], + double linkPosition[/*3*/], + double linkOrientation[/*4*/], + double linkInertialFramePosition[/*3*/], + double linkInertialFrameOrientation[/*4*/], int linkParentIndex, int linkJointType, - double linkJointAxis[3]); + double linkJointAxis[/*3*/]); //useMaximalCoordinates are disabled by default, enabling them is experimental and not fully supported yet -void b3CreateMultiBodyUseMaximalCoordinates(b3SharedMemoryCommandHandle commandHandle); +B3_SHARED_API void b3CreateMultiBodyUseMaximalCoordinates(b3SharedMemoryCommandHandle commandHandle); //int b3CreateMultiBodyAddLink(b3SharedMemoryCommandHandle commandHandle, int jointType, int parentLinkIndex, double linkMass, int linkCollisionShapeUnique, int linkVisualShapeUniqueId); @@ -382,119 +407,121 @@ void b3CreateMultiBodyUseMaximalCoordinates(b3SharedMemoryCommandHandle commandH ///create a box of size (1,1,1) at world origin (0,0,0) at orientation quat (0,0,0,1) ///after that, you can optionally adjust the initial position, orientation and size -b3SharedMemoryCommandHandle b3CreateBoxShapeCommandInit(b3PhysicsClientHandle physClient); -int b3CreateBoxCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ); -int b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW); -int b3CreateBoxCommandSetHalfExtents(b3SharedMemoryCommandHandle commandHandle, double halfExtentsX,double halfExtentsY,double halfExtentsZ); -int b3CreateBoxCommandSetMass(b3SharedMemoryCommandHandle commandHandle, double mass); -int b3CreateBoxCommandSetCollisionShapeType(b3SharedMemoryCommandHandle commandHandle, int collisionShapeType); -int b3CreateBoxCommandSetColorRGBA(b3SharedMemoryCommandHandle commandHandle, double red,double green,double blue, double alpha); +B3_SHARED_API b3SharedMemoryCommandHandle b3CreateBoxShapeCommandInit(b3PhysicsClientHandle physClient); +B3_SHARED_API int b3CreateBoxCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ); +B3_SHARED_API int b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW); +B3_SHARED_API int b3CreateBoxCommandSetHalfExtents(b3SharedMemoryCommandHandle commandHandle, double halfExtentsX,double halfExtentsY,double halfExtentsZ); +B3_SHARED_API int b3CreateBoxCommandSetMass(b3SharedMemoryCommandHandle commandHandle, double mass); +B3_SHARED_API int b3CreateBoxCommandSetCollisionShapeType(b3SharedMemoryCommandHandle commandHandle, int collisionShapeType); +B3_SHARED_API int b3CreateBoxCommandSetColorRGBA(b3SharedMemoryCommandHandle commandHandle, double red,double green,double blue, double alpha); ///b3CreatePoseCommandInit will initialize (teleport) the pose of a body/robot. You can individually set the base position, ///base orientation and joint angles. This will set all velocities of base and joints to zero. ///This is not a robot control command using actuators/joint motors, but manual repositioning the robot. -b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex); -int b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ); -int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW); -int b3CreatePoseCommandSetBaseLinearVelocity(b3SharedMemoryCommandHandle commandHandle, double linVel[3]); -int b3CreatePoseCommandSetBaseAngularVelocity(b3SharedMemoryCommandHandle commandHandle, double angVel[3]); +B3_SHARED_API b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex); +B3_SHARED_API int b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ); +B3_SHARED_API int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW); +B3_SHARED_API int b3CreatePoseCommandSetBaseLinearVelocity(b3SharedMemoryCommandHandle commandHandle, double linVel[/*3*/]); +B3_SHARED_API int b3CreatePoseCommandSetBaseAngularVelocity(b3SharedMemoryCommandHandle commandHandle, double angVel[/*3*/]); -int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* jointPositions); -int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition); +B3_SHARED_API int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* jointPositions); +B3_SHARED_API int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition); -int b3CreatePoseCommandSetJointVelocities(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int numJointVelocities, const double* jointVelocities); -int b3CreatePoseCommandSetJointVelocity(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointVelocity); +B3_SHARED_API int b3CreatePoseCommandSetJointVelocities(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int numJointVelocities, const double* jointVelocities); +B3_SHARED_API int b3CreatePoseCommandSetJointVelocity(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointVelocity); ///We are currently not reading the sensor information from the URDF file, and programmatically assign sensors. ///This is rather inconsistent, to mix programmatical creation with loading from file. -b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId); -int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle commandHandle, int jointIndex, int enable); +B3_SHARED_API b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId); +B3_SHARED_API int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle commandHandle, int jointIndex, int enable); ///b3CreateSensorEnableIMUForLink is not implemented yet. ///For now, if the IMU is located in the root link, use the root world transform to mimic an IMU. -int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, int linkIndex, int enable); +B3_SHARED_API int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, int linkIndex, int enable); -b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId); -int b3RequestActualStateCommandComputeLinkVelocity(b3SharedMemoryCommandHandle commandHandle, int computeLinkVelocity); +B3_SHARED_API b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId); +B3_SHARED_API int b3RequestActualStateCommandComputeLinkVelocity(b3SharedMemoryCommandHandle commandHandle, int computeLinkVelocity); +B3_SHARED_API int b3RequestActualStateCommandComputeForwardKinematics(b3SharedMemoryCommandHandle commandHandle, int computeForwardKinematics); -int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, struct b3JointSensorState *state); -int b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int linkIndex, struct b3LinkState *state); -b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX, +B3_SHARED_API int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, struct b3JointSensorState *state); +B3_SHARED_API int b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int linkIndex, struct b3LinkState *state); + +B3_SHARED_API b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX, double rayFromWorldY, double rayFromWorldZ, double rayToWorldX, double rayToWorldY, double rayToWorldZ); -b3SharedMemoryCommandHandle b3MovePickedBody(b3PhysicsClientHandle physClient, double rayFromWorldX, +B3_SHARED_API b3SharedMemoryCommandHandle b3MovePickedBody(b3PhysicsClientHandle physClient, double rayFromWorldX, double rayFromWorldY, double rayFromWorldZ, double rayToWorldX, double rayToWorldY, double rayToWorldZ); -b3SharedMemoryCommandHandle b3RemovePickingConstraint(b3PhysicsClientHandle physClient); +B3_SHARED_API b3SharedMemoryCommandHandle b3RemovePickingConstraint(b3PhysicsClientHandle physClient); -b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsClientHandle physClient, double rayFromWorldX, +B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsClientHandle physClient, double rayFromWorldX, double rayFromWorldY, double rayFromWorldZ, double rayToWorldX, double rayToWorldY, double rayToWorldZ); -b3SharedMemoryCommandHandle b3CreateRaycastBatchCommandInit(b3PhysicsClientHandle physClient); -void b3RaycastBatchAddRay(b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[3], const double rayToWorld[3]); +B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastBatchCommandInit(b3PhysicsClientHandle physClient); +B3_SHARED_API void b3RaycastBatchAddRay(b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[/*3*/], const double rayToWorld[/*3*/]); -void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo); +B3_SHARED_API void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo); /// Apply external force at the body (or link) center of mass, in world space/Cartesian coordinates. -b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient); -void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flags); -void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flags); +B3_SHARED_API b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient); +B3_SHARED_API void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[/*3*/], const double position[/*3*/], int flag); +B3_SHARED_API void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[/*3*/], int flag); ///experiments of robots interacting with non-rigid objects (such as btSoftBody) -b3SharedMemoryCommandHandle b3LoadBunnyCommandInit(b3PhysicsClientHandle physClient); -int b3LoadBunnySetScale(b3SharedMemoryCommandHandle commandHandle, double scale); -int b3LoadBunnySetMass(b3SharedMemoryCommandHandle commandHandle, double mass); -int b3LoadBunnySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, double collisionMargin); +B3_SHARED_API b3SharedMemoryCommandHandle b3LoadBunnyCommandInit(b3PhysicsClientHandle physClient); +B3_SHARED_API int b3LoadBunnySetScale(b3SharedMemoryCommandHandle commandHandle, double scale); +B3_SHARED_API int b3LoadBunnySetMass(b3SharedMemoryCommandHandle commandHandle, double mass); +B3_SHARED_API int b3LoadBunnySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, double collisionMargin); -b3SharedMemoryCommandHandle b3RequestVREventsCommandInit(b3PhysicsClientHandle physClient); -void b3VREventsSetDeviceTypeFilter(b3SharedMemoryCommandHandle commandHandle, int deviceTypeFilter); +B3_SHARED_API b3SharedMemoryCommandHandle b3RequestVREventsCommandInit(b3PhysicsClientHandle physClient); +B3_SHARED_API void b3VREventsSetDeviceTypeFilter(b3SharedMemoryCommandHandle commandHandle, int deviceTypeFilter); -void b3GetVREventsData(b3PhysicsClientHandle physClient, struct b3VREventsData* vrEventsData); +B3_SHARED_API void b3GetVREventsData(b3PhysicsClientHandle physClient, struct b3VREventsData* vrEventsData); -b3SharedMemoryCommandHandle b3SetVRCameraStateCommandInit(b3PhysicsClientHandle physClient); -int b3SetVRCameraRootPosition(b3SharedMemoryCommandHandle commandHandle, double rootPos[3]); -int b3SetVRCameraRootOrientation(b3SharedMemoryCommandHandle commandHandle, double rootOrn[4]); -int b3SetVRCameraTrackingObject(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId); -int b3SetVRCameraTrackingObjectFlag(b3SharedMemoryCommandHandle commandHandle, int flag); +B3_SHARED_API b3SharedMemoryCommandHandle b3SetVRCameraStateCommandInit(b3PhysicsClientHandle physClient); +B3_SHARED_API int b3SetVRCameraRootPosition(b3SharedMemoryCommandHandle commandHandle, double rootPos[/*3*/]); +B3_SHARED_API int b3SetVRCameraRootOrientation(b3SharedMemoryCommandHandle commandHandle, double rootOrn[/*4*/]); +B3_SHARED_API int b3SetVRCameraTrackingObject(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId); +B3_SHARED_API int b3SetVRCameraTrackingObjectFlag(b3SharedMemoryCommandHandle commandHandle, int flag); -b3SharedMemoryCommandHandle b3RequestKeyboardEventsCommandInit(b3PhysicsClientHandle physClient); -void b3GetKeyboardEventsData(b3PhysicsClientHandle physClient, struct b3KeyboardEventsData* keyboardEventsData); +B3_SHARED_API b3SharedMemoryCommandHandle b3RequestKeyboardEventsCommandInit(b3PhysicsClientHandle physClient); +B3_SHARED_API void b3GetKeyboardEventsData(b3PhysicsClientHandle physClient, struct b3KeyboardEventsData* keyboardEventsData); -b3SharedMemoryCommandHandle b3RequestMouseEventsCommandInit(b3PhysicsClientHandle physClient); -void b3GetMouseEventsData(b3PhysicsClientHandle physClient, struct b3MouseEventsData* mouseEventsData); +B3_SHARED_API b3SharedMemoryCommandHandle b3RequestMouseEventsCommandInit(b3PhysicsClientHandle physClient); +B3_SHARED_API void b3GetMouseEventsData(b3PhysicsClientHandle physClient, struct b3MouseEventsData* mouseEventsData); -b3SharedMemoryCommandHandle b3StateLoggingCommandInit(b3PhysicsClientHandle physClient); -int b3StateLoggingStart(b3SharedMemoryCommandHandle commandHandle, int loggingType, const char* fileName); -int b3StateLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId); -int b3StateLoggingSetMaxLogDof(b3SharedMemoryCommandHandle commandHandle, int maxLogDof); -int b3StateLoggingSetLinkIndexA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA); -int b3StateLoggingSetLinkIndexB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB); -int b3StateLoggingSetBodyAUniqueId(b3SharedMemoryCommandHandle commandHandle, int bodyAUniqueId); -int b3StateLoggingSetBodyBUniqueId(b3SharedMemoryCommandHandle commandHandle, int bodyBUniqueId); -int b3StateLoggingSetDeviceTypeFilter(b3SharedMemoryCommandHandle commandHandle, int deviceTypeFilter); -int b3StateLoggingSetLogFlags(b3SharedMemoryCommandHandle commandHandle, int logFlags); +B3_SHARED_API b3SharedMemoryCommandHandle b3StateLoggingCommandInit(b3PhysicsClientHandle physClient); +B3_SHARED_API int b3StateLoggingStart(b3SharedMemoryCommandHandle commandHandle, int loggingType, const char* fileName); +B3_SHARED_API int b3StateLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId); +B3_SHARED_API int b3StateLoggingSetMaxLogDof(b3SharedMemoryCommandHandle commandHandle, int maxLogDof); +B3_SHARED_API int b3StateLoggingSetLinkIndexA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA); +B3_SHARED_API int b3StateLoggingSetLinkIndexB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB); +B3_SHARED_API int b3StateLoggingSetBodyAUniqueId(b3SharedMemoryCommandHandle commandHandle, int bodyAUniqueId); +B3_SHARED_API int b3StateLoggingSetBodyBUniqueId(b3SharedMemoryCommandHandle commandHandle, int bodyBUniqueId); +B3_SHARED_API int b3StateLoggingSetDeviceTypeFilter(b3SharedMemoryCommandHandle commandHandle, int deviceTypeFilter); +B3_SHARED_API int b3StateLoggingSetLogFlags(b3SharedMemoryCommandHandle commandHandle, int logFlags); -int b3GetStatusLoggingUniqueId(b3SharedMemoryStatusHandle statusHandle); -int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUniqueId); +B3_SHARED_API int b3GetStatusLoggingUniqueId(b3SharedMemoryStatusHandle statusHandle); +B3_SHARED_API int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUid); -b3SharedMemoryCommandHandle b3ProfileTimingCommandInit(b3PhysicsClientHandle physClient, const char* name); -void b3SetProfileTimingDuractionInMicroSeconds(b3SharedMemoryCommandHandle commandHandle, int duration); +B3_SHARED_API b3SharedMemoryCommandHandle b3ProfileTimingCommandInit(b3PhysicsClientHandle physClient, const char* name); +B3_SHARED_API void b3SetProfileTimingDuractionInMicroSeconds(b3SharedMemoryCommandHandle commandHandle, int duration); -void b3SetTimeOut(b3PhysicsClientHandle physClient, double timeOutInSeconds); -double b3GetTimeOut(b3PhysicsClientHandle physClient); +B3_SHARED_API void b3SetTimeOut(b3PhysicsClientHandle physClient, double timeOutInSeconds); +B3_SHARED_API double b3GetTimeOut(b3PhysicsClientHandle physClient); -b3SharedMemoryCommandHandle b3SetAdditionalSearchPath(b3PhysicsClientHandle physClient, char* path); +B3_SHARED_API b3SharedMemoryCommandHandle b3SetAdditionalSearchPath(b3PhysicsClientHandle physClient, char* path); -void b3MultiplyTransforms(const double posA[3], const double ornA[4], const double posB[3], const double ornB[4], double outPos[3], double outOrn[4]); -void b3InvertTransform(const double pos[3], const double orn[4], double outPos[3], double outOrn[4]); +B3_SHARED_API void b3MultiplyTransforms(const double posA[/*3*/], const double ornA[/*4*/], const double posB[/*3*/], const double ornB[/*4*/], double outPos[/*3*/], double outOrn[/*4*/]); +B3_SHARED_API void b3InvertTransform(const double pos[/*3*/], const double orn[/*4*/], double outPos[/*3*/], double outOrn[/*4*/]); #ifdef __cplusplus } diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index e5cf7b58d..94c73f5f2 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -78,7 +78,7 @@ struct PhysicsClientSharedMemoryInternalData { m_hasLastServerStatus(false), m_sharedMemoryKey(SHARED_MEMORY_KEY), m_verboseOutput(false), - m_timeOutInSeconds(5) + m_timeOutInSeconds(30) {} void processServerStatus(); @@ -1200,6 +1200,24 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { b3Warning("Request getCollisionInfo failed"); break; } + case CMD_CUSTOM_COMMAND_COMPLETED: + { + break; + } + case CMD_CALCULATED_JACOBIAN_COMPLETED: + { + break; + } + case CMD_CALCULATED_JACOBIAN_FAILED: + { + b3Warning("jacobian calculation failed"); + break; + } + case CMD_CUSTOM_COMMAND_FAILED: + { + b3Warning("custom plugin command failed"); + break; + } default: { b3Error("Unknown server status %d\n", serverCmd.m_type); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp b/examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp index 36c794f80..eedcb0160 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp @@ -2,7 +2,7 @@ #include "PhysicsClientSharedMemory.h" -b3PhysicsClientHandle b3ConnectSharedMemory(int key) +B3_SHARED_API b3PhysicsClientHandle b3ConnectSharedMemory(int key) { PhysicsClientSharedMemory* cl = new PhysicsClientSharedMemory(); cl->setSharedMemoryKey(key); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory_C_API.h b/examples/SharedMemory/PhysicsClientSharedMemory_C_API.h index 2a4e4e665..44f9997d7 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory_C_API.h +++ b/examples/SharedMemory/PhysicsClientSharedMemory_C_API.h @@ -7,7 +7,7 @@ extern "C" { #endif - b3PhysicsClientHandle b3ConnectSharedMemory(int key); +B3_SHARED_API b3PhysicsClientHandle b3ConnectSharedMemory(int key); #ifdef __cplusplus } diff --git a/examples/SharedMemory/PhysicsClientTCP_C_API.cpp b/examples/SharedMemory/PhysicsClientTCP_C_API.cpp index e333b967a..6efd39f1e 100644 --- a/examples/SharedMemory/PhysicsClientTCP_C_API.cpp +++ b/examples/SharedMemory/PhysicsClientTCP_C_API.cpp @@ -4,7 +4,7 @@ #include "PhysicsDirect.h" #include -b3PhysicsClientHandle b3ConnectPhysicsTCP(const char* hostName, int port) +B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsTCP(const char* hostName, int port) { TcpNetworkedPhysicsProcessor* tcp = new TcpNetworkedPhysicsProcessor(hostName, port); diff --git a/examples/SharedMemory/PhysicsClientTCP_C_API.h b/examples/SharedMemory/PhysicsClientTCP_C_API.h index dee180377..53216eeec 100644 --- a/examples/SharedMemory/PhysicsClientTCP_C_API.h +++ b/examples/SharedMemory/PhysicsClientTCP_C_API.h @@ -9,7 +9,7 @@ extern "C" { ///send physics commands using TCP networking - b3PhysicsClientHandle b3ConnectPhysicsTCP(const char* hostName, int port); +B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsTCP(const char* hostName, int port); #ifdef __cplusplus diff --git a/examples/SharedMemory/PhysicsClientUDP_C_API.cpp b/examples/SharedMemory/PhysicsClientUDP_C_API.cpp index c3b024c43..50b8b39eb 100644 --- a/examples/SharedMemory/PhysicsClientUDP_C_API.cpp +++ b/examples/SharedMemory/PhysicsClientUDP_C_API.cpp @@ -5,7 +5,7 @@ #include //think more about naming. The b3ConnectPhysicsLoopback -b3PhysicsClientHandle b3ConnectPhysicsUDP(const char* hostName, int port) +B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsUDP(const char* hostName, int port) { UdpNetworkedPhysicsProcessor* udp = new UdpNetworkedPhysicsProcessor(hostName, port); diff --git a/examples/SharedMemory/PhysicsClientUDP_C_API.h b/examples/SharedMemory/PhysicsClientUDP_C_API.h index fdb97bcab..8ea5ef027 100644 --- a/examples/SharedMemory/PhysicsClientUDP_C_API.h +++ b/examples/SharedMemory/PhysicsClientUDP_C_API.h @@ -9,7 +9,7 @@ extern "C" { ///send physics commands using UDP networking - b3PhysicsClientHandle b3ConnectPhysicsUDP(const char* hostName, int port); +B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsUDP(const char* hostName, int port); #ifdef __cplusplus diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index d26ff70da..76d09dfe7 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -940,6 +940,40 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd break; } + case CMD_CUSTOM_COMMAND_COMPLETED: + { + break; + } + case CMD_CUSTOM_COMMAND_FAILED: + { + b3Warning("custom plugin command failed"); + break; + } + case CMD_CLIENT_COMMAND_COMPLETED: + { + break; + } + case CMD_CALCULATED_JACOBIAN_COMPLETED: + { + break; + } + case CMD_CALCULATED_JACOBIAN_FAILED: + { + b3Warning("jacobian calculation failed"); + break; + } + case CMD_ACTUAL_STATE_UPDATE_COMPLETED: + { + break; + } + case CMD_DESIRED_STATE_RECEIVED_COMPLETED: + { + break; + } + case CMD_STEP_FORWARD_SIMULATION_COMPLETED: + { + break; + } default: { //b3Warning("Unknown server status type"); diff --git a/examples/SharedMemory/PhysicsDirectC_API.cpp b/examples/SharedMemory/PhysicsDirectC_API.cpp index 50a89b34f..803d36728 100644 --- a/examples/SharedMemory/PhysicsDirectC_API.cpp +++ b/examples/SharedMemory/PhysicsDirectC_API.cpp @@ -7,7 +7,7 @@ //think more about naming. The b3ConnectPhysicsLoopback -b3PhysicsClientHandle b3ConnectPhysicsDirect() +B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsDirect() { PhysicsServerCommandProcessor* sdk = new PhysicsServerCommandProcessor; diff --git a/examples/SharedMemory/PhysicsDirectC_API.h b/examples/SharedMemory/PhysicsDirectC_API.h index 17681af61..7060bb832 100644 --- a/examples/SharedMemory/PhysicsDirectC_API.h +++ b/examples/SharedMemory/PhysicsDirectC_API.h @@ -9,7 +9,7 @@ extern "C" { ///think more about naming. Directly execute commands without transport (no shared memory, UDP, socket, grpc etc) -b3PhysicsClientHandle b3ConnectPhysicsDirect(); +B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsDirect(); #ifdef __cplusplus diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 1a1b13275..7f3223b59 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1,5 +1,6 @@ #include "PhysicsServerCommandProcessor.h" + #include "../Importers/ImportURDFDemo/BulletUrdfImporter.h" #include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h" #include "../Importers/ImportURDFDemo/URDF2Bullet.h" @@ -37,6 +38,8 @@ #include "LinearMath/btRandom.h" #include "Bullet3Common/b3ResizablePool.h" #include "../Utils/b3Clock.h" +#include "b3PluginManager.h" + #ifdef B3_ENABLE_TINY_AUDIO #include "../TinyAudio/b3SoundEngine.h" #endif @@ -1053,13 +1056,13 @@ struct GenericRobotStateLogger : public InternalStateLogger std::string m_fileName; FILE* m_logFileHandle; std::string m_structTypes; - btMultiBodyDynamicsWorld* m_dynamicsWorld; + const btMultiBodyDynamicsWorld* m_dynamicsWorld; btAlignedObjectArray m_bodyIdList; bool m_filterObjectUniqueId; int m_maxLogDof; int m_logFlags; - GenericRobotStateLogger(int loggingUniqueId, const std::string& fileName, btMultiBodyDynamicsWorld* dynamicsWorld, int maxLogDof, int logFlags) + GenericRobotStateLogger(int loggingUniqueId, const std::string& fileName, const btMultiBodyDynamicsWorld* dynamicsWorld, int maxLogDof, int logFlags) :m_loggingTimeStamp(0), m_logFileHandle(0), m_dynamicsWorld(dynamicsWorld), @@ -1137,7 +1140,7 @@ struct GenericRobotStateLogger : public InternalStateLogger { for (int i=0;igetNumMultibodies();i++) { - btMultiBody* mb = m_dynamicsWorld->getMultiBody(i); + const btMultiBody* mb = m_dynamicsWorld->getMultiBody(i); int objectUniqueId = mb->getUserIndex2(); if (m_filterObjectUniqueId && m_bodyIdList.findLinearSearch2(objectUniqueId) < 0) { @@ -1430,6 +1433,8 @@ struct PhysicsServerCommandProcessorInternalData b3ResizablePool< InternalBodyHandle > m_bodyHandles; b3ResizablePool m_userCollisionShapeHandles; + b3PluginManager m_pluginManager; + bool m_allowRealTimeSimulation; @@ -1484,7 +1489,7 @@ struct PhysicsServerCommandProcessorInternalData btAlignedObjectArray m_sdfRecentLoadedBodies; - + btAlignedObjectArray m_stateLoggers; int m_stateLoggersUniqueId; int m_profileTimingLoggingUid; @@ -1512,8 +1517,8 @@ struct PhysicsServerCommandProcessorInternalData b3HashMap m_profileEvents; - PhysicsServerCommandProcessorInternalData() - : + PhysicsServerCommandProcessorInternalData(PhysicsCommandProcessorInterface* proc) + :m_pluginManager(proc), m_allowRealTimeSimulation(false), m_commandLogger(0), m_logPlayback(0), @@ -1537,6 +1542,16 @@ struct PhysicsServerCommandProcessorInternalData m_pickedConstraint(0), m_pickingMultiBodyPoint2Point(0) { + + + + { + //test to statically link a plugin + //#include "plugins/testPlugin/testplugin.h" + //register static plugins: + //m_pluginManager.registerStaticLinkedPlugin("path", initPlugin, exitPlugin, executePluginCommand); + } + m_vrControllerEvents.init(); m_bodyHandles.exitHandles(); @@ -1643,8 +1658,9 @@ void PhysicsServerCommandProcessor::setGuiHelper(struct GUIHelperInterface* guiH PhysicsServerCommandProcessor::PhysicsServerCommandProcessor() + :m_data(0) { - m_data = new PhysicsServerCommandProcessorInternalData(); + m_data = new PhysicsServerCommandProcessorInternalData(this); createEmptyDynamicsWorld(); @@ -1666,13 +1682,23 @@ PhysicsServerCommandProcessor::~PhysicsServerCommandProcessor() delete m_data; } + +void preTickCallback(btDynamicsWorld *world, btScalar timeStep) +{ + PhysicsServerCommandProcessor* proc = (PhysicsServerCommandProcessor*) world->getWorldUserInfo(); + bool isPreTick = true; + proc->tickPlugins(timeStep, isPreTick); +} + void logCallback(btDynamicsWorld *world, btScalar timeStep) { //handle the logging and playing sounds PhysicsServerCommandProcessor* proc = (PhysicsServerCommandProcessor*) world->getWorldUserInfo(); proc->processCollisionForces(timeStep); - proc->logObjectStates(timeStep); + + bool isPreTick = false; + proc->tickPlugins(timeStep, isPreTick); } @@ -1781,6 +1807,12 @@ void PhysicsServerCommandProcessor::processCollisionForces(btScalar timeStep) #endif//B3_ENABLE_TINY_AUDIO } +void PhysicsServerCommandProcessor::tickPlugins(btScalar timeStep, bool isPreTick) +{ + m_data->m_pluginManager.tickPlugins(timeStep, isPreTick); +} + + void PhysicsServerCommandProcessor::logObjectStates(btScalar timeStep) { for (int i=0;im_stateLoggers.size();i++) @@ -2144,7 +2176,11 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld() { m_data->m_guiHelper->createPhysicsDebugDrawer(m_data->m_dynamicsWorld); } - m_data->m_dynamicsWorld->setInternalTickCallback(logCallback,this); + bool isPreTick=false; + m_data->m_dynamicsWorld->setInternalTickCallback(logCallback,this,isPreTick); + isPreTick = true; + m_data->m_dynamicsWorld->setInternalTickCallback(preTickCallback,this,isPreTick); + #ifdef B3_ENABLE_TINY_AUDIO m_data->m_soundEngine.init(16,true); @@ -2961,27 +2997,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm bool hasStatus = false; { - ///we ignore overflow of integer for now - { - - //until we implement a proper ring buffer, we assume always maximum of 1 outstanding commands - - - //const SharedMemoryCommand& clientCmd =m_data->m_testBlock1->m_clientCommands[0]; -#if 1 if (m_data->m_commandLogger) { m_data->m_commandLogger->logCommand(clientCmd); } -#endif - - //m_data->m_testBlock1->m_numProcessedClientCommands++; - - //no timestamp yet - //int timeStamp = 0; - - //catch uninitialized cases serverStatusOut.m_type = CMD_INVALID_STATUS; serverStatusOut.m_numDataStreamBytes = 0; serverStatusOut.m_dataStream = 0; @@ -2989,32 +3009,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm //consume the command switch (clientCmd.m_type) { -#if 0 - case CMD_SEND_BULLET_DATA_STREAM: - { - if (m_data->m_verboseOutput) - { - b3Printf("Processed CMD_SEND_BULLET_DATA_STREAM length %d",clientCmd.m_dataStreamArguments.m_streamChunkLength); - } - btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld); - m_data->m_worldImporters.push_back(worldImporter); - bool completedOk = worldImporter->loadFileFromMemory(m_data->m_testBlock1->m_bulletStreamDataClientToServer,clientCmd.m_dataStreamArguments.m_streamChunkLength); - - if (completedOk) - { - SharedMemoryStatus& status = m_data->createServerStatus(CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); - m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); - m_data->submitServerStatus(status); - } else - { - SharedMemoryStatus& status = m_data->createServerStatus(CMD_BULLET_DATA_STREAM_RECEIVED_FAILED,clientCmd.m_sequenceNumber,timeStamp); - m_data->submitServerStatus(status); - } - - break; - } -#endif case CMD_STATE_LOGGING: { BT_PROFILE("CMD_STATE_LOGGING"); @@ -5210,6 +5205,17 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm btAlignedObjectArray omega; btAlignedObjectArray linVel; + bool computeForwardKinematics = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_FORWARD_KINEMATICS)!=0); + if (computeForwardKinematics) + { + B3_PROFILE("compForwardKinematics"); + btAlignedObjectArray world_to_local; + btAlignedObjectArray local_origin; + world_to_local.resize(mb->getNumLinks()+1); + local_origin.resize(mb->getNumLinks()+1); + mb->forwardKinematics(world_to_local,local_origin); + } + bool computeLinkVelocities = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_LINKVELOCITY)!=0); if (computeLinkVelocities) { @@ -6623,30 +6629,60 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm if (tree) { int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6; - const int num_dofs = bodyHandle->m_multiBody->getNumDofs(); - btInverseDynamics::vecx nu(num_dofs+baseDofs), qdot(num_dofs + baseDofs), q(num_dofs + baseDofs), joint_force(num_dofs + baseDofs); - for (int i = 0; i < num_dofs; i++) + const int numDofs = bodyHandle->m_multiBody->getNumDofs(); + btInverseDynamics::vecx q(numDofs + baseDofs); + btInverseDynamics::vecx qdot(numDofs + baseDofs); + btInverseDynamics::vecx nu(numDofs + baseDofs); + btInverseDynamics::vecx joint_force(numDofs + baseDofs); + for (int i = 0; i < numDofs; i++) { q[i + baseDofs] = clientCmd.m_calculateJacobianArguments.m_jointPositionsQ[i]; qdot[i + baseDofs] = clientCmd.m_calculateJacobianArguments.m_jointVelocitiesQdot[i]; - nu[i+baseDofs] = clientCmd.m_calculateJacobianArguments.m_jointAccelerations[i]; + nu[i + baseDofs] = clientCmd.m_calculateJacobianArguments.m_jointAccelerations[i]; } // Set the gravity to correspond to the world gravity btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity()); - if (-1 != tree->setGravityInWorldFrame(id_grav) && -1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force)) { - serverCmd.m_jacobianResultArgs.m_dofCount = num_dofs; + serverCmd.m_jacobianResultArgs.m_dofCount = numDofs + baseDofs; // Set jacobian value tree->calculateJacobians(q); - btInverseDynamics::mat3x jac_t(3, num_dofs); - tree->getBodyJacobianTrans(clientCmd.m_calculateJacobianArguments.m_linkIndex+1, &jac_t); + btInverseDynamics::mat3x jac_t(3, numDofs + baseDofs); + btInverseDynamics::mat3x jac_r(3, numDofs + baseDofs); + + // Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link. + tree->getBodyJacobianTrans(clientCmd.m_calculateJacobianArguments.m_linkIndex + 1, &jac_t); + tree->getBodyJacobianRot(clientCmd.m_calculateJacobianArguments.m_linkIndex + 1, &jac_r); + // Update the translational jacobian based on the desired local point. + // v_pt = v_frame + w x pt + // v_pt = J_t * qd + (J_r * qd) x pt + // v_pt = J_t * qd - pt x (J_r * qd) + // v_pt = J_t * qd - pt_x * J_r * qd) + // v_pt = (J_t - pt_x * J_r) * qd + // J_t_new = J_t - pt_x * J_r + btInverseDynamics::vec3 localPosition; + for (int i = 0; i < 3; ++i) { + localPosition(i) = clientCmd.m_calculateJacobianArguments.m_localPosition[i]; + } + // Only calculate if the localPosition is non-zero. + if (btInverseDynamics::maxAbs(localPosition) > 0.0) { + btInverseDynamics::mat33 skewCrossProduct; + btInverseDynamics::skew(localPosition, &skewCrossProduct); + btInverseDynamics::mat3x jac_l(3, numDofs + baseDofs); + btInverseDynamics::mul(skewCrossProduct, jac_r, &jac_l); + btInverseDynamics::mat3x jac_t_new(3, numDofs + baseDofs); + btInverseDynamics::sub(jac_t, jac_l, &jac_t_new); + jac_t = jac_t_new; + } + // Fill in the result into the shared memory. for (int i = 0; i < 3; ++i) { - for (int j = 0; j < num_dofs; ++j) + for (int j = 0; j < (numDofs + baseDofs); ++j) { - serverCmd.m_jacobianResultArgs.m_linearJacobian[i*num_dofs+j] = jac_t(i,j); + int element = (numDofs + baseDofs) * i + j; + serverCmd.m_jacobianResultArgs.m_linearJacobian[element] = jac_t(i,j); + serverCmd.m_jacobianResultArgs.m_angularJacobian[element] = jac_r(i,j); } } serverCmd.m_type = CMD_CALCULATED_JACOBIAN_COMPLETED; @@ -7242,6 +7278,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { userConstraintPtr->m_mbConstraint->setGearRatio(clientCmd.m_userConstraintArguments.m_gearRatio); } + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET) + { + userConstraintPtr->m_mbConstraint->setRelativePositionTarget(clientCmd.m_userConstraintArguments.m_relativePositionTarget); + } + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_ERP) + { + userConstraintPtr->m_mbConstraint->setErp(clientCmd.m_userConstraintArguments.m_erp); + } + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK) { userConstraintPtr->m_mbConstraint->setGearAuxLink(clientCmd.m_userConstraintArguments.m_gearAuxLink); @@ -7370,7 +7415,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm tree->calculateJacobians(q); btInverseDynamics::mat3x jac_t(3, numDofs); btInverseDynamics::mat3x jac_r(3,numDofs); - // Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link. + // Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link. tree->getBodyJacobianTrans(endEffectorLinkIndex+1, &jac_t); tree->getBodyJacobianRot(endEffectorLinkIndex+1, &jac_r); for (int i = 0; i < 3; ++i) @@ -7968,6 +8013,39 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm break; } + case CMD_CUSTOM_COMMAND: + { + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_CUSTOM_COMMAND_FAILED; + serverCmd.m_customCommandResultArgs.m_pluginUniqueId = -1; + + 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); + if (pluginUniqueId>=0) + { + serverCmd.m_customCommandResultArgs.m_pluginUniqueId = pluginUniqueId; + serverCmd.m_type = CMD_CUSTOM_COMMAND_COMPLETED; + } + } + if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_UNLOAD_PLUGIN) + { + 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; + serverCmd.m_type = CMD_CUSTOM_COMMAND_COMPLETED; + + } + + hasStatus = true; + break; + } default: { BT_PROFILE("CMD_UNKNOWN"); @@ -8186,12 +8264,6 @@ void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName) } -btVector3 gVRGripperPos(0.7, 0.3, 0.7); -btQuaternion gVRGripperOrn(0,0,0,1); -btVector3 gVRController2Pos(0,0,0.2); -btQuaternion gVRController2Orn(0,0,0,1); -btScalar gVRGripper2Analog = 0; -btScalar gVRGripperAnalog = 0; @@ -8214,6 +8286,8 @@ 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); + + for (int i=0;im_stateLoggers.size();i++) { if (m_data->m_stateLoggers[i]->m_loggingType==STATE_LOGGING_VR_CONTROLLERS) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index 65bb0bb6d..d46203419 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -91,6 +91,7 @@ public: virtual void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes ); //logging of object states (position etc) + void tickPlugins(btScalar timeStep, bool isPreTick); void logObjectStates(btScalar timeStep); void processCollisionForces(btScalar timeStep); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 357314740..b196d6c71 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -109,6 +109,26 @@ struct b3SearchPathfArgs char m_path[MAX_FILENAME_LENGTH]; }; +enum CustomCommandEnum +{ + CMD_CUSTOM_COMMAND_LOAD_PLUGIN=1, + CMD_CUSTOM_COMMAND_UNLOAD_PLUGIN=2, + CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND=4, +}; + +struct b3CustomCommand +{ + int m_pluginUniqueId; + b3PluginArguments m_arguments; + char m_pluginPath[MAX_FILENAME_LENGTH]; +}; + +struct b3CustomCommandResultArgs +{ + int m_pluginUniqueId; + int m_executeCommandResult; + +}; struct BulletDataStreamArgs { @@ -658,7 +678,9 @@ enum EnumUserConstraintFlags USER_CONSTRAINT_CHANGE_MAX_FORCE=32, USER_CONSTRAINT_REQUEST_INFO=64, USER_CONSTRAINT_CHANGE_GEAR_RATIO=128, - USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK=256, + USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK=256, + USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET=512, + USER_CONSTRAINT_CHANGE_ERP=1024, }; @@ -968,6 +990,7 @@ struct SharedMemoryCommand struct b3RequestCollisionInfoArgs m_requestCollisionInfoArgs; struct b3ChangeTextureArgs m_changeTextureArgs; struct b3SearchPathfArgs m_searchPathArgs; + struct b3CustomCommand m_customCommandArgs; }; }; @@ -1039,6 +1062,7 @@ struct SharedMemoryStatus struct b3SendCollisionInfoArgs m_sendCollisionInfoArgs; struct SendMouseEvents m_sendMouseEvents; struct b3LoadTextureResultArgs m_loadTextureResultArguments; + struct b3CustomCommandResultArgs m_customCommandResultArgs; }; }; diff --git a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp index b82d550a6..3c13898ba 100644 --- a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp +++ b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp @@ -83,7 +83,7 @@ public: }; -b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int argc, char* argv[]) +B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int argc, char* argv[]) { InProcessPhysicsClientSharedMemoryMainThread* cl = new InProcessPhysicsClientSharedMemoryMainThread(argc, argv, 1); cl->setSharedMemoryKey(SHARED_MEMORY_KEY+1); @@ -91,7 +91,7 @@ b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int arg return (b3PhysicsClientHandle ) cl; } -b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThreadSharedMemory(int argc, char* argv[]) +B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThreadSharedMemory(int argc, char* argv[]) { InProcessPhysicsClientSharedMemoryMainThread* cl = new InProcessPhysicsClientSharedMemoryMainThread(argc, argv, 0); cl->setSharedMemoryKey(SHARED_MEMORY_KEY+1); @@ -133,7 +133,7 @@ public: }; -b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* argv[]) +B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* argv[]) { InProcessPhysicsClientSharedMemory* cl = new InProcessPhysicsClientSharedMemory(argc, argv, 1); @@ -141,7 +141,7 @@ b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* a cl->connect(); return (b3PhysicsClientHandle ) cl; } -b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectSharedMemory(int argc, char* argv[]) +B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectSharedMemory(int argc, char* argv[]) { InProcessPhysicsClientSharedMemory* cl = new InProcessPhysicsClientSharedMemory(argc, argv, 0); @@ -248,7 +248,7 @@ int b3InProcessMouseButtonCallback(b3PhysicsClientHandle clientHandle, int butto } -b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(void* guiHelperPtr) +B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(void* guiHelperPtr) { GUIHelperInterface* guiHelper = (GUIHelperInterface*) guiHelperPtr; InProcessPhysicsClientExistingExampleBrowser* cl = new InProcessPhysicsClientExistingExampleBrowser(guiHelper); diff --git a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.h b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.h index 04acba313..9f2a82be0 100644 --- a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.h +++ b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.h @@ -10,13 +10,13 @@ extern "C" { ///think more about naming. The b3ConnectPhysicsLoopback -b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* argv[]); -b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectSharedMemory(int argc, char* argv[]); +B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* argv[]); +B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectSharedMemory(int argc, char* argv[]); -b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int argc, char* argv[]); -b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThreadSharedMemory(int argc, char* argv[]); +B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int argc, char* argv[]); +B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThreadSharedMemory(int argc, char* argv[]); -b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(void* guiHelperPtr); +B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(void* guiHelperPtr); ///ignore the following APIs, they are for internal use for example browser void b3InProcessRenderSceneInternal(b3PhysicsClientHandle clientHandle); diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index cfa605bc4..28b7ec5b1 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -5,7 +5,8 @@ ///increase the SHARED_MEMORY_MAGIC_NUMBER whenever incompatible changes are made in the structures ///my convention is year/month/day/rev -#define SHARED_MEMORY_MAGIC_NUMBER 201708270 +#define SHARED_MEMORY_MAGIC_NUMBER 201709260 +//#define SHARED_MEMORY_MAGIC_NUMBER 201708270 //#define SHARED_MEMORY_MAGIC_NUMBER 201707140 //#define SHARED_MEMORY_MAGIC_NUMBER 201706015 //#define SHARED_MEMORY_MAGIC_NUMBER 201706001 @@ -72,6 +73,7 @@ enum EnumSharedMemoryClientCommand CMD_REQUEST_MOUSE_EVENTS_DATA, CMD_CHANGE_TEXTURE, CMD_SET_ADDITIONAL_SEARCH_PATH, + CMD_CUSTOM_COMMAND, //don't go beyond this command! CMD_MAX_CLIENT_COMMANDS, @@ -167,6 +169,9 @@ enum EnumSharedMemoryServerStatus CMD_REQUEST_COLLISION_INFO_FAILED, CMD_REQUEST_MOUSE_EVENTS_DATA_COMPLETED, CMD_CHANGE_TEXTURE_COMMAND_FAILED, + CMD_CUSTOM_COMMAND_COMPLETED, + CMD_CUSTOM_COMMAND_FAILED, + //don't go beyond 'CMD_MAX_SERVER_COMMANDS! CMD_MAX_SERVER_COMMANDS }; @@ -242,6 +247,8 @@ struct b3UserConstraint int m_userConstraintUniqueId; double m_gearRatio; int m_gearAuxLink; + double m_relativePositionTarget; + double m_erp; }; @@ -501,7 +508,8 @@ struct b3VisualShapeInformation enum eLinkStateFlags { - ACTUAL_STATE_COMPUTE_LINKVELOCITY=1 + ACTUAL_STATE_COMPUTE_LINKVELOCITY=1, + ACTUAL_STATE_COMPUTE_FORWARD_KINEMATICS=2, }; ///b3LinkState provides extra information such as the Cartesian world coordinates @@ -616,6 +624,19 @@ enum eStateLoggingFlags STATE_LOG_JOINT_TORQUES = STATE_LOG_JOINT_MOTOR_TORQUES+STATE_LOG_JOINT_USER_TORQUES, }; +#define B3_MAX_PLUGIN_ARG_SIZE 128 +#define B3_MAX_PLUGIN_ARG_TEXT_LEN 1024 + +struct b3PluginArguments +{ + char m_text[B3_MAX_PLUGIN_ARG_TEXT_LEN]; + int m_numInts; + int m_ints[B3_MAX_PLUGIN_ARG_SIZE]; + int m_numFloats; + int m_floats[B3_MAX_PLUGIN_ARG_SIZE]; + + +}; #endif//SHARED_MEMORY_PUBLIC_H diff --git a/examples/SharedMemory/b3PluginManager.cpp b/examples/SharedMemory/b3PluginManager.cpp new file mode 100644 index 000000000..7f6052349 --- /dev/null +++ b/examples/SharedMemory/b3PluginManager.cpp @@ -0,0 +1,255 @@ + +#include "b3PluginManager.h" +#include "Bullet3Common/b3HashMap.h" +#include "Bullet3Common/b3ResizablePool.h" +#include "PhysicsClientC_API.h" +#include "PhysicsDirect.h" +#include "plugins/b3PluginContext.h" + +#ifdef _WIN32 + #define WIN32_LEAN_AND_MEAN + #define VC_EXTRALEAN + #include + + typedef HMODULE B3_DYNLIB_HANDLE; + + #define B3_DYNLIB_OPEN LoadLibrary + #define B3_DYNLIB_CLOSE FreeLibrary + #define B3_DYNLIB_IMPORT GetProcAddress +#else + #include + + typedef void* B3_DYNLIB_HANDLE; + + #define B3_DYNLIB_OPEN(path) dlopen(path, RTLD_NOW | RTLD_GLOBAL) + #define B3_DYNLIB_CLOSE dlclose + #define B3_DYNLIB_IMPORT dlsym +#endif + +struct b3Plugin +{ + B3_DYNLIB_HANDLE m_pluginHandle; + bool m_ownsPluginHandle; + std::string m_pluginPath; + int m_pluginUniqueId; + PFN_INIT m_initFunc; + PFN_EXIT m_exitFunc; + PFN_EXECUTE m_executeCommandFunc; + + PFN_TICK m_preTickFunc; + PFN_TICK m_postTickFunc; + + void* m_userPointer; + + b3Plugin() + :m_pluginHandle(0), + m_ownsPluginHandle(false), + m_pluginUniqueId(-1), + m_initFunc(0), + m_exitFunc(0), + m_executeCommandFunc(0), + m_preTickFunc(0), + m_postTickFunc(0), + m_userPointer(0) + { + } + void clear() + { + if (m_ownsPluginHandle) + { + B3_DYNLIB_CLOSE(m_pluginHandle); + } + m_pluginHandle = 0; + m_initFunc = 0; + m_exitFunc = 0; + m_executeCommandFunc = 0; + m_preTickFunc = 0; + m_postTickFunc = 0; + m_userPointer = 0; + } +}; + +typedef b3PoolBodyHandle b3PluginHandle; + +struct b3PluginManagerInternalData +{ + b3ResizablePool m_plugins; + b3HashMap m_pluginMap; + PhysicsDirect* m_physicsDirect; +}; + +b3PluginManager::b3PluginManager(class PhysicsCommandProcessorInterface* physSdk) +{ + m_data = new b3PluginManagerInternalData; + m_data->m_physicsDirect = new PhysicsDirect(physSdk,false); + +} + +b3PluginManager::~b3PluginManager() +{ + while (m_data->m_pluginMap.size()) + { + b3PluginHandle* plugin = m_data->m_pluginMap.getAtIndex(0); + unloadPlugin(plugin->m_pluginUniqueId); + } + delete m_data->m_physicsDirect; + m_data->m_pluginMap.clear(); + m_data->m_plugins.exitHandles(); + delete m_data; +} + + +int b3PluginManager::loadPlugin(const char* pluginPath) +{ + int pluginUniqueId = -1; + + b3Plugin* pluginOrg = m_data->m_pluginMap.find(pluginPath); + if (pluginOrg) + { + //already loaded + pluginUniqueId = pluginOrg->m_pluginUniqueId; + } + else + { + pluginUniqueId = m_data->m_plugins.allocHandle(); + b3PluginHandle* plugin = m_data->m_plugins.getHandle(pluginUniqueId); + plugin->m_pluginUniqueId = pluginUniqueId; + B3_DYNLIB_HANDLE pluginHandle = B3_DYNLIB_OPEN(pluginPath); + bool ok = false; + if (pluginHandle) + { + + 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"); + + if (plugin->m_initFunc && plugin->m_exitFunc && plugin->m_executeCommandFunc) + { + + b3PluginContext context; + context.m_userPointer = plugin->m_userPointer; + context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect; + int version = plugin->m_initFunc(&context); + //keep the user pointer persistent + plugin->m_userPointer = context.m_userPointer; + if (version == SHARED_MEMORY_MAGIC_NUMBER) + { + ok = true; + plugin->m_ownsPluginHandle = true; + plugin->m_pluginHandle = pluginHandle; + plugin->m_pluginPath = pluginPath; + m_data->m_pluginMap.insert(pluginPath, *plugin); + } + else + { + int expect = SHARED_MEMORY_MAGIC_NUMBER; + b3Warning("Warning: plugin is wrong version: expected %d, got %d\n", expect, version); + } + } + else + { + b3Warning("Loaded plugin but couldn't bind functions"); + } + + if (!ok) + { + B3_DYNLIB_CLOSE(pluginHandle); + } + } + else + { + b3Warning("Warning: couldn't load plugin %s\n", pluginPath); + } + if (!ok) + { + m_data->m_plugins.freeHandle(pluginUniqueId); + pluginUniqueId = -1; + } + } + return pluginUniqueId; +} + +void b3PluginManager::unloadPlugin(int pluginUniqueId) +{ + b3PluginHandle* plugin = m_data->m_plugins.getHandle(pluginUniqueId); + if (plugin) + { + b3PluginContext context; + context.m_userPointer = plugin->m_userPointer; + context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect; + + plugin->m_exitFunc(&context); + m_data->m_pluginMap.remove(plugin->m_pluginPath.c_str()); + m_data->m_plugins.freeHandle(pluginUniqueId); + } +} + +void b3PluginManager::tickPlugins(double timeStep, bool isPreTick) +{ + for (int i=0;im_pluginMap.size();i++) + { + b3PluginHandle* plugin = m_data->m_pluginMap.getAtIndex(i); + + 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; + int result = tick(&context); + plugin->m_userPointer = context.m_userPointer; + } + } +} + +int b3PluginManager::executePluginCommand(int pluginUniqueId, const b3PluginArguments* arguments) +{ + int result = -1; + + b3PluginHandle* plugin = m_data->m_plugins.getHandle(pluginUniqueId); + if (plugin) + { + b3PluginContext context; + context.m_userPointer = plugin->m_userPointer; + context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect; + + result = plugin->m_executeCommandFunc(&context, arguments); + plugin->m_userPointer = context.m_userPointer; + } + return result; +} + + +int b3PluginManager::registerStaticLinkedPlugin(const char* pluginPath, PFN_INIT initFunc,PFN_EXIT exitFunc, PFN_EXECUTE executeCommandFunc, PFN_TICK preTickFunc, PFN_TICK postTickFunc) +{ + + b3Plugin orgPlugin; + + + int pluginUniqueId = m_data->m_plugins.allocHandle(); + b3PluginHandle* pluginHandle = m_data->m_plugins.getHandle(pluginUniqueId); + pluginHandle->m_pluginHandle = 0; + pluginHandle->m_ownsPluginHandle =false; + pluginHandle->m_pluginUniqueId = pluginUniqueId; + pluginHandle->m_executeCommandFunc = executeCommandFunc; + pluginHandle->m_exitFunc = exitFunc; + pluginHandle->m_initFunc = initFunc; + pluginHandle->m_preTickFunc = preTickFunc; + pluginHandle->m_postTickFunc = postTickFunc; + pluginHandle->m_pluginHandle = 0; + pluginHandle->m_pluginPath = pluginPath; + pluginHandle->m_userPointer = 0; + + m_data->m_pluginMap.insert(pluginPath, *pluginHandle); + + { + b3PluginContext context; + context.m_userPointer = 0; + context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect; + int result = pluginHandle->m_initFunc(&context); + pluginHandle->m_userPointer = context.m_userPointer; + } + return pluginUniqueId; +} diff --git a/examples/SharedMemory/b3PluginManager.h b/examples/SharedMemory/b3PluginManager.h new file mode 100644 index 000000000..6d4fa7ef1 --- /dev/null +++ b/examples/SharedMemory/b3PluginManager.h @@ -0,0 +1,23 @@ +#ifndef B3_PLUGIN_MANAGER_H +#define B3_PLUGIN_MANAGER_H + +#include "plugins/b3PluginAPI.h" + +class b3PluginManager +{ + struct b3PluginManagerInternalData* m_data; + + public: + + b3PluginManager(class PhysicsCommandProcessorInterface* physSdk); + virtual ~b3PluginManager(); + + int loadPlugin(const char* pluginPath); + void unloadPlugin(int pluginUniqueId); + int executePluginCommand(int pluginUniqueId, const struct b3PluginArguments* arguments); + 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); + +}; + +#endif //B3_PLUGIN_MANAGER_H diff --git a/examples/SharedMemory/plugins/b3PluginAPI.h b/examples/SharedMemory/plugins/b3PluginAPI.h new file mode 100644 index 000000000..54387668f --- /dev/null +++ b/examples/SharedMemory/plugins/b3PluginAPI.h @@ -0,0 +1,37 @@ +#ifndef B3_PLUGIN_API_H +#define B3_PLUGIN_API_H + +#ifdef _WIN32 +#define B3_SHARED_API __declspec(dllexport) +#elif defined (__GNUC__) +#define B3_SHARED_API __attribute__((visibility("default"))) +#else +#define B3_SHARED_API +#endif + + +#if defined(_WIN32) +#define B3_API_ENTRY +#define B3_API_CALL __cdecl +#define B3_CALLBACK __cdecl +#else +#define B3_API_ENTRY +#define B3_API_CALL +#define B3_CALLBACK +#endif + + + +#ifdef __cplusplus +extern "C" { +#endif + /* Plugin API */ + typedef B3_API_ENTRY int (B3_API_CALL * PFN_INIT)(struct b3PluginContext* context); + typedef B3_API_ENTRY void (B3_API_CALL * PFN_EXIT)(struct b3PluginContext* context); + typedef B3_API_ENTRY int (B3_API_CALL * PFN_EXECUTE)(struct b3PluginContext* context, const struct b3PluginArguments* arguments); + typedef B3_API_ENTRY int (B3_API_CALL * PFN_TICK)(struct b3PluginContext* context); +#ifdef __cplusplus +} +#endif + +#endif //B3_PLUGIN_API_H diff --git a/examples/SharedMemory/plugins/b3PluginContext.h b/examples/SharedMemory/plugins/b3PluginContext.h new file mode 100644 index 000000000..cb8bd754d --- /dev/null +++ b/examples/SharedMemory/plugins/b3PluginContext.h @@ -0,0 +1,21 @@ +#ifndef B3_PLUGIN_CONTEXT_H +#define B3_PLUGIN_CONTEXT_H + +#include "../PhysicsClientC_API.h" + +struct b3PluginContext +{ + + b3PhysicsClientHandle m_physClient; + + //plugin can modify the m_userPointer to store persistent object pointer (class or struct instance etc) + void* m_userPointer; + +}; + + + + + + +#endif //B3_PLUGIN_CONTEXT_H \ No newline at end of file diff --git a/examples/SharedMemory/plugins/testPlugin/premake4.lua b/examples/SharedMemory/plugins/testPlugin/premake4.lua new file mode 100644 index 000000000..edc27bacf --- /dev/null +++ b/examples/SharedMemory/plugins/testPlugin/premake4.lua @@ -0,0 +1,42 @@ + + +project ("pybullet_testplugin") + language "C++" + kind "SharedLib" + + includedirs {".","../../../../src", "../../../../examples", + "../../../ThirdPartyLibs"} + defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"} + hasCL = findOpenCL("clew") + + links{"BulletFileLoader", "Bullet3Common", "LinearMath"} + + + if os.is("MacOSX") then +-- targetextension {"so"} + links{"Cocoa.framework","Python"} + end + + + files { + "testplugin.cpp", + "../../PhysicsClient.cpp", + "../../PhysicsClient.h", + "../../PhysicsClientSharedMemory.cpp", + "../../PhysicsClientSharedMemory.h", + "../../PhysicsClientSharedMemory_C_API.cpp", + "../../PhysicsClientSharedMemory_C_API.h", + "../../PhysicsClientC_API.cpp", + "../../PhysicsClientC_API.h", + "../../Win32SharedMemory.cpp", + "../../Win32SharedMemory.h", + "../../PosixSharedMemory.cpp", + "../../PosixSharedMemory.h", + "../../../Utils/b3Clock.cpp", + "../../../Utils/b3Clock.h", + "../../../Utils/b3ResourcePath.cpp", + "../../../Utils/b3ResourcePath.h", + } + + + diff --git a/examples/SharedMemory/plugins/testPlugin/testplugin.cpp b/examples/SharedMemory/plugins/testPlugin/testplugin.cpp new file mode 100644 index 000000000..2ba2b6bf2 --- /dev/null +++ b/examples/SharedMemory/plugins/testPlugin/testplugin.cpp @@ -0,0 +1,150 @@ + +//test plugin, can load a URDF file, example usage on a Windows machine: + +/* +import pybullet as p +p.connect(p.GUI) +pluginUid = p.loadPlugin("E:/develop/bullet3/bin/pybullet_testplugin_vs2010_x64_debug.dll") +commandUid = 0 +argument = "plane.urdf" +p.executePluginCommand(pluginUid,commandUid,argument) +p.unloadPlugin(pluginUid) +*/ + +#include "testplugin.h" +#include "../../SharedMemoryPublic.h" +#include "../b3PluginContext.h" +#include + +struct MyClass +{ + int m_testData; + + MyClass() + :m_testData(42) + { + } + virtual ~MyClass() + { + } +}; + +B3_SHARED_API int initPlugin(struct b3PluginContext* context) +{ + MyClass* obj = new MyClass(); + context->m_userPointer = obj; + + printf("hi!\n"); + return SHARED_MEMORY_MAGIC_NUMBER; +} + + +B3_SHARED_API int preTickPluginCallback(struct b3PluginContext* context) +{ + MyClass* obj = (MyClass* )context->m_userPointer; + + { + 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) + { + //this is only for a test, normally you wouldn't print to the console at each simulation substep! + printf("got %d VR controller events!\n", vrEvents.m_numControllerEvents); + } + } + } + { + b3KeyboardEventsData keyboardEventsData; + b3SharedMemoryCommandHandle commandHandle = b3RequestKeyboardEventsCommandInit(context->m_physClient); + b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle); + b3GetKeyboardEventsData(context->m_physClient, &keyboardEventsData); + if (keyboardEventsData.m_numKeyboardEvents) + { + //this is only for a test, normally you wouldn't print to the console at each simulation substep! + printf("got %d keyboard events\n", keyboardEventsData.m_numKeyboardEvents); + } + } + + { + b3MouseEventsData mouseEventsData; + b3SharedMemoryCommandHandle commandHandle = b3RequestMouseEventsCommandInit(context->m_physClient); + b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle); + b3GetMouseEventsData(context->m_physClient, &mouseEventsData); + if (mouseEventsData.m_numMouseEvents) + { + //this is only for a test, normally you wouldn't print to the console at each simulation substep! + printf("got %d mouse events\n", mouseEventsData.m_numMouseEvents); + } + } + + return 0; +} + + +B3_SHARED_API int postTickPluginCallback(struct b3PluginContext* context) +{ + MyClass* obj = (MyClass* )context->m_userPointer; + obj->m_testData++; + return 0; +} + +B3_SHARED_API int executePluginCommand(struct b3PluginContext* context, const struct b3PluginArguments* arguments) +{ + printf("text argument:%s\n",arguments->m_text); + printf("int args: ["); + for (int i=0;im_numInts;i++) + { + printf("%d", arguments->m_ints[i]); + if ((i+1)m_numInts) + { + printf(","); + } + } + printf("]\nfloat args: ["); + for (int i=0;im_numFloats;i++) + { + printf("%f", arguments->m_floats[i]); + if ((i+1)m_numFloats) + { + printf(","); + } + } + printf("]\n"); + + MyClass* obj = (MyClass*) context->m_userPointer; + + b3SharedMemoryStatusHandle statusHandle; + int statusType = -1; + int bodyUniqueId = -1; + + b3SharedMemoryCommandHandle command = + b3LoadUrdfCommandInit(context->m_physClient, arguments->m_text); + + statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, command); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_URDF_LOADING_COMPLETED) + { + bodyUniqueId = b3GetStatusBodyIndex(statusHandle); + } + return bodyUniqueId; +} + + +B3_SHARED_API void exitPlugin(struct b3PluginContext* context) +{ + MyClass* obj = (MyClass*) context->m_userPointer; + delete obj; + context->m_userPointer = 0; + + printf("bye!\n"); +} diff --git a/examples/SharedMemory/plugins/testPlugin/testplugin.h b/examples/SharedMemory/plugins/testPlugin/testplugin.h new file mode 100644 index 000000000..bb14345cc --- /dev/null +++ b/examples/SharedMemory/plugins/testPlugin/testplugin.h @@ -0,0 +1,26 @@ +#ifndef TEST_PLUGIN_H +#define TEST_PLUGIN_H + +#include "../b3PluginAPI.h" + +#ifdef __cplusplus +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); + +//preTickPluginCallback and postTickPluginCallback are optional. +B3_SHARED_API int preTickPluginCallback(struct b3PluginContext* context); +B3_SHARED_API int postTickPluginCallback(struct b3PluginContext* context); + + + +#ifdef __cplusplus +}; +#endif + +#endif//#define TEST_PLUGIN_H diff --git a/examples/SharedMemory/plugins/vrSyncPlugin/premake4.lua b/examples/SharedMemory/plugins/vrSyncPlugin/premake4.lua new file mode 100644 index 000000000..f23cc6c7e --- /dev/null +++ b/examples/SharedMemory/plugins/vrSyncPlugin/premake4.lua @@ -0,0 +1,42 @@ + + +project ("pybullet_vrSyncPlugin") + language "C++" + kind "SharedLib" + + includedirs {".","../../../../src", "../../../../examples", + "../../../ThirdPartyLibs"} + defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"} + hasCL = findOpenCL("clew") + + links{"BulletFileLoader", "Bullet3Common", "LinearMath"} + + + if os.is("MacOSX") then +-- targetextension {"so"} + links{"Cocoa.framework","Python"} + end + + + files { + "vrSyncPlugin.cpp", + "../../PhysicsClient.cpp", + "../../PhysicsClient.h", + "../../PhysicsClientSharedMemory.cpp", + "../../PhysicsClientSharedMemory.h", + "../../PhysicsClientSharedMemory_C_API.cpp", + "../../PhysicsClientSharedMemory_C_API.h", + "../../PhysicsClientC_API.cpp", + "../../PhysicsClientC_API.h", + "../../Win32SharedMemory.cpp", + "../../Win32SharedMemory.h", + "../../PosixSharedMemory.cpp", + "../../PosixSharedMemory.h", + "../../../Utils/b3Clock.cpp", + "../../../Utils/b3Clock.h", + "../../../Utils/b3ResourcePath.cpp", + "../../../Utils/b3ResourcePath.h", + } + + + diff --git a/examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.cpp b/examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.cpp new file mode 100644 index 000000000..474e944df --- /dev/null +++ b/examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.cpp @@ -0,0 +1,209 @@ + +//vrSyncPlugin plugin, will query the vr controller events +//and set change the user constraint to match the pose + +//in Python you can load and configure the plugin like this: +//plugin = p.loadPlugin("e:/develop/bullet3/bin/pybullet_vrSyncPlugin_vs2010_x64_release.dll") +//could also be plugin = p.loadPlugin("vrSyncPlugin.so") on Mac/Linux +//controllerId = 3 + +#include "vrSyncPlugin.h" +#include "../../SharedMemoryPublic.h" +#include "../b3PluginContext.h" +#include + +struct MyClass +{ + int m_testData; + + int m_controllerId; + int m_constraintId; + int m_constraintId2; + int m_gripperId; + float m_maxForce; + float m_maxForce2; + MyClass() + :m_testData(42), + m_controllerId(-1), + m_constraintId(-1), + m_constraintId2(-1), + m_gripperId(-1), + m_maxForce(0), + m_maxForce2(0) + { + } + virtual ~MyClass() + { + } +}; + +B3_SHARED_API int initPlugin(struct b3PluginContext* context) +{ + MyClass* obj = new MyClass(); + context->m_userPointer = obj; + + printf("hi vrSyncPlugin!\n"); + return SHARED_MEMORY_MAGIC_NUMBER; +} + + +B3_SHARED_API int preTickPluginCallback(struct b3PluginContext* context) +{ + MyClass* obj = (MyClass* )context->m_userPointer; + if (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_controllerId) + { + if (obj->m_constraintId>=0) + { + //this is basically equivalent to doing this in Python/pybullet: + //p.changeConstraint(pr2_cid, e[POSITION], e[ORIENTATION], maxForce=...) + b3SharedMemoryCommandHandle commandHandle; + int userConstraintUniqueId = obj->m_constraintId; + commandHandle = b3InitChangeUserConstraintCommand(context->m_physClient, userConstraintUniqueId); + double pos[4] = {event.m_pos[0],event.m_pos[1],event.m_pos[2],1}; + b3InitChangeUserConstraintSetPivotInB(commandHandle, pos); + double orn[4] = {event.m_orn[0],event.m_orn[1],event.m_orn[2],event.m_orn[3]}; + b3InitChangeUserConstraintSetFrameInB(commandHandle, orn); + b3InitChangeUserConstraintSetMaxForce(commandHandle, obj->m_maxForce); + b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle); + } + // apply the analogue button to close the constraint, using a gear constraint with position target + if (obj->m_constraintId2>=0) + { + //this block is similar to + //p.changeConstraint(c,gearRatio=1, erp=..., relativePositionTarget=relPosTarget, maxForce=...) + //printf("obj->m_constraintId2=%d\n", obj->m_constraintId2); + b3SharedMemoryCommandHandle commandHandle; + commandHandle = b3InitChangeUserConstraintCommand(context->m_physClient, obj->m_constraintId2); + + //0 -> open, 1 = closed + double openPos = 1.; + double relPosTarget = openPos - (event.m_analogAxis*openPos); + b3InitChangeUserConstraintSetRelativePositionTarget(commandHandle, relPosTarget); + b3InitChangeUserConstraintSetERP(commandHandle,1); + b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle); + } + //printf("event.m_analogAxis=%f\n", event.m_analogAxis); + + // use the pr2_gripper motors to keep the gripper centered/symmetric around the center axis + if (obj->m_gripperId>=0) + { + //this block is similar to + //b = p.getJointState(pr2_gripper,2)[0] + //print("b = " + str(b)) + //p.setJointMotorControl2(pr2_gripper, 0, p.POSITION_CONTROL, targetPosition=b, force=3) + + //printf("obj->m_gripperId=%d\n", obj->m_gripperId); + { + b3SharedMemoryCommandHandle cmd_handle = + b3RequestActualStateCommandInit(context->m_physClient, obj->m_gripperId); + b3SharedMemoryStatusHandle status_handle = + b3SubmitClientCommandAndWaitStatus(context->m_physClient, cmd_handle); + + int status_type = b3GetStatusType(status_handle); + if (status_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED) + { + //printf("status_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED\n"); + + b3JointSensorState sensorState; + if (b3GetJointState(context->m_physClient, status_handle, 2, &sensorState)) + { + + + b3SharedMemoryCommandHandle commandHandle; + double targetPosition = sensorState.m_jointPosition; + //printf("targetPosition =%f\n", targetPosition); + if (1) + { + b3JointInfo info; + b3GetJointInfo(context->m_physClient, obj->m_gripperId, 0, &info); + commandHandle = b3JointControlCommandInit2(context->m_physClient, obj->m_gripperId, CONTROL_MODE_POSITION_VELOCITY_PD); + double kp = .1; + double targetVelocity = 0; + double kd = .6; + b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, targetPosition); + b3JointControlSetKp(commandHandle, info.m_uIndex, kp); + b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex,targetVelocity); + b3JointControlSetKd(commandHandle, info.m_uIndex, kd); + b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, obj->m_maxForce2); + b3SubmitClientCommandAndWaitStatus(context->m_physClient, cmd_handle); + } + } else + { + //printf("???\n"); + } + + } else + { + //printf("no\n"); + } + + } + + } + + } + } + } + } + } + + return 0; +} + + + +B3_SHARED_API int executePluginCommand(struct b3PluginContext* context, const struct b3PluginArguments* arguments) +{ + MyClass* obj = (MyClass*) context->m_userPointer; + if (arguments->m_numInts>=4 && arguments->m_numFloats >= 2) + { + obj->m_constraintId = arguments->m_ints[1]; + obj->m_constraintId2 = arguments->m_ints[2]; + obj->m_gripperId = arguments->m_ints[3]; + printf("obj->m_constraintId=%d\n", obj->m_constraintId); + obj->m_maxForce = arguments->m_floats[0]; + obj->m_maxForce2 = arguments->m_floats[1]; + printf("obj->m_maxForce = %f\n", obj->m_maxForce); + obj->m_controllerId = arguments->m_ints[0]; + printf("obj->m_controllerId=%d\n", obj->m_controllerId); + + b3SharedMemoryCommandHandle command = b3InitSyncBodyInfoCommand(context->m_physClient); + b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, command); + int statusType = b3GetStatusType(statusHandle); + + if (statusType != CMD_SYNC_BODY_INFO_COMPLETED) + { + + } + } + return 0; +} + + +B3_SHARED_API void exitPlugin(struct b3PluginContext* context) +{ + MyClass* obj = (MyClass*) context->m_userPointer; + delete obj; + context->m_userPointer = 0; + + printf("bye vrSyncPlugin!\n"); +} diff --git a/examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.h b/examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.h new file mode 100644 index 000000000..fcc60a610 --- /dev/null +++ b/examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.h @@ -0,0 +1,25 @@ +#ifndef TEST_PLUGIN_H +#define TEST_PLUGIN_H + +#include "../b3PluginAPI.h" + +#ifdef __cplusplus +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); + +//preTickPluginCallback and postTickPluginCallback are optional. +B3_SHARED_API int preTickPluginCallback(struct b3PluginContext* context); + + + +#ifdef __cplusplus +}; +#endif + +#endif//#define TEST_PLUGIN_H diff --git a/examples/SharedMemory/premake4.lua b/examples/SharedMemory/premake4.lua index b7d3ab9e7..f7c004b1e 100644 --- a/examples/SharedMemory/premake4.lua +++ b/examples/SharedMemory/premake4.lua @@ -7,7 +7,7 @@ else kind "ConsoleApp" end -includedirs {".","../../src", "../ThirdPartyLibs",} +includedirs {".","../../src", "../ThirdPartyLibs"} links { "Bullet3Common","BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision", "LinearMath", "BussIK" @@ -53,6 +53,8 @@ myfiles = "SharedMemoryCommandProcessor.h", "PhysicsServerCommandProcessor.cpp", "PhysicsServerCommandProcessor.h", + "b3PluginManager.cpp", + "b3PluginManager.h", "TinyRendererVisualShapeConverter.cpp", "TinyRendererVisualShapeConverter.h", "../TinyRenderer/geometry.cpp", @@ -99,6 +101,7 @@ myfiles = "../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp", "../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp", "../ThirdPartyLibs/stb_image/stb_image.cpp", + } files { @@ -405,4 +408,7 @@ end include "udp" include "tcp" +include "plugins/testPlugin" +include "plugins/vrSyncPlugin" + diff --git a/examples/SharedMemory/tcp/premake4.lua b/examples/SharedMemory/tcp/premake4.lua index b1d5a73ab..9c940402e 100644 --- a/examples/SharedMemory/tcp/premake4.lua +++ b/examples/SharedMemory/tcp/premake4.lua @@ -88,6 +88,9 @@ myfiles = "../SharedMemoryPublic.h", "../PhysicsServerCommandProcessor.cpp", "../PhysicsServerCommandProcessor.h", + "../b3PluginManager.cpp", + "../PhysicsDirect.cpp", + "../PhysicsClient.cpp", "../TinyRendererVisualShapeConverter.cpp", "../TinyRendererVisualShapeConverter.h", "../../TinyRenderer/geometry.cpp", diff --git a/examples/SharedMemory/udp/premake4.lua b/examples/SharedMemory/udp/premake4.lua index a316d6b1c..26aeebc0b 100644 --- a/examples/SharedMemory/udp/premake4.lua +++ b/examples/SharedMemory/udp/premake4.lua @@ -79,6 +79,9 @@ myfiles = "../SharedMemoryPublic.h", "../PhysicsServerCommandProcessor.cpp", "../PhysicsServerCommandProcessor.h", + "../b3PluginManager.cpp", + "../PhysicsDirect.cpp", + "../PhysicsClient.cpp", "../TinyRendererVisualShapeConverter.cpp", "../TinyRendererVisualShapeConverter.h", "../../TinyRenderer/geometry.cpp", diff --git a/examples/StandaloneMain/hellovr_opengl_main.cpp b/examples/StandaloneMain/hellovr_opengl_main.cpp index ad81cad3c..1e48f879f 100644 --- a/examples/StandaloneMain/hellovr_opengl_main.cpp +++ b/examples/StandaloneMain/hellovr_opengl_main.cpp @@ -1808,6 +1808,8 @@ void CMainApplication::RenderStereoTargets() m_app->m_instancingRenderer->getActiveCamera()->setCameraUpVector(mat[0],mat[1],mat[2]); m_app->m_instancingRenderer->getActiveCamera()->setVRCamera(viewMatLeft.get(),m_mat4ProjectionLeft.get()); m_app->m_instancingRenderer->updateCamera(m_app->getUpAxis()); + m_app->m_instancingRenderer->getActiveCamera()->setVRCamera(viewMatLeft.get(),m_mat4ProjectionLeft.get()); + } glBindFramebuffer( GL_FRAMEBUFFER, leftEyeDesc.m_nRenderFramebufferId ); @@ -1866,6 +1868,7 @@ void CMainApplication::RenderStereoTargets() Matrix4 viewMatRight = m_mat4eyePosRight * m_mat4HMDPose * rotYtoZ; m_app->m_instancingRenderer->getActiveCamera()->setVRCamera(viewMatRight.get(),m_mat4ProjectionRight.get()); m_app->m_instancingRenderer->updateCamera(m_app->getUpAxis()); + m_app->m_instancingRenderer->getActiveCamera()->setVRCamera(viewMatRight.get(),m_mat4ProjectionRight.get()); } glBindFramebuffer( GL_FRAMEBUFFER, rightEyeDesc.m_nRenderFramebufferId ); diff --git a/examples/pybullet/CMakeLists.txt b/examples/pybullet/CMakeLists.txt index c2f9fd49f..7ca72805b 100644 --- a/examples/pybullet/CMakeLists.txt +++ b/examples/pybullet/CMakeLists.txt @@ -43,6 +43,9 @@ SET(pybullet_SRCS ../../examples/SharedMemory/PhysicsDirectC_API.h ../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp ../../examples/SharedMemory/PhysicsServerCommandProcessor.h + ../../examples/SharedMemory/b3PluginManager.cpp + ../../examples/SharedMemory/b3PluginManager.h + ../../examples/SharedMemory/PhysicsClientSharedMemory.cpp ../../examples/SharedMemory/PhysicsClientSharedMemory.h ../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp diff --git a/examples/pybullet/examples/mimicJointConstraint.py b/examples/pybullet/examples/mimicJointConstraint.py index 60a0625d9..968a24354 100644 --- a/examples/pybullet/examples/mimicJointConstraint.py +++ b/examples/pybullet/examples/mimicJointConstraint.py @@ -23,6 +23,7 @@ p.changeConstraint(c,gearRatio=-1, maxForce=10000) p.setRealTimeSimulation(1) while(1): + p.setGravity(0,0,-10) time.sleep(0.01) #p.removeConstraint(c) - \ No newline at end of file + diff --git a/examples/pybullet/examples/quadruped.py b/examples/pybullet/examples/quadruped.py index 19527fa47..7e3ee1262 100644 --- a/examples/pybullet/examples/quadruped.py +++ b/examples/pybullet/examples/quadruped.py @@ -187,6 +187,7 @@ t = 0.0 t_end = t + 15 ref_time = time.time() while (t=0) + { + b3InitChangeUserConstraintSetERP(commandHandle, erp); + } + if (maxForce >= 0) { b3InitChangeUserConstraintSetMaxForce(commandHandle, maxForce); @@ -5559,9 +5580,9 @@ static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args, Py { int bodyUniqueIdA = -1; int bodyUniqueIdB = -1; - int linkIndexA = -2; - int linkIndexB = -2; - + int linkIndexA = -2; + int linkIndexB = -2; + b3SharedMemoryCommandHandle commandHandle; struct b3ContactInformation contactPointData; b3SharedMemoryStatusHandle statusHandle; @@ -5583,24 +5604,24 @@ static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args, Py } commandHandle = b3InitRequestContactPointInformation(sm); - if (bodyUniqueIdA>=0) - { - b3SetContactFilterBodyA(commandHandle, bodyUniqueIdA); - } - if (bodyUniqueIdB>=0) - { - b3SetContactFilterBodyB(commandHandle, bodyUniqueIdB); - } + if (bodyUniqueIdA>=0) + { + b3SetContactFilterBodyA(commandHandle, bodyUniqueIdA); + } + if (bodyUniqueIdB>=0) + { + b3SetContactFilterBodyB(commandHandle, bodyUniqueIdB); + } - if (linkIndexA>=-1) - { - b3SetContactFilterLinkA( commandHandle, linkIndexA); - } - if (linkIndexB >=-1) - { - b3SetContactFilterLinkB( commandHandle, linkIndexB); - } - + if (linkIndexA>=-1) + { + b3SetContactFilterLinkA( commandHandle, linkIndexA); + } + if (linkIndexB >=-1) + { + b3SetContactFilterLinkB( commandHandle, linkIndexB); + } + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED) @@ -6574,7 +6595,7 @@ static PyObject* pybullet_invertTransform(PyObject* self, PyErr_SetString(SpamError, "Invalid input: expected position [x,y,z] and orientation [x,y,z,w]."); return NULL; } - + /// quaternion <-> euler yaw/pitch/roll convention from URDF/SDF, see Gazebo /// https://github.com/arpg/Gazebo/blob/master/gazebo/math/Quaternion.cc @@ -6653,6 +6674,131 @@ static PyObject* pybullet_getEulerFromQuaternion(PyObject* self, return Py_None; } + +static PyObject* pybullet_loadPlugin(PyObject* self, + PyObject* args, PyObject* keywds) +{ + int physicsClientId = 0; + + char* pluginPath = 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)) + { + return NULL; + } + + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + command = b3CreateCustomCommand(sm); + b3CustomCommandLoadPlugin(command, pluginPath); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusPluginUniqueId(statusHandle); + return PyInt_FromLong(statusType); +} + + +static PyObject* pybullet_unloadPlugin(PyObject* self, + PyObject* args, PyObject* keywds) +{ + int physicsClientId = 0; + int pluginUniqueId = -1; + + b3SharedMemoryCommandHandle command = 0; + b3SharedMemoryStatusHandle statusHandle = 0; + int statusType = -1; + + b3PhysicsClientHandle sm = 0; + static char* kwlist[] = { "pluginUniqueId", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &pluginUniqueId,&physicsClientId)) + { + return NULL; + } + + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + command = b3CreateCustomCommand(sm); + b3CustomCommandUnloadPlugin(command, pluginUniqueId); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + + Py_INCREF(Py_None); + return Py_None;; +} + +//createCustomCommand for executing commands implemented in a plugin system +static PyObject* pybullet_executePluginCommand(PyObject* self, + PyObject* args, PyObject* keywds) +{ + int physicsClientId = 0; + int pluginUniqueId = -1; + char* textArgument = 0; + b3SharedMemoryCommandHandle command=0; + b3SharedMemoryStatusHandle statusHandle=0; + int statusType = -1; + PyObject* intArgs=0; + PyObject* floatArgs=0; + + b3PhysicsClientHandle sm = 0; + static char* kwlist[] = { "pluginUniqueId", "textArgument", "intArgs", "floatArgs", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|sOOi", kwlist, &pluginUniqueId, &textArgument, &intArgs, &floatArgs, &physicsClientId)) + { + return NULL; + } + + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + + command = b3CreateCustomCommand(sm); + b3CustomCommandExecutePluginCommand(command, pluginUniqueId, textArgument); + + { + PyObject* seqIntArgs = intArgs?PySequence_Fast(intArgs, "expected a sequence"):0; + PyObject* seqFloatArgs = floatArgs?PySequence_Fast(floatArgs, "expected a sequence"):0; + int numIntArgs = seqIntArgs?PySequence_Size(intArgs):0; + int numFloatArgs = seqIntArgs?PySequence_Size(floatArgs):0; + int i; + for (i=0;i + /// Error generating expression: Value cannot be null. + ///Parameter name: node + public const string PHYSICS_CLIENT_C_API_H = ""; + + /// SHARED_MEMORY_PUBLIC_H -> + /// Error generating expression: Value cannot be null. + ///Parameter name: node + public const string SHARED_MEMORY_PUBLIC_H = ""; + + /// SHARED_MEMORY_KEY -> 12347 + public const int SHARED_MEMORY_KEY = 12347; + + /// SHARED_MEMORY_MAGIC_NUMBER -> 201708270 + public const int SHARED_MEMORY_MAGIC_NUMBER = 201708270; + + /// MAX_VR_BUTTONS -> 64 + public const int MAX_VR_BUTTONS = 64; + + /// MAX_VR_CONTROLLERS -> 8 + public const int MAX_VR_CONTROLLERS = 8; + + /// MAX_RAY_INTERSECTION_BATCH_SIZE -> 256 + public const int MAX_RAY_INTERSECTION_BATCH_SIZE = 256; + + /// MAX_RAY_HITS -> MAX_RAY_INTERSECTION_BATCH_SIZE + public const int MAX_RAY_HITS = NativeConstants.MAX_RAY_INTERSECTION_BATCH_SIZE; + + /// MAX_KEYBOARD_EVENTS -> 256 + public const int MAX_KEYBOARD_EVENTS = 256; + + /// MAX_MOUSE_EVENTS -> 256 + public const int MAX_MOUSE_EVENTS = 256; + + /// VISUAL_SHAPE_MAX_PATH_LEN -> 1024 + public const int VISUAL_SHAPE_MAX_PATH_LEN = 1024; + + /// Warning: Generation of Method Macros is not supported at this time + /// B3_DECLARE_HANDLE -> "(name) typedef struct name##__ { int unused; } *name" + public const string B3_DECLARE_HANDLE = "(name) typedef struct name##__ { int unused; } *name"; + + /// PHYSICS_CLIENT_SHARED_MEMORY_H -> + /// Error generating expression: Value cannot be null. + ///Parameter name: node + public const string PHYSICS_CLIENT_SHARED_MEMORY_H = ""; + + /// PHYSICS_CLIENT_SHARED_MEMORY2_H -> + /// Error generating expression: Value cannot be null. + ///Parameter name: node + public const string PHYSICS_CLIENT_SHARED_MEMORY2_H = ""; + + /// PHYSICS_DIRECT_C_API_H -> + /// Error generating expression: Value cannot be null. + ///Parameter name: node + public const string PHYSICS_DIRECT_C_API_H = ""; + + /// IN_PROCESS_PHYSICS_C_API_H -> + /// Error generating expression: Value cannot be null. + ///Parameter name: node + public const string IN_PROCESS_PHYSICS_C_API_H = ""; +} + +public enum EnumSharedMemoryClientCommand { + + CMD_LOAD_SDF, + + CMD_LOAD_URDF, + + CMD_LOAD_BULLET, + + CMD_SAVE_BULLET, + + CMD_LOAD_MJCF, + + CMD_LOAD_BUNNY, + + CMD_SEND_BULLET_DATA_STREAM, + + CMD_CREATE_BOX_COLLISION_SHAPE, + + CMD_CREATE_RIGID_BODY, + + CMD_DELETE_RIGID_BODY, + + CMD_CREATE_SENSOR, + + CMD_INIT_POSE, + + CMD_SEND_PHYSICS_SIMULATION_PARAMETERS, + + CMD_SEND_DESIRED_STATE, + + CMD_REQUEST_ACTUAL_STATE, + + CMD_REQUEST_DEBUG_LINES, + + CMD_REQUEST_BODY_INFO, + + CMD_REQUEST_INTERNAL_DATA, + + CMD_STEP_FORWARD_SIMULATION, + + CMD_RESET_SIMULATION, + + CMD_PICK_BODY, + + CMD_MOVE_PICKED_BODY, + + CMD_REMOVE_PICKING_CONSTRAINT_BODY, + + CMD_REQUEST_CAMERA_IMAGE_DATA, + + CMD_APPLY_EXTERNAL_FORCE, + + CMD_CALCULATE_INVERSE_DYNAMICS, + + CMD_CALCULATE_INVERSE_KINEMATICS, + + CMD_CALCULATE_JACOBIAN, + + CMD_USER_CONSTRAINT, + + CMD_REQUEST_CONTACT_POINT_INFORMATION, + + CMD_REQUEST_RAY_CAST_INTERSECTIONS, + + CMD_REQUEST_AABB_OVERLAP, + + CMD_SAVE_WORLD, + + CMD_REQUEST_VISUAL_SHAPE_INFO, + + CMD_UPDATE_VISUAL_SHAPE, + + CMD_LOAD_TEXTURE, + + CMD_SET_SHADOW, + + CMD_USER_DEBUG_DRAW, + + CMD_REQUEST_VR_EVENTS_DATA, + + CMD_SET_VR_CAMERA_STATE, + + CMD_SYNC_BODY_INFO, + + CMD_STATE_LOGGING, + + CMD_CONFIGURE_OPENGL_VISUALIZER, + + CMD_REQUEST_KEYBOARD_EVENTS_DATA, + + CMD_REQUEST_OPENGL_VISUALIZER_CAMERA, + + CMD_REMOVE_BODY, + + CMD_CHANGE_DYNAMICS_INFO, + + CMD_GET_DYNAMICS_INFO, + + CMD_PROFILE_TIMING, + + CMD_CREATE_COLLISION_SHAPE, + + CMD_CREATE_VISUAL_SHAPE, + + CMD_CREATE_MULTI_BODY, + + CMD_REQUEST_COLLISION_INFO, + + CMD_REQUEST_MOUSE_EVENTS_DATA, + + CMD_CHANGE_TEXTURE, + + CMD_SET_ADDITIONAL_SEARCH_PATH, + + CMD_MAX_CLIENT_COMMANDS, +} + +public enum EnumSharedMemoryServerStatus { + + /// CMD_SHARED_MEMORY_NOT_INITIALIZED -> 0 + CMD_SHARED_MEMORY_NOT_INITIALIZED = 0, + + CMD_WAITING_FOR_CLIENT_COMMAND, + + CMD_CLIENT_COMMAND_COMPLETED, + + CMD_UNKNOWN_COMMAND_FLUSHED, + + CMD_SDF_LOADING_COMPLETED, + + CMD_SDF_LOADING_FAILED, + + CMD_URDF_LOADING_COMPLETED, + + CMD_URDF_LOADING_FAILED, + + CMD_BULLET_LOADING_COMPLETED, + + CMD_BULLET_LOADING_FAILED, + + CMD_BULLET_SAVING_COMPLETED, + + CMD_BULLET_SAVING_FAILED, + + CMD_MJCF_LOADING_COMPLETED, + + CMD_MJCF_LOADING_FAILED, + + CMD_REQUEST_INTERNAL_DATA_COMPLETED, + + CMD_REQUEST_INTERNAL_DATA_FAILED, + + CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED, + + CMD_BULLET_DATA_STREAM_RECEIVED_FAILED, + + CMD_BOX_COLLISION_SHAPE_CREATION_COMPLETED, + + CMD_RIGID_BODY_CREATION_COMPLETED, + + CMD_SET_JOINT_FEEDBACK_COMPLETED, + + CMD_ACTUAL_STATE_UPDATE_COMPLETED, + + CMD_ACTUAL_STATE_UPDATE_FAILED, + + CMD_DEBUG_LINES_COMPLETED, + + CMD_DEBUG_LINES_OVERFLOW_FAILED, + + CMD_DESIRED_STATE_RECEIVED_COMPLETED, + + CMD_STEP_FORWARD_SIMULATION_COMPLETED, + + CMD_RESET_SIMULATION_COMPLETED, + + CMD_CAMERA_IMAGE_COMPLETED, + + CMD_CAMERA_IMAGE_FAILED, + + CMD_BODY_INFO_COMPLETED, + + CMD_BODY_INFO_FAILED, + + CMD_INVALID_STATUS, + + CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED, + + CMD_CALCULATED_INVERSE_DYNAMICS_FAILED, + + CMD_CALCULATED_JACOBIAN_COMPLETED, + + CMD_CALCULATED_JACOBIAN_FAILED, + + CMD_CONTACT_POINT_INFORMATION_COMPLETED, + + CMD_CONTACT_POINT_INFORMATION_FAILED, + + CMD_REQUEST_AABB_OVERLAP_COMPLETED, + + CMD_REQUEST_AABB_OVERLAP_FAILED, + + CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED, + + CMD_CALCULATE_INVERSE_KINEMATICS_FAILED, + + CMD_SAVE_WORLD_COMPLETED, + + CMD_SAVE_WORLD_FAILED, + + CMD_VISUAL_SHAPE_INFO_COMPLETED, + + CMD_VISUAL_SHAPE_INFO_FAILED, + + CMD_VISUAL_SHAPE_UPDATE_COMPLETED, + + CMD_VISUAL_SHAPE_UPDATE_FAILED, + + CMD_LOAD_TEXTURE_COMPLETED, + + CMD_LOAD_TEXTURE_FAILED, + + CMD_USER_DEBUG_DRAW_COMPLETED, + + CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED, + + CMD_USER_DEBUG_DRAW_FAILED, + + CMD_USER_CONSTRAINT_COMPLETED, + + CMD_USER_CONSTRAINT_INFO_COMPLETED, + + CMD_REMOVE_USER_CONSTRAINT_COMPLETED, + + CMD_CHANGE_USER_CONSTRAINT_COMPLETED, + + CMD_REMOVE_USER_CONSTRAINT_FAILED, + + CMD_CHANGE_USER_CONSTRAINT_FAILED, + + CMD_USER_CONSTRAINT_FAILED, + + CMD_REQUEST_VR_EVENTS_DATA_COMPLETED, + + CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED, + + CMD_SYNC_BODY_INFO_COMPLETED, + + CMD_SYNC_BODY_INFO_FAILED, + + CMD_STATE_LOGGING_COMPLETED, + + CMD_STATE_LOGGING_START_COMPLETED, + + CMD_STATE_LOGGING_FAILED, + + CMD_REQUEST_KEYBOARD_EVENTS_DATA_COMPLETED, + + CMD_REQUEST_KEYBOARD_EVENTS_DATA_FAILED, + + CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_FAILED, + + CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_COMPLETED, + + CMD_REMOVE_BODY_COMPLETED, + + CMD_REMOVE_BODY_FAILED, + + CMD_GET_DYNAMICS_INFO_COMPLETED, + + CMD_GET_DYNAMICS_INFO_FAILED, + + CMD_CREATE_COLLISION_SHAPE_FAILED, + + CMD_CREATE_COLLISION_SHAPE_COMPLETED, + + CMD_CREATE_VISUAL_SHAPE_FAILED, + + CMD_CREATE_VISUAL_SHAPE_COMPLETED, + + CMD_CREATE_MULTI_BODY_FAILED, + + CMD_CREATE_MULTI_BODY_COMPLETED, + + CMD_REQUEST_COLLISION_INFO_COMPLETED, + + CMD_REQUEST_COLLISION_INFO_FAILED, + + CMD_REQUEST_MOUSE_EVENTS_DATA_COMPLETED, + + CMD_CHANGE_TEXTURE_COMMAND_FAILED, + + CMD_MAX_SERVER_COMMANDS, +} + +public enum JointInfoFlags { + + /// JOINT_HAS_MOTORIZED_POWER -> 1 + JOINT_HAS_MOTORIZED_POWER = 1, +} + +public enum JointType { + + /// eRevoluteType -> 0 + eRevoluteType = 0, + + /// ePrismaticType -> 1 + ePrismaticType = 1, + + /// eSphericalType -> 2 + eSphericalType = 2, + + /// ePlanarType -> 3 + ePlanarType = 3, + + /// eFixedType -> 4 + eFixedType = 4, + + /// ePoint2PointType -> 5 + ePoint2PointType = 5, + + /// eGearType -> 6 + eGearType = 6, +} + +public enum b3JointInfoFlags { + + /// eJointChangeMaxForce -> 1 + eJointChangeMaxForce = 1, + + /// eJointChangeChildFramePosition -> 2 + eJointChangeChildFramePosition = 2, + + /// eJointChangeChildFrameOrientation -> 4 + eJointChangeChildFrameOrientation = 4, +} + +[System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential)] +public struct b3JointInfo { + + /// char* + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)] + public string m_linkName; + + /// char* + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)] + public string m_jointName; + + /// int + public int m_jointType; + + /// int + public int m_qIndex; + + /// int + public int m_uIndex; + + /// int + public int m_jointIndex; + + /// int + public int m_flags; + + /// double + public double m_jointDamping; + + /// double + public double m_jointFriction; + + /// double + public double m_jointLowerLimit; + + /// double + public double m_jointUpperLimit; + + /// double + public double m_jointMaxForce; + + /// double + public double m_jointMaxVelocity; + + /// double[7] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=7, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R8)] + public double[] m_parentFrame; + + /// double[7] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=7, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R8)] + public double[] m_childFrame; + + /// double[3] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=3, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R8)] + public double[] m_jointAxis; +} + +[System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential)] +public struct b3UserConstraint { + + /// int + public int m_parentBodyIndex; + + /// int + public int m_parentJointIndex; + + /// int + public int m_childBodyIndex; + + /// int + public int m_childJointIndex; + + /// double[7] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=7, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R8)] + public double[] m_parentFrame; + + /// double[7] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=7, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R8)] + public double[] m_childFrame; + + /// double[3] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=3, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R8)] + public double[] m_jointAxis; + + /// int + public int m_jointType; + + /// double + public double m_maxAppliedForce; + + /// int + public int m_userConstraintUniqueId; + + /// double + public double m_gearRatio; + + /// int + public int m_gearAuxLink; +} + +[System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential)] +public struct b3BodyInfo { + + /// char* + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)] + public string m_baseName; + + /// char* + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)] + public string m_bodyName; +} + +[System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential)] +public struct b3DynamicsInfo { + + /// double + public double m_mass; + + /// double[3] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=3, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R8)] + public double[] m_localInertialPosition; + + /// double + public double m_lateralFrictionCoeff; +} + +public enum SensorType { + + /// eSensorForceTorqueType -> 1 + eSensorForceTorqueType = 1, +} + +[System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential)] +public struct b3JointSensorState { + + /// double + public double m_jointPosition; + + /// double + public double m_jointVelocity; + + /// double[6] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=6, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R8)] + public double[] m_jointForceTorque; + + /// double + public double m_jointMotorTorque; +} + +[System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential)] +public struct b3DebugLines { + + /// int + public int m_numDebugLines; + + /// float* + public System.IntPtr m_linesFrom; + + /// float* + public System.IntPtr m_linesTo; + + /// float* + public System.IntPtr m_linesColor; +} + +[System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential)] +public struct b3OverlappingObject { + + /// int + public int m_objectUniqueId; + + /// int + public int m_linkIndex; +} + +[System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential)] +public struct b3AABBOverlapData { + + /// int + public int m_numOverlappingObjects; + + /// b3OverlappingObject* + public System.IntPtr m_overlappingObjects; +} + +[System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential)] +public struct b3CameraImageData { + + /// int + public int m_pixelWidth; + + /// int + public int m_pixelHeight; + + /// char* + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)] + public string m_rgbColorData; + + /// float* + public System.IntPtr m_depthValues; + + /// int* + public System.IntPtr m_segmentationMaskValues; +} + +[System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential)] +public struct b3OpenGLVisualizerCameraInfo { + + /// int + public int m_width; + + /// int + public int m_height; + + /// float[16] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=16, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R4)] + public float[] m_viewMatrix; + + /// float[16] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=16, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R4)] + public float[] m_projectionMatrix; + + /// float[3] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=3, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R4)] + public float[] m_camUp; + + /// float[3] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=3, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R4)] + public float[] m_camForward; + + /// float[3] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=3, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R4)] + public float[] m_horizontal; + + /// float[3] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=3, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R4)] + public float[] m_vertical; + + /// float + public float m_yaw; + + /// float + public float m_pitch; + + /// float + public float m_dist; + + /// float[3] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=3, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R4)] + public float[] m_target; +} + +public enum b3VREventType { + + /// VR_CONTROLLER_MOVE_EVENT -> 1 + VR_CONTROLLER_MOVE_EVENT = 1, + + /// VR_CONTROLLER_BUTTON_EVENT -> 2 + VR_CONTROLLER_BUTTON_EVENT = 2, + + /// VR_HMD_MOVE_EVENT -> 4 + VR_HMD_MOVE_EVENT = 4, + + /// VR_GENERIC_TRACKER_MOVE_EVENT -> 8 + VR_GENERIC_TRACKER_MOVE_EVENT = 8, +} + +public enum b3VRButtonInfo { + + /// eButtonIsDown -> 1 + eButtonIsDown = 1, + + /// eButtonTriggered -> 2 + eButtonTriggered = 2, + + /// eButtonReleased -> 4 + eButtonReleased = 4, +} + +public enum eVRDeviceTypeEnums { + + /// VR_DEVICE_CONTROLLER -> 1 + VR_DEVICE_CONTROLLER = 1, + + /// VR_DEVICE_HMD -> 2 + VR_DEVICE_HMD = 2, + + /// VR_DEVICE_GENERIC_TRACKER -> 4 + VR_DEVICE_GENERIC_TRACKER = 4, +} + +public enum EVRCameraFlags { + + /// VR_CAMERA_TRACK_OBJECT_ORIENTATION -> 1 + VR_CAMERA_TRACK_OBJECT_ORIENTATION = 1, +} + +[System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential)] +public struct b3VRControllerEvent { + + /// int + public int m_controllerId; + + /// int + public int m_deviceType; + + /// int + public int m_numMoveEvents; + + /// int + public int m_numButtonEvents; + + /// float[4] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=4, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R4)] + public float[] m_pos; + + /// float[4] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=4, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R4)] + public float[] m_orn; + + /// float + public float m_analogAxis; + + /// int[64] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=64, ArraySubType=System.Runtime.InteropServices.UnmanagedType.I4)] + public int[] m_buttons; +} + +[System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential)] +public struct b3VREventsData { + + /// int + public int m_numControllerEvents; + + /// b3VRControllerEvent* + public System.IntPtr m_controllerEvents; +} + +[System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential)] +public struct b3KeyboardEvent { + + /// int + public int m_keyCode; + + /// int + public int m_keyState; +} + +[System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential)] +public struct b3KeyboardEventsData { + + /// int + public int m_numKeyboardEvents; + + /// b3KeyboardEvent* + public System.IntPtr m_keyboardEvents; +} + +public enum eMouseEventTypeEnums { + + /// MOUSE_MOVE_EVENT -> 1 + MOUSE_MOVE_EVENT = 1, + + /// MOUSE_BUTTON_EVENT -> 2 + MOUSE_BUTTON_EVENT = 2, +} + +[System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential)] +public struct b3MouseEvent { + + /// int + public int m_eventType; + + /// float + public float m_mousePosX; + + /// float + public float m_mousePosY; + + /// int + public int m_buttonIndex; + + /// int + public int m_buttonState; +} + +[System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential)] +public struct b3MouseEventsData { + + /// int + public int m_numMouseEvents; + + /// b3MouseEvent* + public System.IntPtr m_mouseEvents; +} + +[System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential)] +public struct b3ContactPointData { + + /// int + public int m_contactFlags; + + /// int + public int m_bodyUniqueIdA; + + /// int + public int m_bodyUniqueIdB; + + /// int + public int m_linkIndexA; + + /// int + public int m_linkIndexB; + + /// double[3] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=3, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R8)] + public double[] m_positionOnAInWS; + + /// double[3] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=3, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R8)] + public double[] m_positionOnBInWS; + + /// double[3] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=3, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R8)] + public double[] m_contactNormalOnBInWS; + + /// double + public double m_contactDistance; + + /// double + public double m_normalForce; +} + +public enum b3StateLoggingType { + + /// STATE_LOGGING_MINITAUR -> 0 + STATE_LOGGING_MINITAUR = 0, + + /// STATE_LOGGING_GENERIC_ROBOT -> 1 + STATE_LOGGING_GENERIC_ROBOT = 1, + + /// STATE_LOGGING_VR_CONTROLLERS -> 2 + STATE_LOGGING_VR_CONTROLLERS = 2, + + /// STATE_LOGGING_VIDEO_MP4 -> 3 + STATE_LOGGING_VIDEO_MP4 = 3, + + /// STATE_LOGGING_COMMANDS -> 4 + STATE_LOGGING_COMMANDS = 4, + + /// STATE_LOGGING_CONTACT_POINTS -> 5 + STATE_LOGGING_CONTACT_POINTS = 5, + + /// STATE_LOGGING_PROFILE_TIMINGS -> 6 + STATE_LOGGING_PROFILE_TIMINGS = 6, +} + +[System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential)] +public struct b3ContactInformation { + + /// int + public int m_numContactPoints; + + /// b3ContactPointData* + public System.IntPtr m_contactPointData; +} + +[System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential)] +public struct b3RayHitInfo { + + /// double + public double m_hitFraction; + + /// int + public int m_hitObjectUniqueId; + + /// int + public int m_hitObjectLinkIndex; + + /// double[3] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=3, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R8)] + public double[] m_hitPositionWorld; + + /// double[3] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=3, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R8)] + public double[] m_hitNormalWorld; +} + +[System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential)] +public struct b3RaycastInformation { + + /// int + public int m_numRayHits; + + /// b3RayHitInfo* + public System.IntPtr m_rayHits; +} + +[System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential, CharSet=System.Runtime.InteropServices.CharSet.Ansi)] +public struct b3VisualShapeData { + + /// int + public int m_objectUniqueId; + + /// int + public int m_linkIndex; + + /// int + public int m_visualGeometryType; + + /// double[3] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=3, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R8)] + public double[] m_dimensions; + + /// char[1024] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValTStr, SizeConst=1024)] + public string m_meshAssetFileName; + + /// double[7] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=7, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R8)] + public double[] m_localVisualFrame; + + /// double[4] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=4, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R8)] + public double[] m_rgbaColor; +} + +[System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential)] +public struct b3VisualShapeInformation { + + /// int + public int m_numVisualShapes; + + /// b3VisualShapeData* + public System.IntPtr m_visualShapeData; +} + +public enum eLinkStateFlags { + + /// ACTUAL_STATE_COMPUTE_LINKVELOCITY -> 1 + ACTUAL_STATE_COMPUTE_LINKVELOCITY = 1, +} + +[System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential)] +public struct b3LinkState { + + /// double[3] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=3, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R8)] + public double[] m_worldPosition; + + /// double[4] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=4, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R8)] + public double[] m_worldOrientation; + + /// double[3] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=3, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R8)] + public double[] m_localInertialPosition; + + /// double[4] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=4, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R8)] + public double[] m_localInertialOrientation; + + /// double[3] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=3, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R8)] + public double[] m_worldLinkFramePosition; + + /// double[4] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=4, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R8)] + public double[] m_worldLinkFrameOrientation; + + /// double[3] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=3, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R8)] + public double[] m_worldLinearVelocity; + + /// double[3] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=3, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R8)] + public double[] m_worldAngularVelocity; + + /// double[3] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=3, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R8)] + public double[] m_worldAABBMin; + + /// double[3] + [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValArray, SizeConst=3, ArraySubType=System.Runtime.InteropServices.UnmanagedType.R8)] + public double[] m_worldAABBMax; +} + +public enum EnumExternalForceFlags { + + /// EF_LINK_FRAME -> 1 + EF_LINK_FRAME = 1, + + /// EF_WORLD_FRAME -> 2 + EF_WORLD_FRAME = 2, +} + +public enum EnumRenderer { + + /// ER_TINY_RENDERER -> (1<<16) + ER_TINY_RENDERER = (1) << (16), + + /// ER_BULLET_HARDWARE_OPENGL -> (1<<17) + ER_BULLET_HARDWARE_OPENGL = (1) << (17), +} + +public enum b3ConfigureDebugVisualizerEnum { + + /// COV_ENABLE_GUI -> 1 + COV_ENABLE_GUI = 1, + + COV_ENABLE_SHADOWS, + + COV_ENABLE_WIREFRAME, + + COV_ENABLE_VR_TELEPORTING, + + COV_ENABLE_VR_PICKING, + + COV_ENABLE_VR_RENDER_CONTROLLERS, + + COV_ENABLE_RENDERING, + + COV_ENABLE_SYNC_RENDERING_INTERNAL, + + COV_ENABLE_KEYBOARD_SHORTCUTS, + + COV_ENABLE_MOUSE_PICKING, +} + +public enum b3AddUserDebugItemEnum { + + /// DEB_DEBUG_TEXT_USE_ORIENTATION -> 1 + DEB_DEBUG_TEXT_USE_ORIENTATION = 1, + + /// DEB_DEBUG_TEXT_USE_TRUE_TYPE_FONTS -> 2 + DEB_DEBUG_TEXT_USE_TRUE_TYPE_FONTS = 2, + + /// DEB_DEBUG_TEXT_HAS_TRACKING_OBJECT -> 4 + DEB_DEBUG_TEXT_HAS_TRACKING_OBJECT = 4, +} + +public enum eCONNECT_METHOD { + + /// eCONNECT_GUI -> 1 + eCONNECT_GUI = 1, + + /// eCONNECT_DIRECT -> 2 + eCONNECT_DIRECT = 2, + + /// eCONNECT_SHARED_MEMORY -> 3 + eCONNECT_SHARED_MEMORY = 3, + + /// eCONNECT_UDP -> 4 + eCONNECT_UDP = 4, + + /// eCONNECT_TCP -> 5 + eCONNECT_TCP = 5, + + /// eCONNECT_EXISTING_EXAMPLE_BROWSER -> 6 + eCONNECT_EXISTING_EXAMPLE_BROWSER = 6, + + /// eCONNECT_GUI_SERVER -> 7 + eCONNECT_GUI_SERVER = 7, +} + +public enum eURDF_Flags { + + /// URDF_USE_INERTIA_FROM_FILE -> 2 + URDF_USE_INERTIA_FROM_FILE = 2, + + /// URDF_USE_SELF_COLLISION -> 8 + URDF_USE_SELF_COLLISION = 8, + + /// URDF_USE_SELF_COLLISION_EXCLUDE_PARENT -> 16 + URDF_USE_SELF_COLLISION_EXCLUDE_PARENT = 16, + + /// URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS -> 32 + URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS = 32, + + /// URDF_RESERVED -> 64 + URDF_RESERVED = 64, +} + +public enum eUrdfGeomTypes { + + /// GEOM_SPHERE -> 2 + GEOM_SPHERE = 2, + + GEOM_BOX, + + GEOM_CYLINDER, + + GEOM_MESH, + + GEOM_PLANE, + + GEOM_CAPSULE, + + GEOM_UNKNOWN, +} + +public enum eUrdfCollisionFlags { + + /// GEOM_FORCE_CONCAVE_TRIMESH -> 1 + GEOM_FORCE_CONCAVE_TRIMESH = 1, +} + +public enum eStateLoggingFlags { + + /// STATE_LOG_JOINT_MOTOR_TORQUES -> 1 + STATE_LOG_JOINT_MOTOR_TORQUES = 1, + + /// STATE_LOG_JOINT_USER_TORQUES -> 2 + STATE_LOG_JOINT_USER_TORQUES = 2, + + /// STATE_LOG_JOINT_TORQUES -> STATE_LOG_JOINT_MOTOR_TORQUES+STATE_LOG_JOINT_USER_TORQUES + STATE_LOG_JOINT_TORQUES = (eStateLoggingFlags.STATE_LOG_JOINT_MOTOR_TORQUES + eStateLoggingFlags.STATE_LOG_JOINT_USER_TORQUES), +} + + + +public partial class NativeMethods { + + /// Return Type: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///key: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3ConnectSharedMemory")] +public static extern System.IntPtr b3ConnectSharedMemory(int key) ; + + + /// Return Type: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///key: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3ConnectSharedMemory2")] +public static extern System.IntPtr b3ConnectSharedMemory2(int key) ; + + + /// Return Type: b3PhysicsClientHandle->b3PhysicsClientHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3ConnectPhysicsDirect")] +public static extern System.IntPtr b3ConnectPhysicsDirect() ; + + + /// Return Type: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///argc: int + ///argv: char** + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreateInProcessPhysicsServerAndConnect")] +public static extern System.IntPtr b3CreateInProcessPhysicsServerAndConnect(int argc, ref System.IntPtr argv) ; + + + /// Return Type: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///argc: int + ///argv: char** + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreateInProcessPhysicsServerAndConnectSharedMemory")] +public static extern System.IntPtr b3CreateInProcessPhysicsServerAndConnectSharedMemory(int argc, ref System.IntPtr argv) ; + + + /// Return Type: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///argc: int + ///argv: char** + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreateInProcessPhysicsServerAndConnectMainThread")] +public static extern System.IntPtr b3CreateInProcessPhysicsServerAndConnectMainThread(int argc, ref System.IntPtr argv) ; + + + /// Return Type: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///argc: int + ///argv: char** + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreateInProcessPhysicsServerAndConnectMainThreadSharedMemory")] +public static extern System.IntPtr b3CreateInProcessPhysicsServerAndConnectMainThreadSharedMemory(int argc, ref System.IntPtr argv) ; + + + /// Return Type: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///guiHelperPtr: void* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect")] +public static extern System.IntPtr b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(System.IntPtr guiHelperPtr) ; + + + /// Return Type: void + ///clientHandle: b3PhysicsClientHandle->b3PhysicsClientHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3InProcessRenderSceneInternal")] +public static extern void b3InProcessRenderSceneInternal(System.IntPtr clientHandle) ; + + + /// Return Type: void + ///clientHandle: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///debugDrawMode: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3InProcessDebugDrawInternal")] +public static extern void b3InProcessDebugDrawInternal(System.IntPtr clientHandle, int debugDrawMode) ; + + + /// Return Type: int + ///clientHandle: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///x: float + ///y: float + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3InProcessMouseMoveCallback")] +public static extern int b3InProcessMouseMoveCallback(System.IntPtr clientHandle, float x, float y) ; + + + /// Return Type: int + ///clientHandle: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///button: int + ///state: int + ///x: float + ///y: float + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3InProcessMouseButtonCallback")] +public static extern int b3InProcessMouseButtonCallback(System.IntPtr clientHandle, int button, int state, float x, float y) ; + + + /// Return Type: void + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3DisconnectSharedMemory")] +public static extern void b3DisconnectSharedMemory(System.IntPtr physClient) ; + + + /// Return Type: int + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CanSubmitCommand")] +public static extern int b3CanSubmitCommand(System.IntPtr physClient) ; + + + /// Return Type: b3SharedMemoryStatusHandle->b3SharedMemoryStatusHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3SubmitClientCommandAndWaitStatus")] +public static extern System.IntPtr b3SubmitClientCommandAndWaitStatus(System.IntPtr physClient, System.IntPtr commandHandle) ; + + + /// Return Type: int + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3SubmitClientCommand")] +public static extern int b3SubmitClientCommand(System.IntPtr physClient, System.IntPtr commandHandle) ; + + + /// Return Type: b3SharedMemoryStatusHandle->b3SharedMemoryStatusHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3ProcessServerStatus")] +public static extern System.IntPtr b3ProcessServerStatus(System.IntPtr physClient) ; + + + /// Return Type: int + ///statusHandle: b3SharedMemoryStatusHandle->b3SharedMemoryStatusHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3GetStatusType")] +public static extern int b3GetStatusType(System.IntPtr statusHandle) ; + + + /// Return Type: int + ///statusHandle: b3SharedMemoryStatusHandle->b3SharedMemoryStatusHandle__* + ///bodyIndicesOut: int* + ///bodyIndicesCapacity: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3GetStatusBodyIndices")] +public static extern int b3GetStatusBodyIndices(System.IntPtr statusHandle, ref int bodyIndicesOut, int bodyIndicesCapacity) ; + + + /// Return Type: int + ///statusHandle: b3SharedMemoryStatusHandle->b3SharedMemoryStatusHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3GetStatusBodyIndex")] +public static extern int b3GetStatusBodyIndex(System.IntPtr statusHandle) ; + + + /// Return Type: int + ///statusHandle: b3SharedMemoryStatusHandle->b3SharedMemoryStatusHandle__* + ///bodyUniqueId: int* + ///numDegreeOfFreedomQ: int* + ///numDegreeOfFreedomU: int* + ///rootLocalInertialFrame: double** + ///actualStateQ: double** + ///actualStateQdot: double** + ///jointReactionForces: double** + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3GetStatusActualState")] +public static extern int b3GetStatusActualState(System.IntPtr statusHandle, ref int bodyUniqueId, ref int numDegreeOfFreedomQ, ref int numDegreeOfFreedomU, ref System.IntPtr rootLocalInertialFrame, ref System.IntPtr actualStateQ, ref System.IntPtr actualStateQdot, ref System.IntPtr jointReactionForces) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///bodyUniqueId: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3RequestCollisionInfoCommandInit")] +public static extern System.IntPtr b3RequestCollisionInfoCommandInit(System.IntPtr physClient, int bodyUniqueId) ; + + + /// Return Type: int + ///statusHandle: b3SharedMemoryStatusHandle->b3SharedMemoryStatusHandle__* + ///linkIndex: int + ///aabbMin: double* + ///aabbMax: double* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3GetStatusAABB")] +public static extern int b3GetStatusAABB(System.IntPtr statusHandle, int linkIndex, ref double aabbMin, ref double aabbMax) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3InitSyncBodyInfoCommand")] +public static extern System.IntPtr b3InitSyncBodyInfoCommand(System.IntPtr physClient) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///bodyUniqueId: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3InitRemoveBodyCommand")] +public static extern System.IntPtr b3InitRemoveBodyCommand(System.IntPtr physClient, int bodyUniqueId) ; + + + /// Return Type: int + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3GetNumBodies")] +public static extern int b3GetNumBodies(System.IntPtr physClient) ; + + + /// Return Type: int + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///serialIndex: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3GetBodyUniqueId")] +public static extern int b3GetBodyUniqueId(System.IntPtr physClient, int serialIndex) ; + + + /// Return Type: int + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///bodyUniqueId: int + ///info: b3BodyInfo* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3GetBodyInfo")] +public static extern int b3GetBodyInfo(System.IntPtr physClient, int bodyUniqueId, ref b3BodyInfo info) ; + + + /// Return Type: int + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///bodyIndex: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3GetNumJoints")] +public static extern int b3GetNumJoints(System.IntPtr physClient, int bodyIndex) ; + + + /// Return Type: int + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///bodyIndex: int + ///jointIndex: int + ///info: b3JointInfo* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3GetJointInfo")] +public static extern int b3GetJointInfo(System.IntPtr physClient, int bodyIndex, int jointIndex, ref b3JointInfo info) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///bodyUniqueId: int + ///linkIndex: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3GetDynamicsInfoCommandInit")] +public static extern System.IntPtr b3GetDynamicsInfoCommandInit(System.IntPtr physClient, int bodyUniqueId, int linkIndex) ; + + + /// Return Type: int + ///statusHandle: b3SharedMemoryStatusHandle->b3SharedMemoryStatusHandle__* + ///info: b3DynamicsInfo* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3GetDynamicsInfo")] +public static extern int b3GetDynamicsInfo(System.IntPtr statusHandle, ref b3DynamicsInfo info) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3InitChangeDynamicsInfo")] +public static extern System.IntPtr b3InitChangeDynamicsInfo(System.IntPtr physClient) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///bodyUniqueId: int + ///linkIndex: int + ///mass: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3ChangeDynamicsInfoSetMass")] +public static extern int b3ChangeDynamicsInfoSetMass(System.IntPtr commandHandle, int bodyUniqueId, int linkIndex, double mass) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///bodyUniqueId: int + ///linkIndex: int + ///lateralFriction: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3ChangeDynamicsInfoSetLateralFriction")] +public static extern int b3ChangeDynamicsInfoSetLateralFriction(System.IntPtr commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///bodyUniqueId: int + ///linkIndex: int + ///friction: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3ChangeDynamicsInfoSetSpinningFriction")] +public static extern int b3ChangeDynamicsInfoSetSpinningFriction(System.IntPtr commandHandle, int bodyUniqueId, int linkIndex, double friction) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///bodyUniqueId: int + ///linkIndex: int + ///friction: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3ChangeDynamicsInfoSetRollingFriction")] +public static extern int b3ChangeDynamicsInfoSetRollingFriction(System.IntPtr commandHandle, int bodyUniqueId, int linkIndex, double friction) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///bodyUniqueId: int + ///linkIndex: int + ///restitution: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3ChangeDynamicsInfoSetRestitution")] +public static extern int b3ChangeDynamicsInfoSetRestitution(System.IntPtr commandHandle, int bodyUniqueId, int linkIndex, double restitution) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///bodyUniqueId: int + ///linearDamping: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3ChangeDynamicsInfoSetLinearDamping")] +public static extern int b3ChangeDynamicsInfoSetLinearDamping(System.IntPtr commandHandle, int bodyUniqueId, double linearDamping) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///bodyUniqueId: int + ///angularDamping: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3ChangeDynamicsInfoSetAngularDamping")] +public static extern int b3ChangeDynamicsInfoSetAngularDamping(System.IntPtr commandHandle, int bodyUniqueId, double angularDamping) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///bodyUniqueId: int + ///linkIndex: int + ///contactStiffness: double + ///contactDamping: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3ChangeDynamicsInfoSetContactStiffnessAndDamping")] +public static extern int b3ChangeDynamicsInfoSetContactStiffnessAndDamping(System.IntPtr commandHandle, int bodyUniqueId, int linkIndex, double contactStiffness, double contactDamping) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///bodyUniqueId: int + ///linkIndex: int + ///frictionAnchor: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3ChangeDynamicsInfoSetFrictionAnchor")] +public static extern int b3ChangeDynamicsInfoSetFrictionAnchor(System.IntPtr commandHandle, int bodyUniqueId, int linkIndex, int frictionAnchor) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///parentBodyIndex: int + ///parentJointIndex: int + ///childBodyIndex: int + ///childJointIndex: int + ///info: b3JointInfo* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3InitCreateUserConstraintCommand")] +public static extern System.IntPtr b3InitCreateUserConstraintCommand(System.IntPtr physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, ref b3JointInfo info) ; + + + /// Return Type: int + ///statusHandle: b3SharedMemoryStatusHandle->b3SharedMemoryStatusHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3GetStatusUserConstraintUniqueId")] +public static extern int b3GetStatusUserConstraintUniqueId(System.IntPtr statusHandle) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///userConstraintUniqueId: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3InitChangeUserConstraintCommand")] +public static extern System.IntPtr b3InitChangeUserConstraintCommand(System.IntPtr physClient, int userConstraintUniqueId) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///jointChildPivot: double* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3InitChangeUserConstraintSetPivotInB")] +public static extern int b3InitChangeUserConstraintSetPivotInB(System.IntPtr commandHandle, ref double jointChildPivot) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///jointChildFrameOrn: double* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3InitChangeUserConstraintSetFrameInB")] +public static extern int b3InitChangeUserConstraintSetFrameInB(System.IntPtr commandHandle, ref double jointChildFrameOrn) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///maxAppliedForce: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3InitChangeUserConstraintSetMaxForce")] +public static extern int b3InitChangeUserConstraintSetMaxForce(System.IntPtr commandHandle, double maxAppliedForce) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///gearRatio: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3InitChangeUserConstraintSetGearRatio")] +public static extern int b3InitChangeUserConstraintSetGearRatio(System.IntPtr commandHandle, double gearRatio) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///gearAuxLink: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3InitChangeUserConstraintSetGearAuxLink")] +public static extern int b3InitChangeUserConstraintSetGearAuxLink(System.IntPtr commandHandle, int gearAuxLink) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///userConstraintUniqueId: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3InitRemoveUserConstraintCommand")] +public static extern System.IntPtr b3InitRemoveUserConstraintCommand(System.IntPtr physClient, int userConstraintUniqueId) ; + + + /// Return Type: int + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3GetNumUserConstraints")] +public static extern int b3GetNumUserConstraints(System.IntPtr physClient) ; + + + /// Return Type: int + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///constraintUniqueId: int + ///info: b3UserConstraint* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3GetUserConstraintInfo")] +public static extern int b3GetUserConstraintInfo(System.IntPtr physClient, int constraintUniqueId, ref b3UserConstraint info) ; + + + /// Return Type: int + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///serialIndex: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3GetUserConstraintId")] +public static extern int b3GetUserConstraintId(System.IntPtr physClient, int serialIndex) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///debugMode: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3InitRequestDebugLinesCommand")] +public static extern System.IntPtr b3InitRequestDebugLinesCommand(System.IntPtr physClient, int debugMode) ; + + + /// Return Type: void + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///lines: b3DebugLines* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3GetDebugLines")] +public static extern void b3GetDebugLines(System.IntPtr physClient, ref b3DebugLines lines) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3InitConfigureOpenGLVisualizer")] +public static extern System.IntPtr b3InitConfigureOpenGLVisualizer(System.IntPtr physClient) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///flag: int + ///enabled: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3ConfigureOpenGLVisualizerSetVisualizationFlags")] +public static extern void b3ConfigureOpenGLVisualizerSetVisualizationFlags(System.IntPtr commandHandle, int flag, int enabled) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///cameraDistance: float + ///cameraPitch: float + ///cameraYaw: float + ///cameraTargetPosition: float* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3ConfigureOpenGLVisualizerSetViewMatrix")] +public static extern void b3ConfigureOpenGLVisualizerSetViewMatrix(System.IntPtr commandHandle, float cameraDistance, float cameraPitch, float cameraYaw, ref float cameraTargetPosition) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3InitRequestOpenGLVisualizerCameraCommand")] +public static extern System.IntPtr b3InitRequestOpenGLVisualizerCameraCommand(System.IntPtr physClient) ; + + + /// Return Type: int + ///statusHandle: b3SharedMemoryStatusHandle->b3SharedMemoryStatusHandle__* + ///camera: b3OpenGLVisualizerCameraInfo* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3GetStatusOpenGLVisualizerCamera")] +public static extern int b3GetStatusOpenGLVisualizerCamera(System.IntPtr statusHandle, ref b3OpenGLVisualizerCameraInfo camera) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///fromXYZ: double* + ///toXYZ: double* + ///colorRGB: double* + ///lineWidth: double + ///lifeTime: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3InitUserDebugDrawAddLine3D")] +public static extern System.IntPtr b3InitUserDebugDrawAddLine3D(System.IntPtr physClient, ref double fromXYZ, ref double toXYZ, ref double colorRGB, double lineWidth, double lifeTime) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///txt: char* + ///positionXYZ: double* + ///colorRGB: double* + ///textSize: double + ///lifeTime: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3InitUserDebugDrawAddText3D")] +public static extern System.IntPtr b3InitUserDebugDrawAddText3D(System.IntPtr physClient, [System.Runtime.InteropServices.InAttribute()] [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)] string txt, ref double positionXYZ, ref double colorRGB, double textSize, double lifeTime) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///optionFlags: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3UserDebugTextSetOptionFlags")] +public static extern void b3UserDebugTextSetOptionFlags(System.IntPtr commandHandle, int optionFlags) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///orientation: double* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3UserDebugTextSetOrientation")] +public static extern void b3UserDebugTextSetOrientation(System.IntPtr commandHandle, ref double orientation) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///objectUniqueId: int + ///linkIndex: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3UserDebugItemSetParentObject")] +public static extern void b3UserDebugItemSetParentObject(System.IntPtr commandHandle, int objectUniqueId, int linkIndex) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///txt: char* + ///rangeMin: double + ///rangeMax: double + ///startValue: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3InitUserDebugAddParameter")] +public static extern System.IntPtr b3InitUserDebugAddParameter(System.IntPtr physClient, [System.Runtime.InteropServices.InAttribute()] [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)] string txt, double rangeMin, double rangeMax, double startValue) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///debugItemUniqueId: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3InitUserDebugReadParameter")] +public static extern System.IntPtr b3InitUserDebugReadParameter(System.IntPtr physClient, int debugItemUniqueId) ; + + + /// Return Type: int + ///statusHandle: b3SharedMemoryStatusHandle->b3SharedMemoryStatusHandle__* + ///paramValue: double* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3GetStatusDebugParameterValue")] +public static extern int b3GetStatusDebugParameterValue(System.IntPtr statusHandle, ref double paramValue) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///debugItemUniqueId: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3InitUserDebugDrawRemove")] +public static extern System.IntPtr b3InitUserDebugDrawRemove(System.IntPtr physClient, int debugItemUniqueId) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3InitUserDebugDrawRemoveAll")] +public static extern System.IntPtr b3InitUserDebugDrawRemoveAll(System.IntPtr physClient) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3InitDebugDrawingCommand")] +public static extern System.IntPtr b3InitDebugDrawingCommand(System.IntPtr physClient) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///objectUniqueId: int + ///linkIndex: int + ///objectColorRGB: double* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3SetDebugObjectColor")] +public static extern void b3SetDebugObjectColor(System.IntPtr commandHandle, int objectUniqueId, int linkIndex, ref double objectColorRGB) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///objectUniqueId: int + ///linkIndex: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3RemoveDebugObjectColor")] +public static extern void b3RemoveDebugObjectColor(System.IntPtr commandHandle, int objectUniqueId, int linkIndex) ; + + + /// Return Type: int + ///statusHandle: b3SharedMemoryStatusHandle->b3SharedMemoryStatusHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3GetDebugItemUniqueId")] +public static extern int b3GetDebugItemUniqueId(System.IntPtr statusHandle) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3InitRequestCameraImage")] +public static extern System.IntPtr b3InitRequestCameraImage(System.IntPtr physClient) ; + + + /// Return Type: void + ///command: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///viewMatrix: float* + ///projectionMatrix: float* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3RequestCameraImageSetCameraMatrices")] +public static extern void b3RequestCameraImageSetCameraMatrices(System.IntPtr command, ref float viewMatrix, ref float projectionMatrix) ; + + + /// Return Type: void + ///command: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///width: int + ///height: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3RequestCameraImageSetPixelResolution")] +public static extern void b3RequestCameraImageSetPixelResolution(System.IntPtr command, int width, int height) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///lightDirection: float* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3RequestCameraImageSetLightDirection")] +public static extern void b3RequestCameraImageSetLightDirection(System.IntPtr commandHandle, ref float lightDirection) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///lightColor: float* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3RequestCameraImageSetLightColor")] +public static extern void b3RequestCameraImageSetLightColor(System.IntPtr commandHandle, ref float lightColor) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///lightDistance: float + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3RequestCameraImageSetLightDistance")] +public static extern void b3RequestCameraImageSetLightDistance(System.IntPtr commandHandle, float lightDistance) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///lightAmbientCoeff: float + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3RequestCameraImageSetLightAmbientCoeff")] +public static extern void b3RequestCameraImageSetLightAmbientCoeff(System.IntPtr commandHandle, float lightAmbientCoeff) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///lightDiffuseCoeff: float + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3RequestCameraImageSetLightDiffuseCoeff")] +public static extern void b3RequestCameraImageSetLightDiffuseCoeff(System.IntPtr commandHandle, float lightDiffuseCoeff) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///lightSpecularCoeff: float + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3RequestCameraImageSetLightSpecularCoeff")] +public static extern void b3RequestCameraImageSetLightSpecularCoeff(System.IntPtr commandHandle, float lightSpecularCoeff) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///hasShadow: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3RequestCameraImageSetShadow")] +public static extern void b3RequestCameraImageSetShadow(System.IntPtr commandHandle, int hasShadow) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///renderer: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3RequestCameraImageSelectRenderer")] +public static extern void b3RequestCameraImageSelectRenderer(System.IntPtr commandHandle, int renderer) ; + + + /// Return Type: void + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///imageData: b3CameraImageData* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3GetCameraImageData")] +public static extern void b3GetCameraImageData(System.IntPtr physClient, ref b3CameraImageData imageData) ; + + + /// Return Type: void + ///cameraPosition: float* + ///cameraTargetPosition: float* + ///cameraUp: float* + ///viewMatrix: float* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3ComputeViewMatrixFromPositions")] +public static extern void b3ComputeViewMatrixFromPositions(ref float cameraPosition, ref float cameraTargetPosition, ref float cameraUp, ref float viewMatrix) ; + + + /// Return Type: void + ///cameraTargetPosition: float* + ///distance: float + ///yaw: float + ///pitch: float + ///roll: float + ///upAxis: int + ///viewMatrix: float* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3ComputeViewMatrixFromYawPitchRoll")] +public static extern void b3ComputeViewMatrixFromYawPitchRoll(ref float cameraTargetPosition, float distance, float yaw, float pitch, float roll, int upAxis, ref float viewMatrix) ; + + + /// Return Type: void + ///viewMatrix: float* + ///cameraPosition: float* + ///cameraTargetPosition: float* + ///cameraUp: float* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3ComputePositionFromViewMatrix")] +public static extern void b3ComputePositionFromViewMatrix(ref float viewMatrix, ref float cameraPosition, ref float cameraTargetPosition, ref float cameraUp) ; + + + /// Return Type: void + ///left: float + ///right: float + ///bottom: float + ///top: float + ///nearVal: float + ///farVal: float + ///projectionMatrix: float* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3ComputeProjectionMatrix")] +public static extern void b3ComputeProjectionMatrix(float left, float right, float bottom, float top, float nearVal, float farVal, ref float projectionMatrix) ; + + + /// Return Type: void + ///fov: float + ///aspect: float + ///nearVal: float + ///farVal: float + ///projectionMatrix: float* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3ComputeProjectionMatrixFOV")] +public static extern void b3ComputeProjectionMatrixFOV(float fov, float aspect, float nearVal, float farVal, ref float projectionMatrix) ; + + + /// Return Type: void + ///command: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///cameraPosition: float* + ///cameraTargetPosition: float* + ///cameraUp: float* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3RequestCameraImageSetViewMatrix")] +public static extern void b3RequestCameraImageSetViewMatrix(System.IntPtr command, ref float cameraPosition, ref float cameraTargetPosition, ref float cameraUp) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///cameraTargetPosition: float* + ///distance: float + ///yaw: float + ///pitch: float + ///roll: float + ///upAxis: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3RequestCameraImageSetViewMatrix2")] +public static extern void b3RequestCameraImageSetViewMatrix2(System.IntPtr commandHandle, ref float cameraTargetPosition, float distance, float yaw, float pitch, float roll, int upAxis) ; + + + /// Return Type: void + ///command: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///left: float + ///right: float + ///bottom: float + ///top: float + ///nearVal: float + ///farVal: float + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3RequestCameraImageSetProjectionMatrix")] +public static extern void b3RequestCameraImageSetProjectionMatrix(System.IntPtr command, float left, float right, float bottom, float top, float nearVal, float farVal) ; + + + /// Return Type: void + ///command: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///fov: float + ///aspect: float + ///nearVal: float + ///farVal: float + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3RequestCameraImageSetFOVProjectionMatrix")] +public static extern void b3RequestCameraImageSetFOVProjectionMatrix(System.IntPtr command, float fov, float aspect, float nearVal, float farVal) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3InitRequestContactPointInformation")] +public static extern System.IntPtr b3InitRequestContactPointInformation(System.IntPtr physClient) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///bodyUniqueIdA: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3SetContactFilterBodyA")] +public static extern void b3SetContactFilterBodyA(System.IntPtr commandHandle, int bodyUniqueIdA) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///bodyUniqueIdB: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3SetContactFilterBodyB")] +public static extern void b3SetContactFilterBodyB(System.IntPtr commandHandle, int bodyUniqueIdB) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///linkIndexA: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3SetContactFilterLinkA")] +public static extern void b3SetContactFilterLinkA(System.IntPtr commandHandle, int linkIndexA) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///linkIndexB: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3SetContactFilterLinkB")] +public static extern void b3SetContactFilterLinkB(System.IntPtr commandHandle, int linkIndexB) ; + + + /// Return Type: void + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///contactPointInfo: b3ContactInformation* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3GetContactPointInformation")] +public static extern void b3GetContactPointInformation(System.IntPtr physClient, ref b3ContactInformation contactPointInfo) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3InitClosestDistanceQuery")] +public static extern System.IntPtr b3InitClosestDistanceQuery(System.IntPtr physClient) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///bodyUniqueIdA: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3SetClosestDistanceFilterBodyA")] +public static extern void b3SetClosestDistanceFilterBodyA(System.IntPtr commandHandle, int bodyUniqueIdA) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///linkIndexA: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3SetClosestDistanceFilterLinkA")] +public static extern void b3SetClosestDistanceFilterLinkA(System.IntPtr commandHandle, int linkIndexA) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///bodyUniqueIdB: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3SetClosestDistanceFilterBodyB")] +public static extern void b3SetClosestDistanceFilterBodyB(System.IntPtr commandHandle, int bodyUniqueIdB) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///linkIndexB: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3SetClosestDistanceFilterLinkB")] +public static extern void b3SetClosestDistanceFilterLinkB(System.IntPtr commandHandle, int linkIndexB) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///distance: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3SetClosestDistanceThreshold")] +public static extern void b3SetClosestDistanceThreshold(System.IntPtr commandHandle, double distance) ; + + + /// Return Type: void + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///contactPointInfo: b3ContactInformation* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3GetClosestPointInformation")] +public static extern void b3GetClosestPointInformation(System.IntPtr physClient, ref b3ContactInformation contactPointInfo) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///aabbMin: double* + ///aabbMax: double* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3InitAABBOverlapQuery")] +public static extern System.IntPtr b3InitAABBOverlapQuery(System.IntPtr physClient, ref double aabbMin, ref double aabbMax) ; + + + /// Return Type: void + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///data: b3AABBOverlapData* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3GetAABBOverlapResults")] +public static extern void b3GetAABBOverlapResults(System.IntPtr physClient, ref b3AABBOverlapData data) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///bodyUniqueIdA: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3InitRequestVisualShapeInformation")] +public static extern System.IntPtr b3InitRequestVisualShapeInformation(System.IntPtr physClient, int bodyUniqueIdA) ; + + + /// Return Type: void + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///visualShapeInfo: b3VisualShapeInformation* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3GetVisualShapeInformation")] +public static extern void b3GetVisualShapeInformation(System.IntPtr physClient, ref b3VisualShapeInformation visualShapeInfo) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///filename: char* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3InitLoadTexture")] +public static extern System.IntPtr b3InitLoadTexture(System.IntPtr physClient, [System.Runtime.InteropServices.InAttribute()] [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)] string filename) ; + + + /// Return Type: int + ///statusHandle: b3SharedMemoryStatusHandle->b3SharedMemoryStatusHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3GetStatusTextureUniqueId")] +public static extern int b3GetStatusTextureUniqueId(System.IntPtr statusHandle) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///textureUniqueId: int + ///width: int + ///height: int + ///rgbPixels: char* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreateChangeTextureCommandInit")] +public static extern System.IntPtr b3CreateChangeTextureCommandInit(System.IntPtr physClient, int textureUniqueId, int width, int height, [System.Runtime.InteropServices.InAttribute()] [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)] string rgbPixels) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///bodyUniqueId: int + ///jointIndex: int + ///shapeIndex: int + ///textureUniqueId: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3InitUpdateVisualShape")] +public static extern System.IntPtr b3InitUpdateVisualShape(System.IntPtr physClient, int bodyUniqueId, int jointIndex, int shapeIndex, int textureUniqueId) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///rgbaColor: double* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3UpdateVisualShapeRGBAColor")] +public static extern void b3UpdateVisualShapeRGBAColor(System.IntPtr commandHandle, ref double rgbaColor) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///specularColor: double* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3UpdateVisualShapeSpecularColor")] +public static extern void b3UpdateVisualShapeSpecularColor(System.IntPtr commandHandle, ref double specularColor) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3InitPhysicsParamCommand")] +public static extern System.IntPtr b3InitPhysicsParamCommand(System.IntPtr physClient) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///gravx: double + ///gravy: double + ///gravz: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3PhysicsParamSetGravity")] +public static extern int b3PhysicsParamSetGravity(System.IntPtr commandHandle, double gravx, double gravy, double gravz) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///timeStep: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3PhysicsParamSetTimeStep")] +public static extern int b3PhysicsParamSetTimeStep(System.IntPtr commandHandle, double timeStep) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///defaultContactERP: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3PhysicsParamSetDefaultContactERP")] +public static extern int b3PhysicsParamSetDefaultContactERP(System.IntPtr commandHandle, double defaultContactERP) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///defaultNonContactERP: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3PhysicsParamSetDefaultNonContactERP")] +public static extern int b3PhysicsParamSetDefaultNonContactERP(System.IntPtr commandHandle, double defaultNonContactERP) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///frictionERP: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3PhysicsParamSetDefaultFrictionERP")] +public static extern int b3PhysicsParamSetDefaultFrictionERP(System.IntPtr commandHandle, double frictionERP) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///numSubSteps: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3PhysicsParamSetNumSubSteps")] +public static extern int b3PhysicsParamSetNumSubSteps(System.IntPtr commandHandle, int numSubSteps) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///enableRealTimeSimulation: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3PhysicsParamSetRealTimeSimulation")] +public static extern int b3PhysicsParamSetRealTimeSimulation(System.IntPtr commandHandle, int enableRealTimeSimulation) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///numSolverIterations: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3PhysicsParamSetNumSolverIterations")] +public static extern int b3PhysicsParamSetNumSolverIterations(System.IntPtr commandHandle, int numSolverIterations) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///filterMode: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3PhysicsParamSetCollisionFilterMode")] +public static extern int b3PhysicsParamSetCollisionFilterMode(System.IntPtr commandHandle, int filterMode) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///useSplitImpulse: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3PhysicsParamSetUseSplitImpulse")] +public static extern int b3PhysicsParamSetUseSplitImpulse(System.IntPtr commandHandle, int useSplitImpulse) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///splitImpulsePenetrationThreshold: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3PhysicsParamSetSplitImpulsePenetrationThreshold")] +public static extern int b3PhysicsParamSetSplitImpulsePenetrationThreshold(System.IntPtr commandHandle, double splitImpulsePenetrationThreshold) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///contactBreakingThreshold: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3PhysicsParamSetContactBreakingThreshold")] +public static extern int b3PhysicsParamSetContactBreakingThreshold(System.IntPtr commandHandle, double contactBreakingThreshold) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///maxNumCmdPer1ms: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3PhysicsParamSetMaxNumCommandsPer1ms")] +public static extern int b3PhysicsParamSetMaxNumCommandsPer1ms(System.IntPtr commandHandle, int maxNumCmdPer1ms) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///enableFileCaching: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3PhysicsParamSetEnableFileCaching")] +public static extern int b3PhysicsParamSetEnableFileCaching(System.IntPtr commandHandle, int enableFileCaching) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///restitutionVelocityThreshold: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3PhysicsParamSetRestitutionVelocityThreshold")] +public static extern int b3PhysicsParamSetRestitutionVelocityThreshold(System.IntPtr commandHandle, double restitutionVelocityThreshold) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///flags: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3PhysicsParamSetInternalSimFlags")] +public static extern int b3PhysicsParamSetInternalSimFlags(System.IntPtr commandHandle, int flags) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3InitStepSimulationCommand")] +public static extern System.IntPtr b3InitStepSimulationCommand(System.IntPtr physClient) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3InitResetSimulationCommand")] +public static extern System.IntPtr b3InitResetSimulationCommand(System.IntPtr physClient) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///urdfFileName: char* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3LoadUrdfCommandInit")] +public static extern System.IntPtr b3LoadUrdfCommandInit(System.IntPtr physClient, [System.Runtime.InteropServices.InAttribute()] [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)] string urdfFileName) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///startPosX: double + ///startPosY: double + ///startPosZ: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3LoadUrdfCommandSetStartPosition")] +public static extern int b3LoadUrdfCommandSetStartPosition(System.IntPtr commandHandle, double startPosX, double startPosY, double startPosZ) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///startOrnX: double + ///startOrnY: double + ///startOrnZ: double + ///startOrnW: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3LoadUrdfCommandSetStartOrientation")] +public static extern int b3LoadUrdfCommandSetStartOrientation(System.IntPtr commandHandle, double startOrnX, double startOrnY, double startOrnZ, double startOrnW) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///useMultiBody: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3LoadUrdfCommandSetUseMultiBody")] +public static extern int b3LoadUrdfCommandSetUseMultiBody(System.IntPtr commandHandle, int useMultiBody) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///useFixedBase: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3LoadUrdfCommandSetUseFixedBase")] +public static extern int b3LoadUrdfCommandSetUseFixedBase(System.IntPtr commandHandle, int useFixedBase) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///flags: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3LoadUrdfCommandSetFlags")] +public static extern int b3LoadUrdfCommandSetFlags(System.IntPtr commandHandle, int flags) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///globalScaling: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3LoadUrdfCommandSetGlobalScaling")] +public static extern int b3LoadUrdfCommandSetGlobalScaling(System.IntPtr commandHandle, double globalScaling) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///fileName: char* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3LoadBulletCommandInit")] +public static extern System.IntPtr b3LoadBulletCommandInit(System.IntPtr physClient, [System.Runtime.InteropServices.InAttribute()] [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)] string fileName) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///fileName: char* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3SaveBulletCommandInit")] +public static extern System.IntPtr b3SaveBulletCommandInit(System.IntPtr physClient, [System.Runtime.InteropServices.InAttribute()] [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)] string fileName) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///fileName: char* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3LoadMJCFCommandInit")] +public static extern System.IntPtr b3LoadMJCFCommandInit(System.IntPtr physClient, [System.Runtime.InteropServices.InAttribute()] [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)] string fileName) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///flags: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3LoadMJCFCommandSetFlags")] +public static extern void b3LoadMJCFCommandSetFlags(System.IntPtr commandHandle, int flags) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///bodyIndex: int + ///jointPositionsQ: double* + ///jointVelocitiesQdot: double* + ///jointAccelerations: double* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CalculateInverseDynamicsCommandInit")] +public static extern System.IntPtr b3CalculateInverseDynamicsCommandInit(System.IntPtr physClient, int bodyIndex, ref double jointPositionsQ, ref double jointVelocitiesQdot, ref double jointAccelerations) ; + + + /// Return Type: int + ///statusHandle: b3SharedMemoryStatusHandle->b3SharedMemoryStatusHandle__* + ///bodyUniqueId: int* + ///dofCount: int* + ///jointForces: double* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3GetStatusInverseDynamicsJointForces")] +public static extern int b3GetStatusInverseDynamicsJointForces(System.IntPtr statusHandle, ref int bodyUniqueId, ref int dofCount, ref double jointForces) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///bodyIndex: int + ///linkIndex: int + ///localPosition: double* + ///jointPositionsQ: double* + ///jointVelocitiesQdot: double* + ///jointAccelerations: double* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CalculateJacobianCommandInit")] +public static extern System.IntPtr b3CalculateJacobianCommandInit(System.IntPtr physClient, int bodyIndex, int linkIndex, ref double localPosition, ref double jointPositionsQ, ref double jointVelocitiesQdot, ref double jointAccelerations) ; + + + /// Return Type: int + ///statusHandle: b3SharedMemoryStatusHandle->b3SharedMemoryStatusHandle__* + ///linearJacobian: double* + ///angularJacobian: double* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3GetStatusJacobian")] +public static extern int b3GetStatusJacobian(System.IntPtr statusHandle, ref double linearJacobian, ref double angularJacobian) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///bodyIndex: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CalculateInverseKinematicsCommandInit")] +public static extern System.IntPtr b3CalculateInverseKinematicsCommandInit(System.IntPtr physClient, int bodyIndex) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///endEffectorLinkIndex: int + ///targetPosition: double* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CalculateInverseKinematicsAddTargetPurePosition")] +public static extern void b3CalculateInverseKinematicsAddTargetPurePosition(System.IntPtr commandHandle, int endEffectorLinkIndex, ref double targetPosition) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///endEffectorLinkIndex: int + ///targetPosition: double* + ///targetOrientation: double* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CalculateInverseKinematicsAddTargetPositionWithOrientation")] +public static extern void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(System.IntPtr commandHandle, int endEffectorLinkIndex, ref double targetPosition, ref double targetOrientation) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///numDof: int + ///endEffectorLinkIndex: int + ///targetPosition: double* + ///lowerLimit: double* + ///upperLimit: double* + ///jointRange: double* + ///restPose: double* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CalculateInverseKinematicsPosWithNullSpaceVel")] +public static extern void b3CalculateInverseKinematicsPosWithNullSpaceVel(System.IntPtr commandHandle, int numDof, int endEffectorLinkIndex, ref double targetPosition, ref double lowerLimit, ref double upperLimit, ref double jointRange, ref double restPose) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///numDof: int + ///endEffectorLinkIndex: int + ///targetPosition: double* + ///targetOrientation: double* + ///lowerLimit: double* + ///upperLimit: double* + ///jointRange: double* + ///restPose: double* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CalculateInverseKinematicsPosOrnWithNullSpaceVel")] +public static extern void b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(System.IntPtr commandHandle, int numDof, int endEffectorLinkIndex, ref double targetPosition, ref double targetOrientation, ref double lowerLimit, ref double upperLimit, ref double jointRange, ref double restPose) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///numDof: int + ///jointDampingCoeff: double* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CalculateInverseKinematicsSetJointDamping")] +public static extern void b3CalculateInverseKinematicsSetJointDamping(System.IntPtr commandHandle, int numDof, ref double jointDampingCoeff) ; + + + /// Return Type: int + ///statusHandle: b3SharedMemoryStatusHandle->b3SharedMemoryStatusHandle__* + ///bodyUniqueId: int* + ///dofCount: int* + ///jointPositions: double* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3GetStatusInverseKinematicsJointPositions")] +public static extern int b3GetStatusInverseKinematicsJointPositions(System.IntPtr statusHandle, ref int bodyUniqueId, ref int dofCount, ref double jointPositions) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///sdfFileName: char* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3LoadSdfCommandInit")] +public static extern System.IntPtr b3LoadSdfCommandInit(System.IntPtr physClient, [System.Runtime.InteropServices.InAttribute()] [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)] string sdfFileName) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///useMultiBody: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3LoadSdfCommandSetUseMultiBody")] +public static extern int b3LoadSdfCommandSetUseMultiBody(System.IntPtr commandHandle, int useMultiBody) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///globalScaling: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3LoadSdfCommandSetUseGlobalScaling")] +public static extern int b3LoadSdfCommandSetUseGlobalScaling(System.IntPtr commandHandle, double globalScaling) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///sdfFileName: char* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3SaveWorldCommandInit")] +public static extern System.IntPtr b3SaveWorldCommandInit(System.IntPtr physClient, [System.Runtime.InteropServices.InAttribute()] [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)] string sdfFileName) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///controlMode: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3JointControlCommandInit")] +public static extern System.IntPtr b3JointControlCommandInit(System.IntPtr physClient, int controlMode) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///bodyUniqueId: int + ///controlMode: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3JointControlCommandInit2")] +public static extern System.IntPtr b3JointControlCommandInit2(System.IntPtr physClient, int bodyUniqueId, int controlMode) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///qIndex: int + ///value: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3JointControlSetDesiredPosition")] +public static extern int b3JointControlSetDesiredPosition(System.IntPtr commandHandle, int qIndex, double value) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///dofIndex: int + ///value: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3JointControlSetKp")] +public static extern int b3JointControlSetKp(System.IntPtr commandHandle, int dofIndex, double value) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///dofIndex: int + ///value: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3JointControlSetKd")] +public static extern int b3JointControlSetKd(System.IntPtr commandHandle, int dofIndex, double value) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///dofIndex: int + ///value: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3JointControlSetDesiredVelocity")] +public static extern int b3JointControlSetDesiredVelocity(System.IntPtr commandHandle, int dofIndex, double value) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///dofIndex: int + ///value: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3JointControlSetMaximumForce")] +public static extern int b3JointControlSetMaximumForce(System.IntPtr commandHandle, int dofIndex, double value) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///dofIndex: int + ///value: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3JointControlSetDesiredForceTorque")] +public static extern int b3JointControlSetDesiredForceTorque(System.IntPtr commandHandle, int dofIndex, double value) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreateCollisionShapeCommandInit")] +public static extern System.IntPtr b3CreateCollisionShapeCommandInit(System.IntPtr physClient) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///radius: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreateCollisionShapeAddSphere")] +public static extern int b3CreateCollisionShapeAddSphere(System.IntPtr commandHandle, double radius) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///halfExtents: double* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreateCollisionShapeAddBox")] +public static extern int b3CreateCollisionShapeAddBox(System.IntPtr commandHandle, ref double halfExtents) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///radius: double + ///height: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreateCollisionShapeAddCapsule")] +public static extern int b3CreateCollisionShapeAddCapsule(System.IntPtr commandHandle, double radius, double height) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///radius: double + ///height: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreateCollisionShapeAddCylinder")] +public static extern int b3CreateCollisionShapeAddCylinder(System.IntPtr commandHandle, double radius, double height) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///planeNormal: double* + ///planeConstant: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreateCollisionShapeAddPlane")] +public static extern int b3CreateCollisionShapeAddPlane(System.IntPtr commandHandle, ref double planeNormal, double planeConstant) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///fileName: char* + ///meshScale: double* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreateCollisionShapeAddMesh")] +public static extern int b3CreateCollisionShapeAddMesh(System.IntPtr commandHandle, [System.Runtime.InteropServices.InAttribute()] [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)] string fileName, ref double meshScale) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///shapeIndex: int + ///flags: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreateCollisionSetFlag")] +public static extern void b3CreateCollisionSetFlag(System.IntPtr commandHandle, int shapeIndex, int flags) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///shapeIndex: int + ///childPosition: double* + ///childOrientation: double* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreateCollisionShapeSetChildTransform")] +public static extern void b3CreateCollisionShapeSetChildTransform(System.IntPtr commandHandle, int shapeIndex, ref double childPosition, ref double childOrientation) ; + + + /// Return Type: int + ///statusHandle: b3SharedMemoryStatusHandle->b3SharedMemoryStatusHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3GetStatusCollisionShapeUniqueId")] +public static extern int b3GetStatusCollisionShapeUniqueId(System.IntPtr statusHandle) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreateVisualShapeCommandInit")] +public static extern System.IntPtr b3CreateVisualShapeCommandInit(System.IntPtr physClient) ; + + + /// Return Type: int + ///statusHandle: b3SharedMemoryStatusHandle->b3SharedMemoryStatusHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3GetStatusVisualShapeUniqueId")] +public static extern int b3GetStatusVisualShapeUniqueId(System.IntPtr statusHandle) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreateMultiBodyCommandInit")] +public static extern System.IntPtr b3CreateMultiBodyCommandInit(System.IntPtr physClient) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///mass: double + ///collisionShapeUnique: int + ///visualShapeUniqueId: int + ///basePosition: double* + ///baseOrientation: double* + ///baseInertialFramePosition: double* + ///baseInertialFrameOrientation: double* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreateMultiBodyBase")] +public static extern int b3CreateMultiBodyBase(System.IntPtr commandHandle, double mass, int collisionShapeUnique, int visualShapeUniqueId, ref double basePosition, ref double baseOrientation, ref double baseInertialFramePosition, ref double baseInertialFrameOrientation) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///linkMass: double + ///linkCollisionShapeIndex: double + ///linkVisualShapeIndex: double + ///linkPosition: double* + ///linkOrientation: double* + ///linkInertialFramePosition: double* + ///linkInertialFrameOrientation: double* + ///linkParentIndex: int + ///linkJointType: int + ///linkJointAxis: double* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreateMultiBodyLink")] +public static extern int b3CreateMultiBodyLink(System.IntPtr commandHandle, double linkMass, double linkCollisionShapeIndex, double linkVisualShapeIndex, ref double linkPosition, ref double linkOrientation, ref double linkInertialFramePosition, ref double linkInertialFrameOrientation, int linkParentIndex, int linkJointType, ref double linkJointAxis) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreateMultiBodyUseMaximalCoordinates")] +public static extern void b3CreateMultiBodyUseMaximalCoordinates(System.IntPtr commandHandle) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreateBoxShapeCommandInit")] +public static extern System.IntPtr b3CreateBoxShapeCommandInit(System.IntPtr physClient) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///startPosX: double + ///startPosY: double + ///startPosZ: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreateBoxCommandSetStartPosition")] +public static extern int b3CreateBoxCommandSetStartPosition(System.IntPtr commandHandle, double startPosX, double startPosY, double startPosZ) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///startOrnX: double + ///startOrnY: double + ///startOrnZ: double + ///startOrnW: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreateBoxCommandSetStartOrientation")] +public static extern int b3CreateBoxCommandSetStartOrientation(System.IntPtr commandHandle, double startOrnX, double startOrnY, double startOrnZ, double startOrnW) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///halfExtentsX: double + ///halfExtentsY: double + ///halfExtentsZ: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreateBoxCommandSetHalfExtents")] +public static extern int b3CreateBoxCommandSetHalfExtents(System.IntPtr commandHandle, double halfExtentsX, double halfExtentsY, double halfExtentsZ) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///mass: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreateBoxCommandSetMass")] +public static extern int b3CreateBoxCommandSetMass(System.IntPtr commandHandle, double mass) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///collisionShapeType: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreateBoxCommandSetCollisionShapeType")] +public static extern int b3CreateBoxCommandSetCollisionShapeType(System.IntPtr commandHandle, int collisionShapeType) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///red: double + ///green: double + ///blue: double + ///alpha: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreateBoxCommandSetColorRGBA")] +public static extern int b3CreateBoxCommandSetColorRGBA(System.IntPtr commandHandle, double red, double green, double blue, double alpha) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///bodyIndex: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreatePoseCommandInit")] +public static extern System.IntPtr b3CreatePoseCommandInit(System.IntPtr physClient, int bodyIndex) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///startPosX: double + ///startPosY: double + ///startPosZ: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreatePoseCommandSetBasePosition")] +public static extern int b3CreatePoseCommandSetBasePosition(System.IntPtr commandHandle, double startPosX, double startPosY, double startPosZ) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///startOrnX: double + ///startOrnY: double + ///startOrnZ: double + ///startOrnW: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreatePoseCommandSetBaseOrientation")] +public static extern int b3CreatePoseCommandSetBaseOrientation(System.IntPtr commandHandle, double startOrnX, double startOrnY, double startOrnZ, double startOrnW) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///linVel: double* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreatePoseCommandSetBaseLinearVelocity")] +public static extern int b3CreatePoseCommandSetBaseLinearVelocity(System.IntPtr commandHandle, ref double linVel) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///angVel: double* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreatePoseCommandSetBaseAngularVelocity")] +public static extern int b3CreatePoseCommandSetBaseAngularVelocity(System.IntPtr commandHandle, ref double angVel) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///numJointPositions: int + ///jointPositions: double* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreatePoseCommandSetJointPositions")] +public static extern int b3CreatePoseCommandSetJointPositions(System.IntPtr commandHandle, int numJointPositions, ref double jointPositions) ; + + + /// Return Type: int + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///jointIndex: int + ///jointPosition: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreatePoseCommandSetJointPosition")] +public static extern int b3CreatePoseCommandSetJointPosition(System.IntPtr physClient, System.IntPtr commandHandle, int jointIndex, double jointPosition) ; + + + /// Return Type: int + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///numJointVelocities: int + ///jointVelocities: double* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreatePoseCommandSetJointVelocities")] +public static extern int b3CreatePoseCommandSetJointVelocities(System.IntPtr physClient, System.IntPtr commandHandle, int numJointVelocities, ref double jointVelocities) ; + + + /// Return Type: int + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///jointIndex: int + ///jointVelocity: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreatePoseCommandSetJointVelocity")] +public static extern int b3CreatePoseCommandSetJointVelocity(System.IntPtr physClient, System.IntPtr commandHandle, int jointIndex, double jointVelocity) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///bodyUniqueId: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreateSensorCommandInit")] +public static extern System.IntPtr b3CreateSensorCommandInit(System.IntPtr physClient, int bodyUniqueId) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///jointIndex: int + ///enable: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreateSensorEnable6DofJointForceTorqueSensor")] +public static extern int b3CreateSensorEnable6DofJointForceTorqueSensor(System.IntPtr commandHandle, int jointIndex, int enable) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///linkIndex: int + ///enable: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreateSensorEnableIMUForLink")] +public static extern int b3CreateSensorEnableIMUForLink(System.IntPtr commandHandle, int linkIndex, int enable) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///bodyUniqueId: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3RequestActualStateCommandInit")] +public static extern System.IntPtr b3RequestActualStateCommandInit(System.IntPtr physClient, int bodyUniqueId) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///computeLinkVelocity: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3RequestActualStateCommandComputeLinkVelocity")] +public static extern int b3RequestActualStateCommandComputeLinkVelocity(System.IntPtr commandHandle, int computeLinkVelocity) ; + + + /// Return Type: int + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///statusHandle: b3SharedMemoryStatusHandle->b3SharedMemoryStatusHandle__* + ///jointIndex: int + ///state: b3JointSensorState* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3GetJointState")] +public static extern int b3GetJointState(System.IntPtr physClient, System.IntPtr statusHandle, int jointIndex, ref b3JointSensorState state) ; + + + /// Return Type: int + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///statusHandle: b3SharedMemoryStatusHandle->b3SharedMemoryStatusHandle__* + ///linkIndex: int + ///state: b3LinkState* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3GetLinkState")] +public static extern int b3GetLinkState(System.IntPtr physClient, System.IntPtr statusHandle, int linkIndex, ref b3LinkState state) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///rayFromWorldX: double + ///rayFromWorldY: double + ///rayFromWorldZ: double + ///rayToWorldX: double + ///rayToWorldY: double + ///rayToWorldZ: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3PickBody")] +public static extern System.IntPtr b3PickBody(System.IntPtr physClient, double rayFromWorldX, double rayFromWorldY, double rayFromWorldZ, double rayToWorldX, double rayToWorldY, double rayToWorldZ) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///rayFromWorldX: double + ///rayFromWorldY: double + ///rayFromWorldZ: double + ///rayToWorldX: double + ///rayToWorldY: double + ///rayToWorldZ: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3MovePickedBody")] +public static extern System.IntPtr b3MovePickedBody(System.IntPtr physClient, double rayFromWorldX, double rayFromWorldY, double rayFromWorldZ, double rayToWorldX, double rayToWorldY, double rayToWorldZ) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3RemovePickingConstraint")] +public static extern System.IntPtr b3RemovePickingConstraint(System.IntPtr physClient) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///rayFromWorldX: double + ///rayFromWorldY: double + ///rayFromWorldZ: double + ///rayToWorldX: double + ///rayToWorldY: double + ///rayToWorldZ: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreateRaycastCommandInit")] +public static extern System.IntPtr b3CreateRaycastCommandInit(System.IntPtr physClient, double rayFromWorldX, double rayFromWorldY, double rayFromWorldZ, double rayToWorldX, double rayToWorldY, double rayToWorldZ) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3CreateRaycastBatchCommandInit")] +public static extern System.IntPtr b3CreateRaycastBatchCommandInit(System.IntPtr physClient) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///rayFromWorld: double* + ///rayToWorld: double* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3RaycastBatchAddRay")] +public static extern void b3RaycastBatchAddRay(System.IntPtr commandHandle, ref double rayFromWorld, ref double rayToWorld) ; + + + /// Return Type: void + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///raycastInfo: b3RaycastInformation* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3GetRaycastInformation")] +public static extern void b3GetRaycastInformation(System.IntPtr physClient, ref b3RaycastInformation raycastInfo) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3ApplyExternalForceCommandInit")] +public static extern System.IntPtr b3ApplyExternalForceCommandInit(System.IntPtr physClient) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///bodyUniqueId: int + ///linkId: int + ///force: double* + ///position: double* + ///flags: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3ApplyExternalForce")] +public static extern void b3ApplyExternalForce(System.IntPtr commandHandle, int bodyUniqueId, int linkId, ref double force, ref double position, int flags) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///bodyUniqueId: int + ///linkId: int + ///torque: double* + ///flags: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3ApplyExternalTorque")] +public static extern void b3ApplyExternalTorque(System.IntPtr commandHandle, int bodyUniqueId, int linkId, ref double torque, int flags) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3LoadBunnyCommandInit")] +public static extern System.IntPtr b3LoadBunnyCommandInit(System.IntPtr physClient) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///scale: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3LoadBunnySetScale")] +public static extern int b3LoadBunnySetScale(System.IntPtr commandHandle, double scale) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///mass: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3LoadBunnySetMass")] +public static extern int b3LoadBunnySetMass(System.IntPtr commandHandle, double mass) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///collisionMargin: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3LoadBunnySetCollisionMargin")] +public static extern int b3LoadBunnySetCollisionMargin(System.IntPtr commandHandle, double collisionMargin) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3RequestVREventsCommandInit")] +public static extern System.IntPtr b3RequestVREventsCommandInit(System.IntPtr physClient) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///deviceTypeFilter: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3VREventsSetDeviceTypeFilter")] +public static extern void b3VREventsSetDeviceTypeFilter(System.IntPtr commandHandle, int deviceTypeFilter) ; + + + /// Return Type: void + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///vrEventsData: b3VREventsData* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3GetVREventsData")] +public static extern void b3GetVREventsData(System.IntPtr physClient, ref b3VREventsData vrEventsData) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3SetVRCameraStateCommandInit")] +public static extern System.IntPtr b3SetVRCameraStateCommandInit(System.IntPtr physClient) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///rootPos: double* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3SetVRCameraRootPosition")] +public static extern int b3SetVRCameraRootPosition(System.IntPtr commandHandle, ref double rootPos) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///rootOrn: double* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3SetVRCameraRootOrientation")] +public static extern int b3SetVRCameraRootOrientation(System.IntPtr commandHandle, ref double rootOrn) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///objectUniqueId: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3SetVRCameraTrackingObject")] +public static extern int b3SetVRCameraTrackingObject(System.IntPtr commandHandle, int objectUniqueId) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///flag: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3SetVRCameraTrackingObjectFlag")] +public static extern int b3SetVRCameraTrackingObjectFlag(System.IntPtr commandHandle, int flag) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3RequestKeyboardEventsCommandInit")] +public static extern System.IntPtr b3RequestKeyboardEventsCommandInit(System.IntPtr physClient) ; + + + /// Return Type: void + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///keyboardEventsData: b3KeyboardEventsData* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3GetKeyboardEventsData")] +public static extern void b3GetKeyboardEventsData(System.IntPtr physClient, ref b3KeyboardEventsData keyboardEventsData) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3RequestMouseEventsCommandInit")] +public static extern System.IntPtr b3RequestMouseEventsCommandInit(System.IntPtr physClient) ; + + + /// Return Type: void + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///mouseEventsData: b3MouseEventsData* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3GetMouseEventsData")] +public static extern void b3GetMouseEventsData(System.IntPtr physClient, ref b3MouseEventsData mouseEventsData) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3StateLoggingCommandInit")] +public static extern System.IntPtr b3StateLoggingCommandInit(System.IntPtr physClient) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///loggingType: int + ///fileName: char* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3StateLoggingStart")] +public static extern int b3StateLoggingStart(System.IntPtr commandHandle, int loggingType, [System.Runtime.InteropServices.InAttribute()] [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)] string fileName) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///objectUniqueId: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3StateLoggingAddLoggingObjectUniqueId")] +public static extern int b3StateLoggingAddLoggingObjectUniqueId(System.IntPtr commandHandle, int objectUniqueId) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///maxLogDof: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3StateLoggingSetMaxLogDof")] +public static extern int b3StateLoggingSetMaxLogDof(System.IntPtr commandHandle, int maxLogDof) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///linkIndexA: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3StateLoggingSetLinkIndexA")] +public static extern int b3StateLoggingSetLinkIndexA(System.IntPtr commandHandle, int linkIndexA) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///linkIndexB: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3StateLoggingSetLinkIndexB")] +public static extern int b3StateLoggingSetLinkIndexB(System.IntPtr commandHandle, int linkIndexB) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///bodyAUniqueId: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3StateLoggingSetBodyAUniqueId")] +public static extern int b3StateLoggingSetBodyAUniqueId(System.IntPtr commandHandle, int bodyAUniqueId) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///bodyBUniqueId: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3StateLoggingSetBodyBUniqueId")] +public static extern int b3StateLoggingSetBodyBUniqueId(System.IntPtr commandHandle, int bodyBUniqueId) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///deviceTypeFilter: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3StateLoggingSetDeviceTypeFilter")] +public static extern int b3StateLoggingSetDeviceTypeFilter(System.IntPtr commandHandle, int deviceTypeFilter) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///logFlags: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3StateLoggingSetLogFlags")] +public static extern int b3StateLoggingSetLogFlags(System.IntPtr commandHandle, int logFlags) ; + + + /// Return Type: int + ///statusHandle: b3SharedMemoryStatusHandle->b3SharedMemoryStatusHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3GetStatusLoggingUniqueId")] +public static extern int b3GetStatusLoggingUniqueId(System.IntPtr statusHandle) ; + + + /// Return Type: int + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///loggingUniqueId: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3StateLoggingStop")] +public static extern int b3StateLoggingStop(System.IntPtr commandHandle, int loggingUniqueId) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///name: char* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3ProfileTimingCommandInit")] +public static extern System.IntPtr b3ProfileTimingCommandInit(System.IntPtr physClient, [System.Runtime.InteropServices.InAttribute()] [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)] string name) ; + + + /// Return Type: void + ///commandHandle: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///duration: int + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3SetProfileTimingDuractionInMicroSeconds")] +public static extern void b3SetProfileTimingDuractionInMicroSeconds(System.IntPtr commandHandle, int duration) ; + + + /// Return Type: void + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///timeOutInSeconds: double + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3SetTimeOut")] +public static extern void b3SetTimeOut(System.IntPtr physClient, double timeOutInSeconds) ; + + + /// Return Type: double + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3GetTimeOut")] +public static extern double b3GetTimeOut(System.IntPtr physClient) ; + + + /// Return Type: b3SharedMemoryCommandHandle->b3SharedMemoryCommandHandle__* + ///physClient: b3PhysicsClientHandle->b3PhysicsClientHandle__* + ///path: char* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3SetAdditionalSearchPath")] +public static extern System.IntPtr b3SetAdditionalSearchPath(System.IntPtr physClient, System.IntPtr path) ; + + + /// Return Type: void + ///posA: double* + ///ornA: double* + ///posB: double* + ///ornB: double* + ///outPos: double* + ///outOrn: double* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3MultiplyTransforms")] +public static extern void b3MultiplyTransforms(ref double posA, ref double ornA, ref double posB, ref double ornB, ref double outPos, ref double outOrn) ; + + + /// Return Type: void + ///pos: double* + ///orn: double* + ///outPos: double* + ///outOrn: double* + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010.dll", EntryPoint="b3InvertTransform")] +public static extern void b3InvertTransform(ref double pos, ref double orn, ref double outPos, ref double outOrn) ; + +} diff --git a/examples/pybullet/unity3d/examples/NewBehaviourScript.cs b/examples/pybullet/unity3d/examples/NewBehaviourScript.cs new file mode 100644 index 000000000..026e7a709 --- /dev/null +++ b/examples/pybullet/unity3d/examples/NewBehaviourScript.cs @@ -0,0 +1,86 @@ +using System.Collections; +using System.Collections.Generic; +using UnityEngine; +using UnityEngine.UI; +using System.Runtime.InteropServices; +using System; + +[System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential)] + + +public class NewBehaviourScript : MonoBehaviour { + + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010_x64_release.dll", EntryPoint = "b3ConnectSharedMemory")] + public static extern System.IntPtr b3ConnectSharedMemory(int key); + + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010_x64_release.dll", EntryPoint = "b3CreateInProcessPhysicsServerAndConnect")] + public static extern System.IntPtr b3CreateInProcessPhysicsServerAndConnect(int argc, ref System.IntPtr argv); + + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010_x64_release.dll", EntryPoint = "b3InitStepSimulationCommand")] + public static extern System.IntPtr b3InitStepSimulationCommand(IntPtr physClient); + + + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010_x64_release.dll", EntryPoint = "b3LoadUrdfCommandInit")] + public static extern System.IntPtr b3LoadUrdfCommandInit(IntPtr physClient, [System.Runtime.InteropServices.InAttribute()] [System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)] string urdfFileName); + + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010_x64_release.dll", EntryPoint = "b3InitResetSimulationCommand")] + public static extern System.IntPtr b3InitResetSimulationCommand(IntPtr physClient); + + [System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010_x64_release.dll", EntryPoint = "b3SubmitClientCommandAndWaitStatus")] + public static extern System.IntPtr b3SubmitClientCommandAndWaitStatus(IntPtr physClient, IntPtr commandHandle); + + + [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; + + // Use this for initialization + void Start () { + text = GetComponent(); + CallMe(IWasCalled); + sharedAPI = CreateSharedAPI(30); + + IntPtr pybullet = b3ConnectSharedMemory(12347); + IntPtr cmd = b3InitResetSimulationCommand(pybullet); + IntPtr status = b3SubmitClientCommandAndWaitStatus(pybullet, cmd); + cmd = b3LoadUrdfCommandInit(pybullet, "plane.urdf"); + status = b3SubmitClientCommandAndWaitStatus(pybullet, cmd); + //IntPtr clientPtr = b3CreateInProcessPhysicsServerAndConnect(0, ref ptr); + } + + // Update is called once per frame + void Update () { + IntPtr cmd = b3InitStepSimulationCommand(pybullet); + IntPtr status = b3SubmitClientCommandAndWaitStatus(pybullet, cmd); + + //System.IO.Directory.GetCurrentDirectory().ToString();// + //text.text = Add(4, 5).ToString(); + text.text = UnityEngine.Random.Range(0f, 1f).ToString();// GetMyIdPlusTen(sharedAPI).ToString(); + } + + void OnDestroy() + { + + DeleteSharedAPI(sharedAPI); + } +} diff --git a/examples/pybullet/unity3d/examples/NewBehaviourScript.cs.meta b/examples/pybullet/unity3d/examples/NewBehaviourScript.cs.meta new file mode 100644 index 000000000..6e595b031 --- /dev/null +++ b/examples/pybullet/unity3d/examples/NewBehaviourScript.cs.meta @@ -0,0 +1,12 @@ +fileFormatVersion: 2 +guid: 6197b3a0389e92c47b7d8698e5f61f06 +timeCreated: 1505961790 +licenseType: Free +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/examples/pybullet/unity3d/readme.txt b/examples/pybullet/unity3d/readme.txt new file mode 100644 index 000000000..43497b816 --- /dev/null +++ b/examples/pybullet/unity3d/readme.txt @@ -0,0 +1,33 @@ +Quick prototype to connect Unity 3D to pybullet + +Generate C# Native Methods using the Microsoft PInvoke Signature Toolkit: + +sigimp.exe /lang:cs e:\develop\bullet3\examples\SharedMemory\PhysicsClientC_API.h + +Add some #define B3_SHARED_API __declspec(dllexport) to the exported methods, +replace [3], [4], [16] by [] to get sigimp.exe working. + +This generates autogen/NativeMethods.cs + +Then put pybullet.dll in the right location, so Unity finds it. + +NewBehaviourScript.cs is a 1 evening prototype that works within Unity 3D: +Create a connection to pybullet, reset the world, load a urdf at startup. +Step the simulation each Update. + +Now the real work can start, converting Unity objects to pybullet, +pybullet robots to Unity, synchronizing the transforms each Update. + +void Start () { + IntPtr pybullet = b3ConnectSharedMemory(12347); + IntPtr cmd = b3InitResetSimulationCommand(pybullet); + IntPtr status = b3SubmitClientCommandAndWaitStatus(pybullet, cmd); + cmd = b3LoadUrdfCommandInit(pybullet, "plane.urdf"); + status = b3SubmitClientCommandAndWaitStatus(pybullet, cmd); +} + +void Update () +{ + IntPtr cmd = b3InitStepSimulationCommand(pybullet); + IntPtr status = b3SubmitClientCommandAndWaitStatus(pybullet, cmd); +} diff --git a/setup.py b/setup.py index ecd208318..29139f534 100644 --- a/setup.py +++ b/setup.py @@ -70,6 +70,7 @@ sources = ["examples/pybullet/pybullet.c"]\ +["examples/SharedMemory/PhysicsClientUDP_C_API.cpp"]\ +["examples/SharedMemory/PhysicsClientTCP.cpp"]\ +["examples/SharedMemory/PhysicsClientTCP_C_API.cpp"]\ ++["examples/SharedMemory/b3PluginManager.cpp"]\ +["examples/Utils/b3ResourcePath.cpp"]\ +["examples/Utils/RobotLoggingUtil.cpp"]\ +["examples/Utils/ChromeTraceUtil.cpp"]\ @@ -440,7 +441,7 @@ print("-----") setup( name = 'pybullet', - version='1.4.5', + version='1.4.9', description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.', url='https://github.com/bulletphysics/bullet3', diff --git a/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.cpp b/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.cpp index 16991bc04..0f04efe33 100644 --- a/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.cpp +++ b/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.cpp @@ -227,26 +227,59 @@ static void b3FetchLeaves(b3DynamicBvh* pdbvt, } } -// -static void b3Split( const b3NodeArray& leaves, - b3NodeArray& left, - b3NodeArray& right, +static bool b3LeftOfAxis( const b3DbvtNode* node, + const b3Vector3& org, + const b3Vector3& axis) +{ + return b3Dot(axis,node->volume.Center()-org) <= 0; +} + +// Partitions leaves such that leaves[0, n) are on the +// left of axis, and leaves[n, count) are on the right +// of axis. returns N. +static int b3Split( b3DbvtNode** leaves, + int count, const b3Vector3& org, const b3Vector3& axis) { - left.resize(0); - right.resize(0); - for(int i=0,ni=leaves.size();ivolume.Center()-org)<0) - left.push_back(leaves[i]); - else - right.push_back(leaves[i]); + while(begin!=end && b3LeftOfAxis(leaves[begin],org,axis)) + { + ++begin; + } + + if(begin==end) + { + break; + } + + while(begin!=end && !b3LeftOfAxis(leaves[end-1],org,axis)) + { + --end; + } + + if(begin==end) + { + break; + } + + // swap out of place nodes + --end; + b3DbvtNode* temp=leaves[begin]; + leaves[begin]=leaves[end]; + leaves[end]=temp; + ++begin; } + + return begin; } // -static b3DbvtVolume b3Bounds( const b3NodeArray& leaves) +static b3DbvtVolume b3Bounds( b3DbvtNode** leaves, + int count) { #if B3_DBVT_MERGE_IMPL==B3_DBVT_IMPL_SSE B3_ATTRIBUTE_ALIGNED16(char locals[sizeof(b3DbvtVolume)]); @@ -255,7 +288,7 @@ static b3DbvtVolume b3Bounds( const b3NodeArray& leaves) #else b3DbvtVolume volume=leaves[0]->volume; #endif - for(int i=1,ni=leaves.size();ivolume,volume); } @@ -264,15 +297,16 @@ static b3DbvtVolume b3Bounds( const b3NodeArray& leaves) // static void b3BottomUp( b3DynamicBvh* pdbvt, - b3NodeArray& leaves) + b3DbvtNode** leaves, + int count) { - while(leaves.size()>1) + while(count>1) { b3Scalar minsize=B3_INFINITY; int minidx[2]={-1,-1}; - for(int i=0;ivolume,leaves[j]->volume)); if(szparent = p; n[1]->parent = p; leaves[minidx[0]] = p; - leaves.swap(minidx[1],leaves.size()-1); - leaves.pop_back(); + leaves[minidx[1]] = leaves[count-1]; + --count; } } // static b3DbvtNode* b3TopDown(b3DynamicBvh* pdbvt, - b3NodeArray& leaves, + b3DbvtNode** leaves, + int count, int bu_treshold) { static const b3Vector3 axis[]={b3MakeVector3(1,0,0), b3MakeVector3(0,1,0), b3MakeVector3(0,0,1)}; - if(leaves.size()>1) + b3Assert(bu_treshold>1); + if(count>1) { - if(leaves.size()>bu_treshold) + if(count>bu_treshold) { - const b3DbvtVolume vol=b3Bounds(leaves); + const b3DbvtVolume vol=b3Bounds(leaves,count); const b3Vector3 org=vol.Center(); - b3NodeArray sets[2]; + int partition; int bestaxis=-1; - int bestmidp=leaves.size(); + int bestmidp=count; int splitcount[3][2]={{0,0},{0,0},{0,0}}; int i; - for( i=0;ivolume.Center()-org; for(int j=0;j<3;++j) @@ -336,29 +372,23 @@ static b3DbvtNode* b3TopDown(b3DynamicBvh* pdbvt, } if(bestaxis>=0) { - sets[0].reserve(splitcount[bestaxis][0]); - sets[1].reserve(splitcount[bestaxis][1]); - b3Split(leaves,sets[0],sets[1],org,axis[bestaxis]); + partition=b3Split(leaves,count,org,axis[bestaxis]); + b3Assert(partition!=0 && partition!=count); } else { - sets[0].reserve(leaves.size()/2+1); - sets[1].reserve(leaves.size()/2); - for(int i=0,ni=leaves.size();ichilds[0]=b3TopDown(pdbvt,sets[0],bu_treshold); - node->childs[1]=b3TopDown(pdbvt,sets[1],bu_treshold); + node->childs[0]=b3TopDown(pdbvt,&leaves[0],partition,bu_treshold); + node->childs[1]=b3TopDown(pdbvt,&leaves[partition],count-partition,bu_treshold); node->childs[0]->parent=node; node->childs[1]->parent=node; return(node); } else { - b3BottomUp(pdbvt,leaves); + b3BottomUp(pdbvt,leaves,count); return(leaves[0]); } } @@ -442,7 +472,7 @@ void b3DynamicBvh::optimizeBottomUp() b3NodeArray leaves; leaves.reserve(m_leaves); b3FetchLeaves(this,m_root,leaves); - b3BottomUp(this,leaves); + b3BottomUp(this,&leaves[0],leaves.size()); m_root=leaves[0]; } } @@ -455,7 +485,7 @@ void b3DynamicBvh::optimizeTopDown(int bu_treshold) b3NodeArray leaves; leaves.reserve(m_leaves); b3FetchLeaves(this,m_root,leaves); - m_root=b3TopDown(this,leaves,bu_treshold); + m_root=b3TopDown(this,&leaves[0],leaves.size(),bu_treshold); } } diff --git a/src/BulletCollision/BroadphaseCollision/btDbvt.cpp b/src/BulletCollision/BroadphaseCollision/btDbvt.cpp index 2ca20cdd8..d791d0741 100644 --- a/src/BulletCollision/BroadphaseCollision/btDbvt.cpp +++ b/src/BulletCollision/BroadphaseCollision/btDbvt.cpp @@ -229,25 +229,60 @@ static void fetchleaves(btDbvt* pdbvt, } // -static void split( const tNodeArray& leaves, - tNodeArray& left, - tNodeArray& right, +static bool leftOfAxis( const btDbvtNode* node, + const btVector3& org, + const btVector3& axis) +{ + return btDot(axis, node->volume.Center() - org) <= 0; +} + + +// Partitions leaves such that leaves[0, n) are on the +// left of axis, and leaves[n, count) are on the right +// of axis. returns N. +static int split( btDbvtNode** leaves, + int count, const btVector3& org, const btVector3& axis) { - left.resize(0); - right.resize(0); - for(int i=0,ni=leaves.size();ivolume.Center()-org)<0) - left.push_back(leaves[i]); - else - right.push_back(leaves[i]); + while(begin!=end && leftOfAxis(leaves[begin],org,axis)) + { + ++begin; + } + + if(begin==end) + { + break; + } + + while(begin!=end && !leftOfAxis(leaves[end-1],org,axis)) + { + --end; + } + + if(begin==end) + { + break; + } + + // swap out of place nodes + --end; + btDbvtNode* temp=leaves[begin]; + leaves[begin]=leaves[end]; + leaves[end]=temp; + ++begin; } + + return begin; } // -static btDbvtVolume bounds( const tNodeArray& leaves) +static btDbvtVolume bounds( btDbvtNode** leaves, + int count) { #if DBVT_MERGE_IMPL==DBVT_IMPL_SSE ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtVolume)]); @@ -257,7 +292,7 @@ static btDbvtVolume bounds( const tNodeArray& leaves) #else btDbvtVolume volume=leaves[0]->volume; #endif - for(int i=1,ni=leaves.size();ivolume,volume); } @@ -266,15 +301,16 @@ static btDbvtVolume bounds( const tNodeArray& leaves) // static void bottomup( btDbvt* pdbvt, - tNodeArray& leaves) + btDbvtNode** leaves, + int count) { - while(leaves.size()>1) + while(count>1) { btScalar minsize=SIMD_INFINITY; int minidx[2]={-1,-1}; - for(int i=0;ivolume,leaves[j]->volume)); if(szparent = p; n[1]->parent = p; leaves[minidx[0]] = p; - leaves.swap(minidx[1],leaves.size()-1); - leaves.pop_back(); + leaves[minidx[1]] = leaves[count-1]; + --count; } } // static btDbvtNode* topdown(btDbvt* pdbvt, - tNodeArray& leaves, + btDbvtNode** leaves, + int count, int bu_treshold) { static const btVector3 axis[]={btVector3(1,0,0), btVector3(0,1,0), btVector3(0,0,1)}; - if(leaves.size()>1) + btAssert(bu_treshold>2); + if(count>1) { - if(leaves.size()>bu_treshold) + if(count>bu_treshold) { - const btDbvtVolume vol=bounds(leaves); + const btDbvtVolume vol=bounds(leaves,count); const btVector3 org=vol.Center(); - tNodeArray sets[2]; + int partition; int bestaxis=-1; - int bestmidp=leaves.size(); + int bestmidp=count; int splitcount[3][2]={{0,0},{0,0},{0,0}}; int i; - for( i=0;ivolume.Center()-org; for(int j=0;j<3;++j) @@ -338,29 +376,23 @@ static btDbvtNode* topdown(btDbvt* pdbvt, } if(bestaxis>=0) { - sets[0].reserve(splitcount[bestaxis][0]); - sets[1].reserve(splitcount[bestaxis][1]); - split(leaves,sets[0],sets[1],org,axis[bestaxis]); + partition=split(leaves,count,org,axis[bestaxis]); + btAssert(partition!=0 && partition!=count); } else { - sets[0].reserve(leaves.size()/2+1); - sets[1].reserve(leaves.size()/2); - for(int i=0,ni=leaves.size();ichilds[0]=topdown(pdbvt,sets[0],bu_treshold); - node->childs[1]=topdown(pdbvt,sets[1],bu_treshold); + node->childs[0]=topdown(pdbvt,&leaves[0],partition,bu_treshold); + node->childs[1]=topdown(pdbvt,&leaves[partition],count-partition,bu_treshold); node->childs[0]->parent=node; node->childs[1]->parent=node; return(node); } else { - bottomup(pdbvt,leaves); + bottomup(pdbvt,leaves,count); return(leaves[0]); } } @@ -444,7 +476,7 @@ void btDbvt::optimizeBottomUp() tNodeArray leaves; leaves.reserve(m_leaves); fetchleaves(this,m_root,leaves); - bottomup(this,leaves); + bottomup(this,&leaves[0],leaves.size()); m_root=leaves[0]; } } @@ -457,7 +489,7 @@ void btDbvt::optimizeTopDown(int bu_treshold) tNodeArray leaves; leaves.reserve(m_leaves); fetchleaves(this,m_root,leaves); - m_root=topdown(this,leaves,bu_treshold); + m_root=topdown(this,&leaves[0],leaves.size(),bu_treshold); } } diff --git a/src/BulletDynamics/Dynamics/btDynamicsWorld.h b/src/BulletDynamics/Dynamics/btDynamicsWorld.h index a9ed1978f..42d8fc0de 100644 --- a/src/BulletDynamics/Dynamics/btDynamicsWorld.h +++ b/src/BulletDynamics/Dynamics/btDynamicsWorld.h @@ -135,6 +135,11 @@ public: return m_solverInfo; } + const btContactSolverInfo& getSolverInfo() const + { + return m_solverInfo; + } + ///obsolete, use addAction instead. virtual void addVehicle(btActionInterface* vehicle) {(void)vehicle;} diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h index 8c28bbf4c..83521b950 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h @@ -185,7 +185,9 @@ public: virtual void setGearRatio(btScalar ratio) {} virtual void setGearAuxLink(int gearAuxLink) {} - + virtual void setRelativePositionTarget(btScalar relPosTarget){} + virtual void setErp(btScalar erp){} + }; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h index e5113ae6c..c0c132bbb 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h @@ -72,6 +72,11 @@ public: return m_multiBodies[mbIndex]; } + const btMultiBody* getMultiBody(int mbIndex) const + { + return m_multiBodies[mbIndex]; + } + virtual void addMultiBodyConstraint( btMultiBodyConstraint* constraint); virtual int getNumMultiBodyConstraints() const diff --git a/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp index 3fdd51815..5fdb7007d 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp @@ -23,7 +23,9 @@ subject to the following restrictions: btMultiBodyGearConstraint::btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB) :btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,1,false), m_gearRatio(1), - m_gearAuxLink(-1) + m_gearAuxLink(-1), + m_erp(0), + m_relativePositionTarget(0) { } @@ -107,9 +109,9 @@ void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray& jacobianA(0)[offsetA] = 1; jacobianB(0)[offsetB] = m_gearRatio; - const btScalar posError = 0; + btScalar posError = 0; const btVector3 dummy(0, 0, 0); - btScalar erp = infoGlobal.m_erp; + btScalar kp = 1; btScalar kd = 1; int numRows = getNumRows(); @@ -129,10 +131,15 @@ void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray& auxVel = m_bodyA->getJointVelMultiDof(m_gearAuxLink)[dof]; } currentVelocity += auxVel; - - //btScalar positionStabiliationTerm = erp*(m_desiredPosition-currentPosition)/infoGlobal.m_timeStep; - //btScalar velocityError = (m_desiredVelocity - currentVelocity); - + if (m_erp!=0) + { + btScalar currentPositionA = m_bodyA->getJointPosMultiDof(m_linkA)[dof]; + btScalar currentPositionB = m_gearRatio*m_bodyA->getJointPosMultiDof(m_linkB)[dof]; + btScalar diff = currentPositionB+currentPositionA; + btScalar desiredPositionDiff = this->m_relativePositionTarget; + posError = -m_erp*(desiredPositionDiff - diff); + } + btScalar desiredRelativeVelocity = auxVel; fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,false,1,false,desiredRelativeVelocity); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h index 711a73e46..0115de624 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h @@ -28,10 +28,12 @@ protected: btRigidBody* m_rigidBodyB; btVector3 m_pivotInA; btVector3 m_pivotInB; - btMatrix3x3 m_frameInA; - btMatrix3x3 m_frameInB; + btMatrix3x3 m_frameInA; + btMatrix3x3 m_frameInB; btScalar m_gearRatio; int m_gearAuxLink; + btScalar m_erp; + btScalar m_relativePositionTarget; public: @@ -102,7 +104,14 @@ public: { m_gearAuxLink = gearAuxLink; } - + virtual void setRelativePositionTarget(btScalar relPosTarget) + { + m_relativePositionTarget = relPosTarget; + } + virtual void setErp(btScalar erp) + { + m_erp = erp; + } }; #endif //BT_MULTIBODY_GEAR_CONSTRAINT_H diff --git a/src/BulletInverseDynamics/IDMath.cpp b/src/BulletInverseDynamics/IDMath.cpp index 021217a17..99fe20e49 100644 --- a/src/BulletInverseDynamics/IDMath.cpp +++ b/src/BulletInverseDynamics/IDMath.cpp @@ -33,6 +33,18 @@ void setZero(mat33 &m) { m(2, 2) = 0; } +void skew(vec3& v, mat33* result) { + (*result)(0, 0) = 0.0; + (*result)(0, 1) = -v(2); + (*result)(0, 2) = v(1); + (*result)(1, 0) = v(2); + (*result)(1, 1) = 0.0; + (*result)(1, 2) = -v(0); + (*result)(2, 0) = -v(1); + (*result)(2, 1) = v(0); + (*result)(2, 2) = 0.0; +} + idScalar maxAbs(const vecx &v) { idScalar result = 0.0; for (int i = 0; i < v.size(); i++) { @@ -69,7 +81,7 @@ idScalar maxAbsMat3x(const mat3x &m) { void mul(const mat33 &a, const mat3x &b, mat3x *result) { if (b.cols() != result->cols()) { - error_message("size missmatch. a.cols()= %d, b.cols()= %d\n", + error_message("size missmatch. b.cols()= %d, result->cols()= %d\n", static_cast(b.cols()), static_cast(result->cols())); abort(); } diff --git a/src/BulletInverseDynamics/IDMath.hpp b/src/BulletInverseDynamics/IDMath.hpp index 63699712a..b355474d4 100644 --- a/src/BulletInverseDynamics/IDMath.hpp +++ b/src/BulletInverseDynamics/IDMath.hpp @@ -12,7 +12,8 @@ void setZero(vec3& v); void setZero(vecx& v); /// set all elements to zero void setZero(mat33& m); - +/// create a skew symmetric matrix from a vector (useful for cross product abstraction, e.g. v x a = V * a) +void skew(vec3& v, mat33* result); /// return maximum absolute value idScalar maxAbs(const vecx& v); #ifndef ID_LINEAR_MATH_USE_EIGEN diff --git a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp index 847e5c6c7..b35c55df6 100644 --- a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp +++ b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp @@ -1013,7 +1013,7 @@ int MultiBodyTree::MultiBodyImpl::getBodyDotJacobianRotU(const int body_index, int MultiBodyTree::MultiBodyImpl::getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const{ CHECK_IF_BODY_INDEX_IS_VALID(body_index); const RigidBody &body = m_body_list[body_index]; - mul(body.m_body_T_world.transpose(), body.m_body_Jac_T,world_jac_trans); + mul(body.m_body_T_world.transpose(), body.m_body_Jac_T, world_jac_trans); return 0; } diff --git a/test/SharedMemory/CMakeLists.txt b/test/SharedMemory/CMakeLists.txt index 542a52bb3..0feab4306 100644 --- a/test/SharedMemory/CMakeLists.txt +++ b/test/SharedMemory/CMakeLists.txt @@ -21,6 +21,9 @@ ENDIF() IF (NOT WIN32) LINK_LIBRARIES( pthread ) + IF (NOT APPLE) + LINK_LIBRARIES(${DL}) + ENDIF() ENDIF() ADD_EXECUTABLE(Test_PhysicsClientServer @@ -39,6 +42,7 @@ ENDIF() ../../examples/SharedMemory/PhysicsDirectC_API.h ../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp ../../examples/SharedMemory/PhysicsServerCommandProcessor.h + ../../examples/SharedMemory/b3PluginManager.cpp ../../examples/SharedMemory/PhysicsClientSharedMemory.cpp ../../examples/SharedMemory/PhysicsClientSharedMemory.h ../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp diff --git a/test/SharedMemory/premake4.lua b/test/SharedMemory/premake4.lua index 282cc31fc..85b74ccfb 100644 --- a/test/SharedMemory/premake4.lua +++ b/test/SharedMemory/premake4.lua @@ -177,6 +177,8 @@ project ("Test_PhysicsServerLoopBack") "../../examples/SharedMemory/PhysicsServerSharedMemory.h", "../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp", "../../examples/SharedMemory/PhysicsServerCommandProcessor.h", + "../../examples/SharedMemory/b3PluginManager.cpp", + "../../examples/SharedMemory/PhysicsDirect.cpp", "../../examples/SharedMemory/PhysicsLoopBack.cpp", "../../examples/SharedMemory/PhysicsLoopBack.h", "../../examples/SharedMemory/PhysicsLoopBackC_API.cpp", @@ -261,6 +263,7 @@ project ("Test_PhysicsServerLoopBack") "../../examples/SharedMemory/PhysicsDirectC_API.h", "../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp", "../../examples/SharedMemory/PhysicsServerCommandProcessor.h", + "../../examples/SharedMemory/b3PluginManager.cpp", "../../examples/SharedMemory/PhysicsClientSharedMemory.cpp", "../../examples/SharedMemory/PhysicsClientSharedMemory.h", "../../examples/SharedMemory/PhysicsClientC_API.cpp", @@ -370,6 +373,7 @@ project ("Test_PhysicsServerInProcessExampleBrowser") "../../examples/SharedMemory/PhysicsDirectC_API.h", "../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp", "../../examples/SharedMemory/PhysicsServerCommandProcessor.h", + "../../examples/SharedMemory/b3PluginManager.cpp", "../../examples/SharedMemory/PhysicsClientSharedMemory.cpp", "../../examples/SharedMemory/PhysicsClientSharedMemory.h", "../../examples/SharedMemory/PhysicsClientC_API.cpp",