diff --git a/docs/pybullet_quickstartguide.pdf b/docs/pybullet_quickstartguide.pdf index 9a96ab0e4..8412d6715 100644 Binary files a/docs/pybullet_quickstartguide.pdf and b/docs/pybullet_quickstartguide.pdf differ diff --git a/examples/CommonInterfaces/CommonExampleInterface.h b/examples/CommonInterfaces/CommonExampleInterface.h index 220f9a583..0bccb2dd5 100644 --- a/examples/CommonInterfaces/CommonExampleInterface.h +++ b/examples/CommonInterfaces/CommonExampleInterface.h @@ -55,7 +55,7 @@ public: virtual bool mouseButtonCallback(int button, int state, float x, float y)=0; virtual bool keyboardCallback(int key, int state)=0; - virtual void vrControllerMoveCallback(int controllerId, float pos[4], float orientation[4], float analogAxis) {} + virtual void vrControllerMoveCallback(int controllerId, float pos[4], float orientation[4], float analogAxis, float auxAnalogAxes[10]) {} virtual void vrControllerButtonCallback(int controllerId, int button, int state, float pos[4], float orientation[4]){} virtual void vrHMDMoveCallback(int controllerId, float pos[4], float orientation[4]){} virtual void vrGenericTrackerMoveCallback(int controllerId, float pos[4], float orientation[4]){} diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index 6c7ac6be2..d0d14b200 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -377,6 +377,13 @@ void OpenGLExampleBrowser::registerFileImporter(const char* extension, CommonExa void OpenGLExampleBrowserVisualizerFlagCallback(int flag, bool enable) { + if (flag == COV_ENABLE_Y_AXIS_UP) + { + //either Y = up or Z + int upAxis = enable? 1:2; + s_app->setUpAxis(upAxis); + } + if (flag == COV_ENABLE_RENDERING) { gEnableRenderLoop = (enable!=0); diff --git a/examples/ExampleBrowser/main.cpp b/examples/ExampleBrowser/main.cpp index 07c168309..3847181b5 100644 --- a/examples/ExampleBrowser/main.cpp +++ b/examples/ExampleBrowser/main.cpp @@ -30,19 +30,16 @@ static OpenGLExampleBrowser* sExampleBrowser=0; static void cleanup(int signo) { - if (interrupted) { // this is the second time, we're hanging somewhere - // if (example) { - // example->abort(); - // } - + if (!interrupted) { // this is the second time, we're hanging somewhere b3Printf("Aborting and deleting SharedMemoryCommon object"); - sleep(1); delete sExampleBrowser; - sExampleBrowser = 0; + sleep(1); + sExampleBrowser = 0; errx(EXIT_FAILURE, "aborted example on signal %d", signo); } else { - b3Printf("no action"); + b3Printf("no action"); + exit(EXIT_FAILURE); } interrupted = true; warnx("caught signal %d", signo); diff --git a/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp b/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp index a7cf426cd..ae7c13a1d 100644 --- a/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp +++ b/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp @@ -17,6 +17,10 @@ struct CachedObjResult static b3HashMap gCachedObjResults; static int gEnableFileCaching = 1; +int b3IsFileCachingEnabled() +{ + return gEnableFileCaching; +} void b3EnableFileCaching(int enable) { gEnableFileCaching = enable; diff --git a/examples/Importers/ImportObjDemo/LoadMeshFromObj.h b/examples/Importers/ImportObjDemo/LoadMeshFromObj.h index 4d23eea8d..9414987f4 100644 --- a/examples/Importers/ImportObjDemo/LoadMeshFromObj.h +++ b/examples/Importers/ImportObjDemo/LoadMeshFromObj.h @@ -6,6 +6,7 @@ struct GLInstanceGraphicsShape; #include"../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h" +int b3IsFileCachingEnabled(); void b3EnableFileCaching(int enable); std::string LoadFromCachedOrFromObj( diff --git a/examples/SharedMemory/PhysicsClient.h b/examples/SharedMemory/PhysicsClient.h index 28b823008..e81f25bfc 100644 --- a/examples/SharedMemory/PhysicsClient.h +++ b/examples/SharedMemory/PhysicsClient.h @@ -66,6 +66,8 @@ public: virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits) = 0; + virtual void getCachedMassMatrix(int dofCountCheck, double* massMatrix) = 0; + virtual void setTimeOut(double timeOutInSeconds) = 0; virtual double getTimeOut() const = 0; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 485d8c87d..8a74f9769 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -325,6 +325,32 @@ B3_SHARED_API int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHand return -1; } +B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestPhysicsParamCommand(b3PhysicsClientHandle physClient) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type = CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS; + command->m_updateFlags = 0; + return (b3SharedMemoryCommandHandle) command; +} + +B3_SHARED_API int b3GetStatusPhysicsSimulationParameters(b3SharedMemoryStatusHandle statusHandle,struct b3PhysicsSimulationParameters* params) +{ + const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; + b3Assert(status); + b3Assert(status->m_type == CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED); + if (status && status->m_type == CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED) + { + *params = status->m_simulationParameterResultArgs; + return 1; + } + return 0; +} + + B3_SHARED_API b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; @@ -352,7 +378,7 @@ B3_SHARED_API int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandH { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); - command->m_physSimParamArgs.m_allowRealTimeSimulation = (enableRealTimeSimulation!=0); + command->m_physSimParamArgs.m_useRealTimeSimulation = (enableRealTimeSimulation!=0); command->m_updateFlags |= SIM_PARAM_UPDATE_REAL_TIME_SIMULATION; return 0; } @@ -395,15 +421,11 @@ B3_SHARED_API int b3PhysicsParamSetContactBreakingThreshold(b3SharedMemoryComman command->m_updateFlags |= SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD; return 0; } + B3_SHARED_API int b3PhysicsParamSetMaxNumCommandsPer1ms(b3SharedMemoryCommandHandle commandHandle, int maxNumCmdPer1ms) { - struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; - b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); - - command->m_physSimParamArgs.m_maxNumCmdPer1ms = maxNumCmdPer1ms; - command->m_updateFlags |= SIM_PARAM_MAX_CMD_PER_1MS; + //obsolete command return 0; - } B3_SHARED_API int b3PhysicsParamSetEnableFileCaching(b3SharedMemoryCommandHandle commandHandle, int enableFileCaching) @@ -3417,8 +3439,11 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3Phy } -B3_SHARED_API int b3GetStatusMassMatrix(b3SharedMemoryStatusHandle statusHandle, int* dofCount, double* massMatrix) +B3_SHARED_API int b3GetStatusMassMatrix(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int* dofCount, double* massMatrix) { + PhysicsClient* cl = (PhysicsClient*)physClient; + b3Assert(cl); + const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; btAssert(status->m_type == CMD_CALCULATED_MASS_MATRIX_COMPLETED); if (status->m_type != CMD_CALCULATED_MASS_MATRIX_COMPLETED) @@ -3430,12 +3455,7 @@ B3_SHARED_API int b3GetStatusMassMatrix(b3SharedMemoryStatusHandle statusHandle, } if (massMatrix) { - int numElements = status->m_massMatrixResultArgs.m_dofCount * status->m_massMatrixResultArgs.m_dofCount; - for (int i = 0; i < numElements; i++) - { - massMatrix[i] = status->m_massMatrixResultArgs.m_massMatrix[i]; - } - + cl->getCachedMassMatrix(status->m_massMatrixResultArgs.m_dofCount, massMatrix); } return true; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index f2dba0e30..54189dafa 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -263,14 +263,10 @@ B3_SHARED_API int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandH 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); - 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); - - - 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); @@ -279,6 +275,9 @@ B3_SHARED_API int b3PhysicsParamSetEnableFileCaching(b3SharedMemoryCommandHandle B3_SHARED_API int b3PhysicsParamSetRestitutionVelocityThreshold(b3SharedMemoryCommandHandle commandHandle, double restitutionVelocityThreshold); +B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestPhysicsParamCommand(b3PhysicsClientHandle physClient); +B3_SHARED_API int b3GetStatusPhysicsSimulationParameters(b3SharedMemoryStatusHandle statusHandle,struct b3PhysicsSimulationParameters* params); + //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 @@ -323,10 +322,8 @@ B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* angularJacobian); B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, const double* jointPositionsQ); -B3_SHARED_API int b3GetStatusMassMatrix(b3SharedMemoryStatusHandle statusHandle, - int* dofCount, - double* massMatrix); - +///the mass matrix is stored in column-major layout of size dofCount*dofCount +B3_SHARED_API int b3GetStatusMassMatrix(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int* dofCount, double* massMatrix); ///compute the joint positions to move the end effector to a desired target using inverse kinematics diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index e3f133acf..050a9a257 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -47,7 +47,7 @@ struct PhysicsClientSharedMemoryInternalData { btAlignedObjectArray m_cachedVREvents; btAlignedObjectArray m_cachedKeyboardEvents; btAlignedObjectArray m_cachedMouseEvents; - + btAlignedObjectArray m_cachedMassMatrix; btAlignedObjectArray m_raycastHits; btAlignedObjectArray m_bodyIdsRequestInfo; @@ -1210,6 +1210,25 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { break; } + case CMD_CALCULATED_MASS_MATRIX_FAILED: + { + b3Warning("calculate mass matrix failed"); + break; + } + case CMD_CALCULATED_MASS_MATRIX_COMPLETED: + { + double* matrixData = (double*)&this->m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0]; + m_data->m_cachedMassMatrix.resize(serverCmd.m_massMatrixResultArgs.m_dofCount*serverCmd.m_massMatrixResultArgs.m_dofCount); + for (int i=0;im_cachedMassMatrix[i] = matrixData[i]; + } + break; + } + case CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED: + { + break; + } default: { b3Error("Unknown server status %d\n", serverCmd.m_type); btAssert(0); @@ -1531,6 +1550,21 @@ void PhysicsClientSharedMemory::getCachedRaycastHits(struct b3RaycastInformation } +void PhysicsClientSharedMemory::getCachedMassMatrix(int dofCountCheck, double* massMatrix) +{ + int sz = dofCountCheck*dofCountCheck; + if (sz == m_data->m_cachedMassMatrix.size()) + { + for (int i=0;im_cachedMassMatrix[i]; + } + } +} + + + + void PhysicsClientSharedMemory::getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo) { visualShapesInfo->m_numVisualShapes = m_data->m_cachedVisualShapes.size(); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.h b/examples/SharedMemory/PhysicsClientSharedMemory.h index 31c717c02..54bb48817 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.h +++ b/examples/SharedMemory/PhysicsClientSharedMemory.h @@ -76,6 +76,8 @@ public: virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits); + virtual void getCachedMassMatrix(int dofCountCheck, double* massMatrix); + virtual void setTimeOut(double timeOutInSeconds); virtual double getTimeOut() const; diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 94ca9bea6..94922ff0d 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -40,7 +40,7 @@ struct PhysicsDirectInternalData btHashMap m_userConstraintInfoMap; char m_bulletStreamDataServerToClient[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE]; - + btAlignedObjectArray m_cachedMassMatrix; int m_cachedCameraPixelsWidth; int m_cachedCameraPixelsHeight; btAlignedObjectArray m_cachedCameraPixelsRGBA; @@ -951,6 +951,21 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd b3Warning("jacobian calculation failed"); break; } + case CMD_CALCULATED_MASS_MATRIX_FAILED: + { + b3Warning("calculate mass matrix failed"); + break; + } + case CMD_CALCULATED_MASS_MATRIX_COMPLETED: + { + double* matrixData = (double*)&m_data->m_bulletStreamDataServerToClient[0]; + m_data->m_cachedMassMatrix.resize(serverCmd.m_massMatrixResultArgs.m_dofCount*serverCmd.m_massMatrixResultArgs.m_dofCount); + for (int i=0;im_cachedMassMatrix[i] = matrixData[i]; + } + break; + } case CMD_ACTUAL_STATE_UPDATE_COMPLETED: { break; @@ -963,6 +978,10 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd { break; } + case CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED: + { + break; + } default: { //b3Warning("Unknown server status type"); @@ -1220,6 +1239,18 @@ void PhysicsDirect::getCachedRaycastHits(struct b3RaycastInformation* raycastHit raycastHits->m_rayHits = raycastHits->m_numRayHits? &m_data->m_raycastHits[0] : 0; } +void PhysicsDirect::getCachedMassMatrix(int dofCountCheck, double* massMatrix) +{ + int sz = dofCountCheck*dofCountCheck; + if (sz == m_data->m_cachedMassMatrix.size()) + { + for (int i=0;im_cachedMassMatrix[i]; + } + } +} + void PhysicsDirect::setTimeOut(double timeOutInSeconds) { m_data->m_timeOutInSeconds = timeOutInSeconds; diff --git a/examples/SharedMemory/PhysicsDirect.h b/examples/SharedMemory/PhysicsDirect.h index 282a61732..a42ec01d8 100644 --- a/examples/SharedMemory/PhysicsDirect.h +++ b/examples/SharedMemory/PhysicsDirect.h @@ -99,6 +99,8 @@ public: virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits); + virtual void getCachedMassMatrix(int dofCountCheck, double* massMatrix); + //the following APIs are for internal use for visualization: virtual bool connect(struct GUIHelperInterface* guiHelper); virtual void renderScene(); diff --git a/examples/SharedMemory/PhysicsLoopBack.cpp b/examples/SharedMemory/PhysicsLoopBack.cpp index 3067f2ace..e9880da7f 100644 --- a/examples/SharedMemory/PhysicsLoopBack.cpp +++ b/examples/SharedMemory/PhysicsLoopBack.cpp @@ -205,6 +205,11 @@ void PhysicsLoopBack::getCachedRaycastHits(struct b3RaycastInformation* raycastH return m_data->m_physicsClient->getCachedRaycastHits(raycastHits); } +void PhysicsLoopBack::getCachedMassMatrix(int dofCountCheck, double* massMatrix) +{ + m_data->m_physicsClient->getCachedMassMatrix(dofCountCheck,massMatrix); +} + void PhysicsLoopBack::setTimeOut(double timeOutInSeconds) { m_data->m_physicsClient->setTimeOut(timeOutInSeconds); diff --git a/examples/SharedMemory/PhysicsLoopBack.h b/examples/SharedMemory/PhysicsLoopBack.h index f83cd7234..44d54a40e 100644 --- a/examples/SharedMemory/PhysicsLoopBack.h +++ b/examples/SharedMemory/PhysicsLoopBack.h @@ -80,6 +80,8 @@ public: virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits); + virtual void getCachedMassMatrix(int dofCountCheck, double* massMatrix); + virtual void setTimeOut(double timeOutInSeconds); virtual double getTimeOut() const; }; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index f01cfa2ef..4cc70b724 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -360,7 +360,7 @@ struct CommandLogger case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: { fwrite((const char*)&command.m_updateFlags,sizeof(int), 1,m_file); - fwrite((const char*)&command.m_physSimParamArgs, sizeof(SendPhysicsSimulationParameters), 1,m_file); + fwrite((const char*)&command.m_physSimParamArgs, sizeof(b3PhysicsSimulationParameters), 1,m_file); break; } case CMD_REQUEST_CONTACT_POINT_INFORMATION: @@ -564,7 +564,7 @@ struct CommandLogPlayback cmd->m_physSimParamArgs = unused.m_physSimParamArgs; #else fread(&cmd->m_updateFlags,sizeof(int),1,m_file); - fread(&cmd->m_physSimParamArgs ,sizeof(SendPhysicsSimulationParameters),1,m_file); + fread(&cmd->m_physSimParamArgs ,sizeof(b3PhysicsSimulationParameters),1,m_file); #endif result = true; @@ -889,8 +889,18 @@ struct b3VRControllerEvents if (vrEvents[i].m_numMoveEvents) { m_vrEvents[controlledId].m_analogAxis = vrEvents[i].m_analogAxis; + for (int a=0;a<10;a++) + { + m_vrEvents[controlledId].m_auxAnalogAxis[a] = vrEvents[i].m_auxAnalogAxis[a]; + } + } else + { + m_vrEvents[controlledId].m_analogAxis = 0; + for (int a=0;a<10;a++) + { + m_vrEvents[controlledId].m_auxAnalogAxis[a] = 0; + } } - if (vrEvents[i].m_numMoveEvents+vrEvents[i].m_numButtonEvents) { m_vrEvents[controlledId].m_controllerId = vrEvents[i].m_controllerId; @@ -1440,7 +1450,7 @@ struct PhysicsServerCommandProcessorInternalData b3PluginManager m_pluginManager; - bool m_allowRealTimeSimulation; + bool m_useRealTimeSimulation; b3VRControllerEvents m_vrControllerEvents; @@ -1524,7 +1534,7 @@ struct PhysicsServerCommandProcessorInternalData PhysicsServerCommandProcessorInternalData(PhysicsCommandProcessorInterface* proc) :m_pluginManager(proc), - m_allowRealTimeSimulation(false), + m_useRealTimeSimulation(false), m_commandLogger(0), m_logPlayback(0), m_physicsDeltaTime(1./240.), @@ -3003,7 +3013,7 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char* bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes ) { // BT_PROFILE("processCommand"); - + bool hasStatus = false; { @@ -5690,6 +5700,33 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } break; } + + case CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS: + { + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED; + serverCmd.m_simulationParameterResultArgs.m_collisionFilterMode = m_data->m_broadphaseCollisionFilterCallback->m_filterMode; + serverCmd.m_simulationParameterResultArgs.m_contactBreakingThreshold = gContactBreakingThreshold; + serverCmd.m_simulationParameterResultArgs.m_defaultContactERP = m_data->m_dynamicsWorld->getSolverInfo().m_erp2; + serverCmd.m_simulationParameterResultArgs.m_defaultNonContactERP = m_data->m_dynamicsWorld->getSolverInfo().m_erp; + serverCmd.m_simulationParameterResultArgs.m_deltaTime = m_data->m_physicsDeltaTime; + serverCmd.m_simulationParameterResultArgs.m_enableFileCaching = b3IsFileCachingEnabled(); + serverCmd.m_simulationParameterResultArgs.m_frictionERP = m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP; + btVector3 grav = m_data->m_dynamicsWorld->getGravity(); + serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[0] = grav[0]; + serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[1] = grav[1]; + serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[2] = grav[2]; + serverCmd.m_simulationParameterResultArgs.m_internalSimFlags = gInternalSimFlags; + serverCmd.m_simulationParameterResultArgs.m_numSimulationSubSteps = m_data->m_numSimulationSubSteps; + serverCmd.m_simulationParameterResultArgs.m_numSolverIterations = m_data->m_dynamicsWorld->getSolverInfo().m_numIterations; + serverCmd.m_simulationParameterResultArgs.m_restitutionVelocityThreshold = m_data->m_dynamicsWorld->getSolverInfo().m_restitutionVelocityThreshold; + serverCmd.m_simulationParameterResultArgs.m_splitImpulsePenetrationThreshold = m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulsePenetrationThreshold; + serverCmd.m_simulationParameterResultArgs.m_useRealTimeSimulation = m_data->m_useRealTimeSimulation; + serverCmd.m_simulationParameterResultArgs.m_useSplitImpulse = m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulse; + hasStatus = true; + break; + } + case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: { BT_PROFILE("CMD_SEND_PHYSICS_SIMULATION_PARAMETERS"); @@ -5700,7 +5737,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_REAL_TIME_SIMULATION) { - m_data->m_allowRealTimeSimulation = clientCmd.m_physSimParamArgs.m_allowRealTimeSimulation; + m_data->m_useRealTimeSimulation = (clientCmd.m_physSimParamArgs.m_useRealTimeSimulation!=0); } //see @@ -6722,10 +6759,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm BT_PROFILE("CMD_CALCULATE_MASS_MATRIX"); SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_FAILED; InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_calculateMassMatrixArguments.m_bodyUniqueId); if (bodyHandle && bodyHandle->m_multiBody) { - serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_FAILED; btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody); @@ -6744,20 +6781,23 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { serverCmd.m_massMatrixResultArgs.m_dofCount = totDofs; // Fill in the result into the shared memory. - for (int i = 0; i < (totDofs); ++i) - { - for (int j = 0; j < (totDofs); ++j) - { - int element = (totDofs) * i + j; - serverCmd.m_massMatrixResultArgs.m_massMatrix[element] = massMatrix(i,j); - } - } - serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_COMPLETED; - } - else - { - serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_FAILED; + double* sharedBuf = (double*)bufferServerToClient; + int sizeInBytes = totDofs*totDofs*sizeof(double); + if (sizeInBytes < bufferSizeInBytes) + { + for (int i = 0; i < (totDofs); ++i) + { + for (int j = 0; j < (totDofs); ++j) + { + int element = (totDofs) * i + j; + + sharedBuf[element] = massMatrix(i,j); + } + } + serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_COMPLETED; + } } + } } @@ -8348,12 +8388,12 @@ double gSubStep = 0.f; void PhysicsServerCommandProcessor::enableRealTimeSimulation(bool enableRealTimeSim) { - m_data->m_allowRealTimeSimulation = enableRealTimeSim; + m_data->m_useRealTimeSimulation = enableRealTimeSim; } bool PhysicsServerCommandProcessor::isRealTimeSimulationEnabled() const { - return m_data->m_allowRealTimeSimulation; + return m_data->m_useRealTimeSimulation; } void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec,const struct b3VRControllerEvent* vrControllerEvents, int numVRControllerEvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents, const struct b3MouseEvent* mouseEvents, int numMouseEvents) @@ -8458,7 +8498,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec,const } } - if ((m_data->m_allowRealTimeSimulation) && m_data->m_guiHelper) + if ((m_data->m_useRealTimeSimulation) && m_data->m_guiHelper) { diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 208eeafe4..3cf461ae5 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -1330,7 +1330,7 @@ public: btVector3 getRayTo(int x,int y); virtual void vrControllerButtonCallback(int controllerId, int button, int state, float pos[4], float orientation[4]); - virtual void vrControllerMoveCallback(int controllerId, float pos[4], float orientation[4], float analogAxis); + virtual void vrControllerMoveCallback(int controllerId, float pos[4], float orientation[4], float analogAxis, float auxAnalogAxes[10]); virtual void vrHMDMoveCallback(int controllerId, float pos[4], float orientation[4]); virtual void vrGenericTrackerMoveCallback(int controllerId, float pos[4], float orientation[4]); @@ -2874,7 +2874,7 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt } -void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[4], float orn[4], float analogAxis) +void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[4], float orn[4], float analogAxis, float auxAnalogAxes[10]) { if (controllerId < 0 || controllerId >= MAX_VR_CONTROLLERS) @@ -2927,6 +2927,11 @@ void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[ m_args[0].m_vrControllerEvents[controllerId].m_orn[3] = trTotal.getRotation()[3]; m_args[0].m_vrControllerEvents[controllerId].m_numMoveEvents++; m_args[0].m_vrControllerEvents[controllerId].m_analogAxis = analogAxis; + for (int i=0;i<10;i++) + { + m_args[0].m_vrControllerEvents[controllerId].m_auxAnalogAxis[i] = auxAnalogAxes[i]; + } + m_args[0].m_csGUI->unlock(); } diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 8466fd2e6..5a983647c 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -428,25 +428,6 @@ enum EnumSimParamInternalSimFlags ///Controlling a robot involves sending the desired state to its joint motor controllers. ///The control mode determines the state variables used for motor control. -struct SendPhysicsSimulationParameters -{ - double m_deltaTime; - double m_gravityAcceleration[3]; - int m_numSimulationSubSteps; - int m_numSolverIterations; - bool m_allowRealTimeSimulation; - int m_useSplitImpulse; - double m_splitImpulsePenetrationThreshold; - double m_contactBreakingThreshold; - int m_maxNumCmdPer1ms; - int m_internalSimFlags; - double m_defaultContactERP; - int m_collisionFilterMode; - int m_enableFileCaching; - double m_restitutionVelocityThreshold; - double m_defaultNonContactERP; - double m_frictionERP; -}; struct LoadBunnyArgs { @@ -650,7 +631,6 @@ struct CalculateMassMatrixArgs struct CalculateMassMatrixResultArgs { int m_dofCount; - double m_massMatrix[MAX_DEGREE_OF_FREEDOM * MAX_DEGREE_OF_FREEDOM]; }; enum EnumCalculateInverseKinematicsFlags @@ -972,7 +952,7 @@ struct SharedMemoryCommand struct ChangeDynamicsInfoArgs m_changeDynamicsInfoArgs; struct GetDynamicsInfoArgs m_getDynamicsInfoArgs; struct InitPoseArgs m_initPoseArgs; - struct SendPhysicsSimulationParameters m_physSimParamArgs; + struct b3PhysicsSimulationParameters m_physSimParamArgs; struct BulletDataStreamArgs m_dataStreamArguments; struct SendDesiredStateArgs m_sendDesiredStateCommandArgument; struct RequestActualStateArgs m_requestActualStateInformationCommandArgument; @@ -1080,7 +1060,7 @@ struct SharedMemoryStatus struct SendMouseEvents m_sendMouseEvents; struct b3LoadTextureResultArgs m_loadTextureResultArguments; struct b3CustomCommandResultArgs m_customCommandResultArgs; - + struct b3PhysicsSimulationParameters m_simulationParameterResultArgs; }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index d911b44c8..3a02af8d4 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -5,7 +5,7 @@ ///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 201709260 +#define SHARED_MEMORY_MAGIC_NUMBER 201710050 //#define SHARED_MEMORY_MAGIC_NUMBER 201708270 //#define SHARED_MEMORY_MAGIC_NUMBER 201707140 //#define SHARED_MEMORY_MAGIC_NUMBER 201706015 @@ -75,6 +75,7 @@ enum EnumSharedMemoryClientCommand CMD_CHANGE_TEXTURE, CMD_SET_ADDITIONAL_SEARCH_PATH, CMD_CUSTOM_COMMAND, + CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS, //don't go beyond this command! CMD_MAX_CLIENT_COMMANDS, @@ -174,7 +175,7 @@ enum EnumSharedMemoryServerStatus CMD_CHANGE_TEXTURE_COMMAND_FAILED, CMD_CUSTOM_COMMAND_COMPLETED, CMD_CUSTOM_COMMAND_FAILED, - + CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED, //don't go beyond 'CMD_MAX_SERVER_COMMANDS! CMD_MAX_SERVER_COMMANDS }; @@ -338,6 +339,7 @@ enum b3VREventType VR_GENERIC_TRACKER_MOVE_EVENT=8, }; +#define MAX_VR_ANALOG_AXIS 5 #define MAX_VR_BUTTONS 64 #define MAX_VR_CONTROLLERS 8 @@ -379,7 +381,7 @@ struct b3VRControllerEvent float m_orn[4];//valid for VR_CONTROLLER_MOVE_EVENT and VR_CONTROLLER_BUTTON_EVENT float m_analogAxis;//valid if VR_CONTROLLER_MOVE_EVENT - + float m_auxAnalogAxis[MAX_VR_ANALOG_AXIS*2];//store x,y per axis, only valid if VR_CONTROLLER_MOVE_EVENT int m_buttons[MAX_VR_BUTTONS];//valid if VR_CONTROLLER_BUTTON_EVENT, see b3VRButtonInfo }; @@ -575,6 +577,7 @@ enum b3ConfigureDebugVisualizerEnum COV_ENABLE_SYNC_RENDERING_INTERNAL, COV_ENABLE_KEYBOARD_SHORTCUTS, COV_ENABLE_MOUSE_PICKING, + COV_ENABLE_Y_AXIS_UP, }; enum b3AddUserDebugItemEnum @@ -639,5 +642,24 @@ struct b3PluginArguments double m_floats[B3_MAX_PLUGIN_ARG_SIZE]; }; +struct b3PhysicsSimulationParameters +{ + double m_deltaTime; + double m_gravityAcceleration[3]; + int m_numSimulationSubSteps; + int m_numSolverIterations; + int m_useRealTimeSimulation; + int m_useSplitImpulse; + double m_splitImpulsePenetrationThreshold; + double m_contactBreakingThreshold; + int m_internalSimFlags; + double m_defaultContactERP; + int m_collisionFilterMode; + int m_enableFileCaching; + double m_restitutionVelocityThreshold; + double m_defaultNonContactERP; + double m_frictionERP; +}; + #endif//SHARED_MEMORY_PUBLIC_H diff --git a/examples/StandaloneMain/hellovr_opengl_main.cpp b/examples/StandaloneMain/hellovr_opengl_main.cpp index 1e48f879f..76517c2e6 100644 --- a/examples/StandaloneMain/hellovr_opengl_main.cpp +++ b/examples/StandaloneMain/hellovr_opengl_main.cpp @@ -376,10 +376,16 @@ void MyKeyboardCallback(int key, int state) extern bool useShadowMap; static bool gEnableVRRenderControllers=true; static bool gEnableVRRendering = true; - +static int gUpAxis = 2; void VRPhysicsServerVisualizerFlagCallback(int flag, bool enable) { + if (flag == COV_ENABLE_Y_AXIS_UP) + { + //either Y = up or Z + gUpAxis = enable? 1:2; + } + if (flag == COV_ENABLE_SHADOWS) { useShadowMap = enable; @@ -808,7 +814,15 @@ bool CMainApplication::HandleInput() for (int button = 0; button < vr::k_EButton_Max; button++) { uint64_t trigger = vr::ButtonMaskFromId((vr::EVRButtonId)button); - + + btAssert(vr::k_unControllerStateAxisCount>=5); + float allAxis[10];//store x,y times 5 controllers + int index=0; + for (int i=0;i<5;i++) + { + allAxis[index++]=state.rAxis[i].x; + allAxis[index++]=state.rAxis[i].y; + } bool isTrigger = (state.ulButtonPressed&trigger) != 0; if (isTrigger) { @@ -818,31 +832,15 @@ bool CMainApplication::HandleInput() if ((sPrevStates[unDevice].ulButtonPressed&trigger)==0) { // printf("Device PRESSED: %d, button %d\n", unDevice, button); - if (button==2) - { - //glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); - - ///todo(erwincoumans) can't use reguar debug drawer, because physics/graphics are not in sync - ///so it can (and likely will) cause crashes - ///add a special debug drawer that deals with this - //gDebugDrawFlags = btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints+ - //btIDebugDraw::DBG_DrawConstraintLimits+ - //btIDebugDraw::DBG_DrawConstraints - //; - //gDebugDrawFlags = btIDebugDraw::DBG_DrawFrames; - - - - } - sExample->vrControllerButtonCallback(unDevice, button, 1, pos, orn); } else { + // printf("Device MOVED: %d\n", unDevice); - sExample->vrControllerMoveCallback(unDevice, pos, orn, state.rAxis[1].x); + sExample->vrControllerMoveCallback(unDevice, pos, orn, state.rAxis[1].x, allAxis); } } else @@ -865,7 +863,7 @@ bool CMainApplication::HandleInput() } else { - sExample->vrControllerMoveCallback(unDevice, pos, orn, state.rAxis[1].x); + sExample->vrControllerMoveCallback(unDevice, pos, orn, state.rAxis[1].x,allAxis); } } } @@ -890,6 +888,7 @@ void CMainApplication::RunMainLoop() while ( !bQuit && !m_app->m_window->requestedExit()) { + this->m_app->setUpAxis(gUpAxis); b3ChromeUtilsEnableProfiling(); if (gEnableVRRendering) { diff --git a/examples/pybullet/examples/vrEvent.py b/examples/pybullet/examples/vrEvent.py index beb369b1d..790d6881b 100644 --- a/examples/pybullet/examples/vrEvent.py +++ b/examples/pybullet/examples/vrEvent.py @@ -8,7 +8,9 @@ import pybullet as p CONTROLLER_ID = 0 POSITION=1 ORIENTATION=2 +NUM_MOVE_EVENTS=5 BUTTONS=6 +ANALOG_AXIS=8 #assume that the VR physics server is already started before c = p.connect(p.SHARED_MEMORY) @@ -20,7 +22,7 @@ p.setInternalSimFlags(0)#don't load default robot assets etc p.resetSimulation() p.loadURDF("plane.urdf") -prevPosition=[None]*p.VR_MAX_CONTROLLERS +prevPosition=[[0,0,0]]*p.VR_MAX_CONTROLLERS colors=[0.,0.5,0.5]*p.VR_MAX_CONTROLLERS widths = [3]*p.VR_MAX_CONTROLLERS @@ -32,10 +34,27 @@ colors[3] = [0,0,0.5] colors[4] = [0.5,0.5,0.] colors[5] = [.5,.5,.5] -while True: +controllerId = -1 +pt=[0,0,0] + +print("waiting for VR controller trigger") +while (controllerId<0): events = p.getVREvents() + for e in (events): + if (e[BUTTONS][33]==p.VR_BUTTON_IS_DOWN): + controllerId = e[CONTROLLER_ID] + if (e[BUTTONS][32]==p.VR_BUTTON_IS_DOWN): + controllerId = e[CONTROLLER_ID] + +print("Using controllerId="+str(controllerId)) + +while True: + events = p.getVREvents(allAnalogAxes=1) for e in (events): + if (e[CONTROLLER_ID]==controllerId ): + for a in range(10): + print("analog axis"+str(a)+"="+str(e[8][a])) if (e[BUTTONS][33]&p.VR_BUTTON_WAS_TRIGGERED): prevPosition[e[CONTROLLER_ID]] = e[POSITION] if (e[BUTTONS][32]&p.VR_BUTTON_WAS_TRIGGERED): @@ -51,7 +70,10 @@ while True: pt = prevPosition[e[CONTROLLER_ID]] #print(prevPosition[e[0]]) - #print(e[1]) + print("e[POSITION]") + print(e[POSITION]) + print("pt") + print(pt) diff = [pt[0]-e[POSITION][0],pt[1]-e[POSITION][1],pt[2]-e[POSITION][2]] lenSqr = diff[0]*diff[0]+diff[1]*diff[1]+diff[2]*diff[2] ptDistThreshold = 0.01 diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index cea0302ce..e2622db3f 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -503,6 +503,35 @@ void b3pybulletExitFunc(void) } +static PyObject* pybullet_getConnectionInfo(PyObject* self, PyObject* args, PyObject* keywds) +{ + int physicsClientId = 0; + int isConnected=0; + int method=0; + PyObject* pylist = 0; + PyObject* val = 0; + b3PhysicsClientHandle sm = 0; + static char* kwlist[] = {"physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm != 0) + { + if (b3CanSubmitCommand(sm)) + { + isConnected = 1; + method = sPhysicsClientsGUI[physicsClientId]; + } + } + + val = Py_BuildValue("{s:i,s:i}","isConnected", isConnected, "connectionMethod", method); + return val; + +} + + static PyObject* pybullet_saveWorld(PyObject* self, PyObject* args, PyObject* keywds) { const char* worldFileName = ""; @@ -824,6 +853,56 @@ static PyObject* pybullet_getDynamicsInfo(PyObject* self, PyObject* args, PyObje return NULL; } +static PyObject* pybullet_getPhysicsEngineParameters(PyObject* self, PyObject* args, PyObject* keywds) +{ + b3PhysicsClientHandle sm = 0; + PyObject* val=0; + int physicsClientId = 0; + static char* kwlist[] = {"physicsClientId", NULL}; + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + b3SharedMemoryCommandHandle command = b3InitRequestPhysicsParamCommand(sm); + b3SharedMemoryStatusHandle statusHandle; + struct b3PhysicsSimulationParameters params; + int statusType; + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + if (statusType!=CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED) + { + PyErr_SetString(SpamError, "Couldn't get physics simulation parameters."); + return NULL; + } + b3GetStatusPhysicsSimulationParameters(statusHandle,¶ms); + + //for now, return a subset, expose more/all on request + val = Py_BuildValue("{s:d,s:i,s:i,s:i,s:d,s:d,s:d}", + "fixedTimeStep", params.m_deltaTime, + "numSubSteps", params.m_numSimulationSubSteps, + "numSolverIterations", params.m_numSolverIterations, + "useRealTimeSimulation", params.m_useRealTimeSimulation, + "gravityAccelerationX", params.m_gravityAcceleration[0], + "gravityAccelerationY", params.m_gravityAcceleration[1], + "gravityAccelerationZ", params.m_gravityAcceleration[2] + ); + return val; + } + //"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "erp", "contactERP", "frictionERP", + //val = Py_BuildValue("{s:i,s:i}","isConnected", isConnected, "connectionMethod", method); + + +} static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* args, PyObject* keywds) { @@ -918,19 +997,10 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar { b3PhysicsParamSetDefaultFrictionERP(command,frictionERP); } - //ret = b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); } -#if 0 - 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 b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps); - int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation); - int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations); -#endif + Py_INCREF(Py_None); return Py_None; @@ -4207,9 +4277,10 @@ static PyObject* pybullet_getVREvents(PyObject* self, PyObject* args, PyObject* int statusType; int deviceTypeFilter = VR_DEVICE_CONTROLLER; int physicsClientId = 0; + int allAnalogAxes = 0; b3PhysicsClientHandle sm = 0; - static char* kwlist[] = {"deviceTypeFilter", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|ii", kwlist, &deviceTypeFilter, &physicsClientId)) + static char* kwlist[] = {"deviceTypeFilter", "allAnalogAxes", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|iii", kwlist, &deviceTypeFilter, &allAnalogAxes, &physicsClientId)) { return NULL; } @@ -4237,7 +4308,8 @@ static PyObject* pybullet_getVREvents(PyObject* self, PyObject* args, PyObject* vrEventsObj = PyTuple_New(vrEvents.m_numControllerEvents); for (i = 0; i < vrEvents.m_numControllerEvents; i++) { - PyObject* vrEventObj = PyTuple_New(8); + int numFields = allAnalogAxes? 9 : 8; + PyObject* vrEventObj = PyTuple_New(numFields); PyTuple_SetItem(vrEventObj, 0, PyInt_FromLong(vrEvents.m_controllerEvents[i].m_controllerId)); { @@ -4270,6 +4342,19 @@ static PyObject* pybullet_getVREvents(PyObject* self, PyObject* args, PyObject* PyTuple_SetItem(vrEventObj, 6, buttonsObj); } PyTuple_SetItem(vrEventObj, 7, PyInt_FromLong(vrEvents.m_controllerEvents[i].m_deviceType)); + + if (allAnalogAxes) + { + PyObject* buttonsObj = PyTuple_New(MAX_VR_ANALOG_AXIS*2); + int b; + for (b = 0; b < MAX_VR_ANALOG_AXIS*2; b++) + { + PyObject* axisVal = PyFloat_FromDouble(vrEvents.m_controllerEvents[i].m_auxAnalogAxis[b]); + PyTuple_SetItem(buttonsObj, b, axisVal); + } + PyTuple_SetItem(vrEventObj, 8, buttonsObj); + } + PyTuple_SetItem(vrEventsObj, i, vrEventObj); } return vrEventsObj; @@ -7314,13 +7399,14 @@ static PyObject* pybullet_calculateMassMatrix(PyObject* self, PyObject* args, Py if (statusType == CMD_CALCULATED_MASS_MATRIX_COMPLETED) { int dofCount; - b3GetStatusMassMatrix(statusHandle, &dofCount, NULL); + b3GetStatusMassMatrix(sm, statusHandle, &dofCount, NULL); if (dofCount) { - pyResultList = PyTuple_New(dofCount); int byteSizeDofCount = sizeof(double) * dofCount; + pyResultList = PyTuple_New(dofCount); + massMatrix = (double*)malloc(dofCount * byteSizeDofCount); - b3GetStatusMassMatrix(statusHandle, NULL, massMatrix); + b3GetStatusMassMatrix(sm, statusHandle, NULL, massMatrix); if (massMatrix) { int r; @@ -7372,6 +7458,10 @@ static PyMethodDef SpamMethods[] = { "disconnect(physicsClientId=0)\n" "Disconnect from the physics server."}, + {"getConnectionInfo", (PyCFunction)pybullet_getConnectionInfo, METH_VARARGS | METH_KEYWORDS, + "getConnectionInfo(physicsClientId=0)\n" + "Return if a given client id is connected, and using what method."}, + {"resetSimulation", (PyCFunction)pybullet_resetSimulation, METH_VARARGS | METH_KEYWORDS, "resetSimulation(physicsClientId=0)\n" "Reset the simulation: remove all objects and start from an empty world."}, @@ -7404,6 +7494,9 @@ static PyMethodDef SpamMethods[] = { {"setPhysicsEngineParameter", (PyCFunction)pybullet_setPhysicsEngineParameter, METH_VARARGS | METH_KEYWORDS, "Set some internal physics engine parameter, such as cfm or erp etc."}, + {"getPhysicsEngineParameters", (PyCFunction)pybullet_getPhysicsEngineParameters, METH_VARARGS | METH_KEYWORDS, + "Get the current values of internal physics engine parameters"}, + {"setInternalSimFlags", (PyCFunction)pybullet_setInternalSimFlags, METH_VARARGS | METH_KEYWORDS, "This is for experimental purposes, use at own risk, magic may or not happen"},