From da2cc483b4c8b56a6f6ae220d6e50aa1ae1f744f Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 27 Dec 2016 20:25:52 -0800 Subject: [PATCH 1/5] pybullet: allow to connect to multiple physics servers, while maintaining backwards compatibility. connect method returns an integer 'physicsClientId'. This can be passed as optional argument to each method (except for a few 'obsolete' ones. --- examples/pybullet/pybullet.c | 1581 +++++++++++++++++++++------------- 1 file changed, 978 insertions(+), 603 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 5928d357d..88cf53b85 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -28,7 +28,10 @@ enum eCONNECT_METHOD { }; static PyObject* SpamError; -static b3PhysicsClientHandle sm = 0; + +#define MAX_PHYSICS_CLIENTS 1024 +static b3PhysicsClientHandle sPhysicsClients[MAX_PHYSICS_CLIENTS] = {0}; +static int sNumPhysicsClients=0; static double pybullet_internalGetFloatFromSequence(PyObject* seq, int index) { @@ -148,12 +151,24 @@ static int pybullet_internalSetVector4d(PyObject* obVec, double vector[4]) { // Step through one timestep of the simulation -static PyObject* pybullet_stepSimulation(PyObject* self, PyObject* args) { - if (0 == sm) { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } +static PyObject* pybullet_stepSimulation(PyObject* self, PyObject* args, PyObject *keywds) +{ + int physicsClientId = 0; + static char *kwlist[] = { "physicsClientId", NULL }; + b3PhysicsClientHandle sm=0; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist,&physicsClientId)) + { + return NULL; + } + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + sm = sPhysicsClients[physicsClientId]; + + { b3SharedMemoryStatusHandle statusHandle; int statusType; @@ -169,12 +184,20 @@ static PyObject* pybullet_stepSimulation(PyObject* self, PyObject* args) { return Py_None; } -static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args) { - if (0 != sm) { - PyErr_SetString(SpamError, - "Already connected to physics server, disconnect first."); - return NULL; - } +static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, PyObject* keywds) { + + + int freeIndex = -1; + int i; + b3PhysicsClientHandle sm=0; + + if (sNumPhysicsClients>=MAX_PHYSICS_CLIENTS) + { + PyErr_SetString(SpamError, + "Exceeding maximum number of physics connections."); + return NULL; + } + { int method = eCONNECT_GUI; @@ -258,40 +281,80 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args) { }; } - Py_INCREF(Py_None); - return Py_None; + + for (i=0;i=0) + { + sPhysicsClients[freeIndex] = sm; + sNumPhysicsClients++; + } + + return PyInt_FromLong(freeIndex); } static PyObject* pybullet_disconnectPhysicsServer(PyObject* self, - PyObject* args) { - if (0 == sm) { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } + PyObject* args, + PyObject *keywds) { + + int physicsClientId = 0; + b3PhysicsClientHandle sm=0; + static char *kwlist[] = { "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist,&physicsClientId)) + { + return NULL; + } + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + sm = sPhysicsClients[physicsClientId]; + + { b3DisconnectSharedMemory(sm); sm = 0; } + sPhysicsClients[physicsClientId] = 0; + sNumPhysicsClients--; + Py_INCREF(Py_None); return Py_None; } -static PyObject* pybullet_saveWorld(PyObject* self, PyObject* args) { +static PyObject* pybullet_saveWorld(PyObject* self, PyObject* args,PyObject *keywds) { int size = PySequence_Size(args); const char* worldFileName = ""; - if (0 == sm) { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - if (size == 1) { - if (!PyArg_ParseTuple(args, "s", &worldFileName)) + b3PhysicsClientHandle sm = 0; + int physicsClientId = 0; + static char *kwlist[] = { "worldFileName","physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &worldFileName,&physicsClientId)) { return NULL; } - else + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + sm = sPhysicsClients[physicsClientId]; + if (0==sm) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { b3SharedMemoryCommandHandle command; b3SharedMemoryStatusHandle statusHandle; @@ -306,10 +369,9 @@ static PyObject* pybullet_saveWorld(PyObject* self, PyObject* args) { } Py_INCREF(Py_None); return Py_None; - } + } - PyErr_SetString(SpamError, "Cannot execute saveWorld command."); return NULL; @@ -317,7 +379,7 @@ static PyObject* pybullet_saveWorld(PyObject* self, PyObject* args) { } #define MAX_SDF_BODIES 512 -static PyObject* pybullet_loadBullet(PyObject* self, PyObject* args) +static PyObject* pybullet_loadBullet(PyObject* self, PyObject* args,PyObject *keywds) { int size = PySequence_Size(args); const char* bulletFileName = ""; @@ -326,14 +388,22 @@ static PyObject* pybullet_loadBullet(PyObject* self, PyObject* args) b3SharedMemoryCommandHandle command; int i,numBodies; int bodyIndicesOut[MAX_SDF_BODIES]; - PyObject* pylist = 0; - if (0 == sm) { + PyObject* pylist = 0; + b3PhysicsClientHandle sm = 0; + + int physicsClientId = 0; + static char *kwlist[] = { "bulletFileName","physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &bulletFileName,&physicsClientId)) + { + return NULL; + } + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; -} - if (size == 1) { - if (!PyArg_ParseTuple(args, "s", &bulletFileName)) return NULL; } + sm = sPhysicsClients[physicsClientId]; + command = b3LoadBulletCommandInit(sm, bulletFileName); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); @@ -363,21 +433,29 @@ static PyObject* pybullet_loadBullet(PyObject* self, PyObject* args) } -static PyObject* pybullet_saveBullet(PyObject* self, PyObject* args) +static PyObject* pybullet_saveBullet(PyObject* self, PyObject* args, PyObject* keywds) { int size = PySequence_Size(args); const char* bulletFileName = ""; b3SharedMemoryStatusHandle statusHandle; int statusType; b3SharedMemoryCommandHandle command; + b3PhysicsClientHandle sm = 0; - if (0 == sm) { + int physicsClientId = 0; + static char *kwlist[] = { "bulletFileName","physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &bulletFileName,&physicsClientId)) + { + return NULL; + } + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - if (size == 1) { - if (!PyArg_ParseTuple(args, "s", &bulletFileName)) return NULL; - } + sm = sPhysicsClients[physicsClientId]; + + command = b3SaveBulletCommandInit(sm, bulletFileName); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); @@ -392,22 +470,30 @@ static PyObject* pybullet_saveBullet(PyObject* self, PyObject* args) -static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args) +static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args, PyObject* keywds) { int size = PySequence_Size(args); - const char* mjcfjFileName = ""; + const char* mjcfFileName = ""; b3SharedMemoryStatusHandle statusHandle; int statusType; b3SharedMemoryCommandHandle command; + b3PhysicsClientHandle sm = 0; - if (0 == sm) { + int physicsClientId = 0; + static char *kwlist[] = { "mjcfFileName","physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &mjcfFileName,&physicsClientId)) + { + return NULL; + } + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - if (size == 1) { - if (!PyArg_ParseTuple(args, "s", &mjcfjFileName)) return NULL; - } - command = b3LoadMJCFCommandInit(sm, mjcfjFileName); + sm = sPhysicsClients[physicsClientId]; + + + command = b3LoadMJCFCommandInit(sm, mjcfFileName); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); if (statusType != CMD_MJCF_LOADING_COMPLETED) @@ -426,18 +512,23 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar int useSplitImpulse = -1; double splitImpulsePenetrationThreshold = -1; int numSubSteps = -1; + b3PhysicsClientHandle sm = 0; - if (0 == sm) { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } + int physicsClientId = 0; + static char *kwlist[] = { "fixedTimeStep", "numSolverIterations","useSplitImpulse","splitImpulsePenetrationThreshold", "numSubSteps","physicsClientId", NULL }; - static char *kwlist[] = { "fixedTimeStep", "numSolverIterations","useSplitImpulse","splitImpulsePenetrationThreshold", "numSubSteps", NULL }; - - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidi", kwlist,&fixedTimeStep,&numSolverIterations,&useSplitImpulse,&splitImpulsePenetrationThreshold,&numSubSteps)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidii", kwlist,&fixedTimeStep,&numSolverIterations,&useSplitImpulse,&splitImpulsePenetrationThreshold,&numSubSteps,&physicsClientId)) { return NULL; } + + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + sm = sPhysicsClients[physicsClientId]; + { b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); b3SharedMemoryStatusHandle statusHandle; @@ -490,7 +581,9 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args, PyObject *keywds) { int size = PySequence_Size(args); - static char *kwlist[] = { "fileName", "basePosition", "baseOrientation", "useMaximalCoordinates","useFixedBase", NULL }; + int physicsClientId = 0; + + static char *kwlist[] = { "fileName", "basePosition", "baseOrientation", "useMaximalCoordinates","useFixedBase","physicsClientId", NULL }; static char *kwlistBackwardCompatible4[] = { "fileName", "startPosX", "startPosY", "startPosZ", NULL }; static char *kwlistBackwardCompatible8[] = { "fileName", "startPosX", "startPosY", "startPosZ", "startOrnX", "startOrnY","startOrnZ","startOrnW", NULL }; @@ -510,12 +603,7 @@ static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args, PyObject *key int useFixedBase = 0; int backwardsCompatibilityArgs = 0; - - if (0 == sm) { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - +b3PhysicsClientHandle sm=0; if (PyArg_ParseTupleAndKeywords(args, keywds, "sddd", kwlistBackwardCompatible4, &urdfFileName, &startPosX, &startPosY, &startPosZ)) { @@ -545,9 +633,9 @@ static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args, PyObject *key PyObject* baseOrnObj = 0; double basePos[3]; double baseOrn[4]; + - - if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|OOii", kwlist, &urdfFileName, &basePosObj, &baseOrnObj, &useMaximalCoordinates,&useFixedBase)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|OOiii", kwlist, &urdfFileName, &basePosObj, &baseOrnObj, &useMaximalCoordinates,&useFixedBase,&physicsClientId)) { return NULL; @@ -581,6 +669,14 @@ static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args, PyObject *key } } + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + sm = sPhysicsClients[physicsClientId]; + + if (strlen(urdfFileName)) { // printf("(%f, %f, %f) (%f, %f, %f, %f)\n", // startPosX,startPosY,startPosZ,startOrnX, startOrnY,startOrnZ, startOrnW); @@ -622,7 +718,7 @@ static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args, PyObject *key -static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args) { +static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args, PyObject *keywds) { const char* sdfFileName = ""; int size = PySequence_Size(args); int numBodies = 0; @@ -632,16 +728,22 @@ static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args) { b3SharedMemoryStatusHandle statusHandle; int statusType; b3SharedMemoryCommandHandle commandHandle; +b3PhysicsClientHandle sm = 0; - if (0 == sm) { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - if (size == 1) { - if (!PyArg_ParseTuple(args, "s", &sdfFileName)) return NULL; - } + int physicsClientId = 0; + static char *kwlist[] = { "sdfFileName","physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &sdfFileName,&physicsClientId)) + { + return NULL; + } + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + sm = sPhysicsClients[physicsClientId]; + commandHandle = b3LoadSdfCommandInit(sm, sdfFileName); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); @@ -668,12 +770,22 @@ static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args) { } // Reset the simulation to remove all loaded objects -static PyObject* pybullet_resetSimulation(PyObject* self, PyObject* args) { - if (0 == sm) { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } +static PyObject* pybullet_resetSimulation(PyObject* self, PyObject* args, PyObject* keywds) { + int physicsClientId = 0; + static char *kwlist[] = { "physicsClientId", NULL }; + b3PhysicsClientHandle sm = 0; + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist,&physicsClientId)) + { + return NULL; + } + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + sm = sPhysicsClients[physicsClientId]; { b3SharedMemoryStatusHandle statusHandle; statusHandle = b3SubmitClientCommandAndWaitStatus( @@ -682,7 +794,7 @@ static PyObject* pybullet_resetSimulation(PyObject* self, PyObject* args) { Py_INCREF(Py_None); return Py_None; } - +//this method is obsolete, use pybullet_setJointMotorControl2 instead static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) { int size; int bodyIndex, jointIndex, controlMode; @@ -694,6 +806,8 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) { double kp = 0.1; double kd = 1.0; int valid = 0; + int physicsClientId = 0; + b3PhysicsClientHandle sm = sPhysicsClients[physicsClientId]; if (0 == sm) { PyErr_SetString(SpamError, "Not connected to physics server."); @@ -851,26 +965,121 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) { return NULL; } -static PyObject* pybullet_setRealTimeSimulation(PyObject* self, - PyObject* args) { - if (0 == sm) { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } + +static PyObject* pybullet_setJointMotorControl2(PyObject* self, PyObject* args, PyObject* keywds) +{ + int bodyIndex, jointIndex, controlMode; + + double targetPosition = 0.0; + double targetVelocity = 0.0; + double force = 100000.0; + double kp = 0.1; + double kd = 1.0; +b3PhysicsClientHandle sm = 0; + + int physicsClientId = 0; + static char *kwlist[] = { "bodyIndex", "jointIndex", "controlMode", "targetPosition", "targetVelocity" + , "force", "positionGain", "velocityGain", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iii|dddddi", kwlist,&bodyIndex, &jointIndex, &controlMode, + &targetPosition, &targetVelocity,&force, &kp, &kd, &physicsClientId)) + { + return NULL; + } + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + sm = sPhysicsClients[physicsClientId]; { + int numJoints; + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + struct b3JointInfo info; + + numJoints = b3GetNumJoints(sm, bodyIndex); + if ((jointIndex >= numJoints) || (jointIndex < 0)) { + PyErr_SetString(SpamError, "Joint index out-of-range."); + return NULL; + } + + if ((controlMode != CONTROL_MODE_VELOCITY) && + (controlMode != CONTROL_MODE_TORQUE) && + (controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)) { + PyErr_SetString(SpamError, "Illegral control mode."); + return NULL; + } + + commandHandle = b3JointControlCommandInit2(sm, bodyIndex, controlMode); + + b3GetJointInfo(sm, bodyIndex, jointIndex, &info); + + switch (controlMode) { + case CONTROL_MODE_VELOCITY: { + b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, + targetVelocity); + b3JointControlSetKd(commandHandle, info.m_uIndex, kd); + b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, force); + break; + } + + case CONTROL_MODE_TORQUE: { + b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, + force); + break; + } + + case CONTROL_MODE_POSITION_VELOCITY_PD: { + 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, force); + break; + } + default: {} + }; + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + + Py_INCREF(Py_None); + return Py_None; + } + PyErr_SetString(SpamError, "Error parsing arguments in setJointControl."); + return NULL; +} + +static PyObject* pybullet_setRealTimeSimulation(PyObject* self, + PyObject* args, + PyObject* keywds) { + int enableRealTimeSimulation = 0; int ret; + b3PhysicsClientHandle sm = 0; + + int physicsClientId = 0; + static char *kwlist[] = { "enableRealTimeSimulation", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,&enableRealTimeSimulation, &physicsClientId)) + { + return NULL; + } + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + sm = sPhysicsClients[physicsClientId]; + + + { + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); b3SharedMemoryStatusHandle statusHandle; - if (!PyArg_ParseTuple(args, "i", &enableRealTimeSimulation)) { - PyErr_SetString( - SpamError, - "setRealTimeSimulation expected a single value (integer)."); - return NULL; - } ret = b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation); @@ -885,27 +1094,33 @@ static PyObject* pybullet_setRealTimeSimulation(PyObject* self, static PyObject* pybullet_setInternalSimFlags(PyObject* self, - PyObject* args) { - if (0 == sm) { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } + PyObject* args, PyObject* keywds) { + + int flags = 0; + int ret; + int physicsClientId = 0; +b3PhysicsClientHandle sm = 0; + + static char *kwlist[] = { "flags", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,&flags, &physicsClientId)) + { + return NULL; + } + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + sm = sPhysicsClients[physicsClientId]; + { - int enableRealTimeSimulation = 0; - int ret; - + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); b3SharedMemoryStatusHandle statusHandle; - if (!PyArg_ParseTuple(args, "i", &enableRealTimeSimulation)) { - PyErr_SetString( - SpamError, - "setInternalSimFlags expected a single value (integer)."); - return NULL; - } ret = - b3PhysicsParamSetInternalSimFlags(command, enableRealTimeSimulation); + b3PhysicsParamSetInternalSimFlags(command, flags); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); // ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); @@ -916,25 +1131,32 @@ static PyObject* pybullet_setInternalSimFlags(PyObject* self, } // Set the gravity of the world with (x, y, z) arguments -static PyObject* pybullet_setGravity(PyObject* self, PyObject* args) { - if (0 == sm) { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - +static PyObject* pybullet_setGravity(PyObject* self, PyObject* args, PyObject* keywds) { + { double gravX = 0.0; double gravY = 0.0; double gravZ = -10.0; int ret; + b3PhysicsClientHandle sm = 0; + + int physicsClientId = 0; + static char *kwlist[] = { "gravX", "gravY", "gravZ", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "ddd|i", kwlist,&gravX, &gravY, &gravZ, &physicsClientId)) + { + return NULL; + } + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + sm = sPhysicsClients[physicsClientId]; + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); b3SharedMemoryStatusHandle statusHandle; - if (!PyArg_ParseTuple(args, "ddd", &gravX, &gravY, &gravZ)) { - PyErr_SetString(SpamError, "setGravity expected (x,y,z) values."); - return NULL; - } ret = b3PhysicsParamSetGravity(command, gravX, gravY, gravZ); // ret = b3PhysicsParamSetTimeStep(command, timeStep); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); @@ -945,55 +1167,58 @@ static PyObject* pybullet_setGravity(PyObject* self, PyObject* args) { return Py_None; } -static PyObject* pybullet_setTimeStep(PyObject* self, PyObject* args) { - if (0 == sm) { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } +static PyObject* pybullet_setTimeStep(PyObject* self, PyObject* args, PyObject* keywds) +{ + double timeStep = 0.001; + int ret; + int physicsClientId = 0; + static char *kwlist[] = { "timeStep", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "d|i", kwlist,&timeStep, &physicsClientId)) + { + return NULL; + } + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + { + b3PhysicsClientHandle sm = sPhysicsClients[physicsClientId]; - { - double timeStep = 0.001; - int ret; + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); + b3SharedMemoryStatusHandle statusHandle; + ret = b3PhysicsParamSetTimeStep(command, timeStep); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); - b3SharedMemoryStatusHandle statusHandle; - - if (!PyArg_ParseTuple(args, "d", &timeStep)) { - PyErr_SetString(SpamError, - "setTimeStep expected a single value (double)."); - return NULL; - } - ret = b3PhysicsParamSetTimeStep(command, timeStep); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - // ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); - } - - Py_INCREF(Py_None); - return Py_None; + Py_INCREF(Py_None); + return Py_None; +} } static PyObject * -pybullet_setDefaultContactERP(PyObject* self, PyObject* args) -{ - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - { - double defaultContactERP=0.005; - int ret; - - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); +pybullet_setDefaultContactERP(PyObject* self, PyObject* args,PyObject* keywds) +{ + double defaultContactERP=0.005; + int physicsClientId = 0; + static char *kwlist[] = { "defaultContactERP", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "d|i", kwlist,&defaultContactERP, &physicsClientId)) + { + return NULL; + } + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + { + b3PhysicsClientHandle sm = sPhysicsClients[physicsClientId]; + int ret; + b3SharedMemoryStatusHandle statusHandle; - if (!PyArg_ParseTuple(args, "d", &defaultContactERP)) - { - PyErr_SetString(SpamError, "default Contact ERP expected a single value (double)."); - return NULL; - } + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); ret = b3PhysicsParamSetDefaultContactERP(command, defaultContactERP); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); @@ -1004,7 +1229,7 @@ pybullet_setDefaultContactERP(PyObject* self, PyObject* args) } static int pybullet_internalGetBaseVelocity( - int bodyIndex, double baseLinearVelocity[3], double baseAngularVelocity[3]) { + int bodyIndex, double baseLinearVelocity[3], double baseAngularVelocity[3],b3PhysicsClientHandle sm) { baseLinearVelocity[0] = 0.; baseLinearVelocity[1] = 0.; baseLinearVelocity[2] = 0.; @@ -1064,7 +1289,7 @@ static int pybullet_internalGetBaseVelocity( // Internal function used to get the base position and orientation // Orientation is returned in quaternions static int pybullet_internalGetBasePositionAndOrientation( - int bodyIndex, double basePosition[3], double baseOrientation[4]) { + int bodyIndex, double basePosition[3], double baseOrientation[4],b3PhysicsClientHandle sm) { basePosition[0] = 0.; basePosition[1] = 0.; basePosition[2] = 0.; @@ -1127,25 +1352,29 @@ static int pybullet_internalGetBasePositionAndOrientation( // Object is retrieved based on body index, which is the order // the object was loaded into the simulation (0-based) static PyObject* pybullet_getBasePositionAndOrientation(PyObject* self, - PyObject* args) { - int bodyIndex = -1; + PyObject* args, PyObject* keywds) { + int bodyUniqueId = -1; double basePosition[3]; double baseOrientation[4]; PyObject* pylistPos; PyObject* pylistOrientation; + b3PhysicsClientHandle sm = 0; - if (0 == sm) { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - if (!PyArg_ParseTuple(args, "i", &bodyIndex)) { - PyErr_SetString(SpamError, "Expected a body index (integer)."); - return NULL; - } - + int physicsClientId = 0; + static char *kwlist[] = { "bodyUniqueId", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,&bodyUniqueId, &physicsClientId)) + { + return NULL; + } + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + sm = sPhysicsClients[physicsClientId]; + if (0 == pybullet_internalGetBasePositionAndOrientation( - bodyIndex, basePosition, baseOrientation)) { + bodyUniqueId, basePosition, baseOrientation,sm)) { PyErr_SetString(SpamError, "GetBasePositionAndOrientation failed."); return NULL; @@ -1184,25 +1413,29 @@ static PyObject* pybullet_getBasePositionAndOrientation(PyObject* self, static PyObject* pybullet_getBaseVelocity(PyObject* self, - PyObject* args) { - int bodyIndex = -1; + PyObject* args, PyObject* keywds) { + int bodyUniqueId = -1; double baseLinearVelocity[3]; double baseAngularVelocity[3]; PyObject* pylistLinVel=0; PyObject* pylistAngVel=0; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; - if (0 == sm) { + static char *kwlist[] = { "bodyUniqueId", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,&bodyUniqueId, &physicsClientId)) + { + return NULL; + } + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - - if (!PyArg_ParseTuple(args, "i", &bodyIndex)) { - PyErr_SetString(SpamError, "Expected a body index (integer)."); - return NULL; - } - + sm = sPhysicsClients[physicsClientId]; + if (0 == pybullet_internalGetBaseVelocity( - bodyIndex, baseLinearVelocity, baseAngularVelocity)) { + bodyUniqueId, baseLinearVelocity, baseAngularVelocity,sm)) { PyErr_SetString(SpamError, "getBaseVelocity failed."); return NULL; @@ -1238,12 +1471,23 @@ static PyObject* pybullet_getBaseVelocity(PyObject* self, return pylist; } } -static PyObject* pybullet_getNumBodies(PyObject* self, PyObject* args) +static PyObject* pybullet_getNumBodies(PyObject* self, PyObject* args, PyObject* keywds) { - if (0 == sm) { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + + static char *kwlist[] = { "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist,&physicsClientId)) + { + return NULL; + } + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + sm = sPhysicsClients[physicsClientId]; + { int numBodies = b3GetNumBodies(sm); @@ -1256,20 +1500,28 @@ static PyObject* pybullet_getNumBodies(PyObject* self, PyObject* args) } } -static PyObject* pybullet_getBodyUniqueId(PyObject* self, PyObject* args) +static PyObject* pybullet_getBodyUniqueId(PyObject* self, PyObject* args, PyObject* keywds) { - if (0 == sm) { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } +int physicsClientId = 0; +int serialIndex=-1; +b3PhysicsClientHandle sm = 0; + + static char *kwlist[] = { "serialIndex", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,&serialIndex, &physicsClientId)) + { + return NULL; + } + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + sm = sPhysicsClients[physicsClientId]; + { - int serialIndex = -1; + int bodyUniqueId = -1; - if (!PyArg_ParseTuple(args, "i", &serialIndex)) { - PyErr_SetString(SpamError, "Expected a serialIndex in range [0..number of bodies)."); - return NULL; - } bodyUniqueId = b3GetBodyUniqueId(sm, serialIndex); #if PY_MAJOR_VERSION >= 3 @@ -1280,35 +1532,41 @@ static PyObject* pybullet_getBodyUniqueId(PyObject* self, PyObject* args) } } -static PyObject* pybullet_getBodyInfo(PyObject* self, PyObject* args) +static PyObject* pybullet_getBodyInfo(PyObject* self, PyObject* args, PyObject* keywds) { - if (0 == sm) { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - + { int bodyUniqueId= -1; int numJoints = 0; - if (!PyArg_ParseTuple(args, "i", &bodyUniqueId)) + b3PhysicsClientHandle sm = 0; + + int physicsClientId = 0; + static char *kwlist[] = { "bodyUniqueId", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,&bodyUniqueId, &physicsClientId)) { - PyErr_SetString(SpamError, "Expected a body unique id (integer)."); - return NULL; - } + return NULL; + } + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + sm = sPhysicsClients[physicsClientId]; + + { + struct b3BodyInfo info; + if (b3GetBodyInfo(sm,bodyUniqueId,&info)) { - struct b3BodyInfo info; - if (b3GetBodyInfo(sm,bodyUniqueId,&info)) - { - PyObject* pyListJointInfo = PyTuple_New(1); - PyTuple_SetItem(pyListJointInfo, 0, PyString_FromString(info.m_baseName)); - return pyListJointInfo; - } else - { - PyErr_SetString(SpamError, "Couldn't get body info"); - return NULL; - } + PyObject* pyListJointInfo = PyTuple_New(1); + PyTuple_SetItem(pyListJointInfo, 0, PyString_FromString(info.m_baseName)); + return pyListJointInfo; + } else + { + PyErr_SetString(SpamError, "Couldn't get body info"); + return NULL; } } + } PyErr_SetString(SpamError, "error in getBodyInfo."); return NULL; @@ -1318,57 +1576,69 @@ static PyObject* pybullet_getBodyInfo(PyObject* self, PyObject* args) // Return the number of joints in an object based on // body index; body index is based on order of sequence // the object is loaded into simulation -static PyObject* pybullet_getNumJoints(PyObject* self, PyObject* args) +static PyObject* pybullet_getNumJoints(PyObject* self, PyObject* args, PyObject* keywds) { - if (0 == sm) { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - { - int bodyIndex = -1; + int bodyUniqueId = -1; int numJoints = 0; - if (!PyArg_ParseTuple(args, "i", &bodyIndex)) { - PyErr_SetString(SpamError, "Expected a body index (integer)."); - return NULL; - } - numJoints = b3GetNumJoints(sm, bodyIndex); + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + static char *kwlist[] = { "bodyUniqueId", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,&bodyUniqueId, &physicsClientId)) + { + return NULL; + } + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + sm = sPhysicsClients[physicsClientId]; + + numJoints = b3GetNumJoints(sm, bodyUniqueId); #if PY_MAJOR_VERSION >= 3 return PyLong_FromLong(numJoints); #else return PyInt_FromLong(numJoints); #endif - } + } // Initalize all joint positions given a list of values -static PyObject* pybullet_resetJointState(PyObject* self, PyObject* args) { - int size; - if (0 == sm) { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - size = PySequence_Size(args); - - if (size == 3) { - int bodyIndex; +static PyObject* pybullet_resetJointState(PyObject* self, PyObject* args, PyObject* keywds) { + +{ + int bodyUniqueId; int jointIndex; double targetValue; + b3PhysicsClientHandle sm = 0; - if (PyArg_ParseTuple(args, "iid", &bodyIndex, &jointIndex, &targetValue)) { + int physicsClientId = 0; + static char *kwlist[] = { "bodyUniqueId", "jointIndex", "targetValue","physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iid|i", kwlist,&bodyUniqueId, &jointIndex, &targetValue, &physicsClientId)) + { + return NULL; + } + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + sm = sPhysicsClients[physicsClientId]; + + + { b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryStatusHandle statusHandle; int numJoints; - numJoints = b3GetNumJoints(sm, bodyIndex); + numJoints = b3GetNumJoints(sm, bodyUniqueId); if ((jointIndex >= numJoints) || (jointIndex < 0)) { PyErr_SetString(SpamError, "Joint index out-of-range."); return NULL; } - commandHandle = b3CreatePoseCommandInit(sm, bodyIndex); + commandHandle = b3CreatePoseCommandInit(sm, bodyUniqueId); b3CreatePoseCommandSetJointPosition(sm, commandHandle, jointIndex, targetValue); @@ -1387,25 +1657,31 @@ static PyObject* pybullet_resetJointState(PyObject* self, PyObject* args) { static PyObject* pybullet_resetBaseVelocity(PyObject* self, PyObject* args, PyObject *keywds) { - static char *kwlist[] = { "objectUniqueId", "linearVelocity", "angularVelocity", NULL }; - - if (0 == sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } + static char *kwlist[] = { "objectUniqueId", "linearVelocity", "angularVelocity","physicsClientId", NULL }; + { int bodyIndex=0; PyObject* linVelObj=0; PyObject* angVelObj=0; double linVel[3] = { 0, 0, 0 }; double angVel[3] = { 0, 0, 0 }; - - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OO", kwlist, &bodyIndex, &linVelObj, &angVelObj)) + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OOi", kwlist, &bodyIndex, &linVelObj, &angVelObj,&physicsClientId)) { return NULL; } + + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + sm = sPhysicsClients[physicsClientId]; + + if (linVelObj || angVelObj) { @@ -1445,23 +1721,30 @@ static PyObject* pybullet_resetBaseVelocity(PyObject* self, PyObject* args, PyOb // Reset the position and orientation of the base/root link, position [x,y,z] // and orientation quaternion [x,y,z,w] static PyObject* pybullet_resetBasePositionAndOrientation(PyObject* self, - PyObject* args) { - int size; - if (0 == sm) { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } + PyObject* args, PyObject* keywds) { - size = PySequence_Size(args); - - if (size == 3) { - int bodyIndex; + { + int bodyUniqueId; PyObject* posObj; PyObject* ornObj; double pos[3]; double orn[4]; // as a quaternion + b3PhysicsClientHandle sm = 0; - if (PyArg_ParseTuple(args, "iOO", &bodyIndex, &posObj, &ornObj)) { + int physicsClientId = 0; + static char *kwlist[] = { "bodyUniqueId", "posObj", "ornObj", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOO|i", kwlist,&bodyUniqueId, &posObj, &ornObj, &physicsClientId)) + { + return NULL; + } + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + sm = sPhysicsClients[physicsClientId]; + + { b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryStatusHandle statusHandle; @@ -1501,7 +1784,7 @@ static PyObject* pybullet_resetBasePositionAndOrientation(PyObject* self, Py_DECREF(seq); } - commandHandle = b3CreatePoseCommandInit(sm, bodyIndex); + commandHandle = b3CreatePoseCommandInit(sm, bodyUniqueId); b3CreatePoseCommandSetBasePosition(commandHandle, pos[0], pos[1], pos[2]); b3CreatePoseCommandSetBaseOrientation(commandHandle, orn[0], orn[1], @@ -1530,30 +1813,36 @@ static PyObject* pybullet_resetBasePositionAndOrientation(PyObject* self, // [int, str, int, int, int, int, float, float] // // TODO(hellojas): get joint positions for a body -static PyObject* pybullet_getJointInfo(PyObject* self, PyObject* args) { +static PyObject* pybullet_getJointInfo(PyObject* self, PyObject* args,PyObject* keywds) { PyObject* pyListJointInfo; struct b3JointInfo info; - int bodyIndex = -1; + int bodyUniqueId = -1; int jointIndex = -1; int jointInfoSize = 8; // size of struct b3JointInfo +b3PhysicsClientHandle sm = 0; +int physicsClientId = 0; + static char *kwlist[] = { "bodyUniqueId", "jointIndex", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist,&bodyUniqueId, &jointIndex, &physicsClientId)) + { + return NULL; + } + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + sm = sPhysicsClients[physicsClientId]; + - int size = PySequence_Size(args); - - if (0 == sm) { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - if (size == 2) // get body index and joint index { - if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex)) { + { // printf("body index = %d, joint index =%d\n", bodyIndex, jointIndex); pyListJointInfo = PyTuple_New(jointInfoSize); - if (b3GetJointInfo(sm, bodyIndex, jointIndex, &info)) { + if (b3GetJointInfo(sm, bodyUniqueId, jointIndex, &info)) { // printf("Joint%d %s, type %d, at q-index %d and u-index %d\n", // info.m_jointIndex, // info.m_jointName, @@ -1600,35 +1889,43 @@ static PyObject* pybullet_getJointInfo(PyObject* self, PyObject* args) { // TODO(hellojas): check accuracy of position and velocity // TODO(hellojas): check force torque values -static PyObject* pybullet_getJointState(PyObject* self, PyObject* args) { +static PyObject* pybullet_getJointState(PyObject* self, PyObject* args,PyObject* keywds) { PyObject* pyListJointForceTorque; PyObject* pyListJointState; struct b3JointSensorState sensorState; - int bodyIndex = -1; + int bodyUniqueId = -1; int jointIndex = -1; int sensorStateSize = 4; // size of struct b3JointSensorState int forceTorqueSize = 6; // size of force torque list from b3JointSensorState int j; - int size = PySequence_Size(args); + b3PhysicsClientHandle sm = 0; + int physicsClientId = 0; + static char *kwlist[] = { "bodyUniqueId", "jointIndex","physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist,&bodyUniqueId, &jointIndex, &physicsClientId)) + { + return NULL; + } + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + sm = sPhysicsClients[physicsClientId]; + - if (0 == sm) { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - if (size == 2) // get body index and joint index { - if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex)) { + + { int status_type = 0; b3SharedMemoryCommandHandle cmd_handle; b3SharedMemoryStatusHandle status_handle; - if (bodyIndex < 0) { + if (bodyUniqueId < 0) { PyErr_SetString(SpamError, "getJointState failed; invalid bodyIndex"); return NULL; } @@ -1639,7 +1936,7 @@ static PyObject* pybullet_getJointState(PyObject* self, PyObject* args) { cmd_handle = - b3RequestActualStateCommandInit(sm, bodyIndex); + b3RequestActualStateCommandInit(sm, bodyUniqueId); status_handle = b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); @@ -1671,18 +1968,13 @@ static PyObject* pybullet_getJointState(PyObject* self, PyObject* args) { return pyListJointState; } - } else { - PyErr_SetString( - SpamError, - "getJointState expects 2 arguments (objectUniqueId and joint index)."); - return NULL; - } + } Py_INCREF(Py_None); return Py_None; } -static PyObject* pybullet_getLinkState(PyObject* self, PyObject* args) { +static PyObject* pybullet_getLinkState(PyObject* self, PyObject* args,PyObject* keywds) { PyObject* pyLinkState; PyObject* pyLinkStateWorldPosition; PyObject* pyLinkStateWorldOrientation; @@ -1691,23 +1983,31 @@ static PyObject* pybullet_getLinkState(PyObject* self, PyObject* args) { struct b3LinkState linkState; - int bodyIndex = -1; + int bodyUniqueId = -1; int linkIndex = -1; int i; +b3PhysicsClientHandle sm = 0; - if (0 == sm) { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - if (PySequence_Size(args) == 2) // body index and link index - { - if (PyArg_ParseTuple(args, "ii", &bodyIndex, &linkIndex)) { + int physicsClientId = 0; + static char *kwlist[] = { "bodyUniqueId", "linkIndex", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist, &bodyUniqueId, &linkIndex, &physicsClientId)) + { + return NULL; + } + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + sm = sPhysicsClients[physicsClientId]; + + { + { int status_type = 0; b3SharedMemoryCommandHandle cmd_handle; b3SharedMemoryStatusHandle status_handle; - if (bodyIndex < 0) { + if (bodyUniqueId < 0) { PyErr_SetString(SpamError, "getLinkState failed; invalid bodyIndex"); return NULL; } @@ -1718,7 +2018,7 @@ static PyObject* pybullet_getLinkState(PyObject* self, PyObject* args) { cmd_handle = - b3RequestActualStateCommandInit(sm, bodyIndex); + b3RequestActualStateCommandInit(sm, bodyUniqueId); status_handle = b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); @@ -1762,11 +2062,6 @@ static PyObject* pybullet_getLinkState(PyObject* self, PyObject* args) { return pyLinkState; } - } else { - PyErr_SetString( - SpamError, - "getLinkState expects 2 arguments (objectUniqueId and link index)."); - return NULL; } Py_INCREF(Py_None); @@ -1793,18 +2088,22 @@ static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObj PyObject* textColorRGBObj=0; double textSize = 1.f; double lifeTime = 0.f; - - static char *kwlist[] = { "text", "textPosition", "textColorRGB", "textSize", "lifeTime", NULL }; + int physicsClientId = 0; + b3PhysicsClientHandle sm=0; + static char *kwlist[] = { "text", "textPosition", "textColorRGB", "textSize", "lifeTime","physicsClientId", NULL }; - if (0 == sm) { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - if (!PyArg_ParseTupleAndKeywords(args, keywds, "sO|Odd", kwlist, &text, &textPositionObj, &textColorRGBObj,&textSize, &lifeTime)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "sO|Oddi", kwlist, &text, &textPositionObj, &textColorRGBObj,&textSize, &lifeTime,&physicsClientId)) { return NULL; } + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + sm = sPhysicsClients[physicsClientId]; + res = pybullet_internalSetVectord(textPositionObj,posXYZ); if (!res) @@ -1860,18 +2159,22 @@ static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObj PyObject* lineColorRGBObj=0; double lineWidth = 1.f; double lifeTime = 0.f; - - static char *kwlist[] = { "lineFromXYZ", "lineToXYZ", "lineColorRGB", "lineWidth", "lifeTime", NULL }; + int physicsClientId = 0; + b3PhysicsClientHandle sm=0; + static char *kwlist[] = { "lineFromXYZ", "lineToXYZ", "lineColorRGB", "lineWidth", "lifeTime","physicsClientId", NULL }; - if (0 == sm) { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|Odd", kwlist, &lineFromObj, &lineToObj, &lineColorRGBObj,&lineWidth, &lifeTime)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|Oddi", kwlist, &lineFromObj, &lineToObj, &lineColorRGBObj,&lineWidth, &lifeTime,&physicsClientId)) { return NULL; } + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + sm = sPhysicsClients[physicsClientId]; + res = pybullet_internalSetVectord(lineFromObj,fromXYZ); if (!res) @@ -1915,16 +2218,23 @@ static PyObject* pybullet_removeUserDebugItem(PyObject* self, PyObject* args, Py b3SharedMemoryStatusHandle statusHandle; int statusType; int itemUniqueId; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; - if (0 == sm) { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } + static char *kwlist[] = { "itemUniqueId", "physicsClientId", NULL }; + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,&itemUniqueId, &physicsClientId)) + { + return NULL; + } + + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + sm = sPhysicsClients[physicsClientId]; - if (!PyArg_ParseTuple(args, "i", &itemUniqueId)) { - PyErr_SetString(SpamError, "Error parsing user debug item unique id"); - return NULL; - } commandHandle = b3InitUserDebugDrawRemove(sm,itemUniqueId); @@ -1940,11 +2250,21 @@ static PyObject* pybullet_removeAllUserDebugItems(PyObject* self, PyObject* args b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryStatusHandle statusHandle; int statusType; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + static char *kwlist[] = { "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist,&physicsClientId)) + { + return NULL; + } + + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + sm = sPhysicsClients[physicsClientId]; - if (0 == sm) { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } commandHandle = b3InitUserDebugDrawRemoveAll(sm); @@ -1966,17 +2286,21 @@ static PyObject* pybullet_rayTest(PyObject* self, PyObject* args, PyObject *keyw PyObject* rayToObj=0; double from[3]; double to[3]; - static char *kwlist[] = { "rayFromPosition", "rayToPosition", NULL }; - - if (0 == sm) { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO", kwlist, - &rayFromObj, &rayToObj)) + b3PhysicsClientHandle sm = 0; + static char *kwlist[] = { "rayFromPosition", "rayToPosition", "physicsClientId", NULL }; + int physicsClientId = 0; + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|i", kwlist, + &rayFromObj, &rayToObj,&physicsClientId)) return NULL; + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + sm = sPhysicsClients[physicsClientId]; + pybullet_internalSetVectord(rayFromObj,from); pybullet_internalSetVectord(rayToObj,to); @@ -2075,11 +2399,22 @@ static PyObject* pybullet_getVREvents(PyObject* self, PyObject* args, PyObject * b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryStatusHandle statusHandle; int statusType; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + static char *kwlist[] = { "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist,&physicsClientId)) + { + return NULL; + } + + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + sm = sPhysicsClients[physicsClientId]; + - if (0 == sm) { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } commandHandle = b3RequestVREventsCommandInit(sm); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); @@ -2145,17 +2480,20 @@ static PyObject* pybullet_setDebugObjectColor(PyObject* self, PyObject* args, Py int objectUniqueId = -1; int linkIndex = -2; + int physicsClientId = 0; + b3PhysicsClientHandle sm=0; + static char *kwlist[] = { "objectUniqueId", "linkIndex","objectDebugColorRGB", "physicsClientId", NULL }; - static char *kwlist[] = { "objectUniqueId", "linkIndex","objectDebugColorRGB", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|Oi", kwlist, + &objectUniqueId, &linkIndex, &objectColorRGBObj,&physicsClientId)) + return NULL; - if (0 == sm) { + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - - if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|O", kwlist, - &objectUniqueId, &linkIndex, &objectColorRGBObj)) - return NULL; + sm = sPhysicsClients[physicsClientId]; if (objectColorRGBObj) { @@ -2177,9 +2515,8 @@ static PyObject* pybullet_setDebugObjectColor(PyObject* self, PyObject* args, Py } -static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args) +static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyObject* keywds) { - int size = PySequence_Size(args); int objectUniqueId = -1; b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryStatusHandle statusHandle; @@ -2187,17 +2524,23 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args) int statusType; int i; PyObject* pyResultList = 0; - - if (0 == sm) { + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + static char *kwlist[] = { "objectUniqueId", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,&objectUniqueId,&physicsClientId)) + { + return NULL; + } + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - if (size == 1) + sm = sPhysicsClients[physicsClientId]; + + { - if (!PyArg_ParseTuple(args, "i", &objectUniqueId)) { - PyErr_SetString(SpamError, "Error parsing object unique id"); - return NULL; - } + commandHandle = b3InitRequestVisualShapeInformation(sm, objectUniqueId); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); @@ -2269,19 +2612,14 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args) return NULL; } } - else - { - PyErr_SetString(SpamError, "getVisualShapeData requires 1 argument (object unique id)"); - return NULL; - } Py_INCREF(Py_None); return Py_None; } -static PyObject* pybullet_resetVisualShapeData(PyObject* self, PyObject* args) +static PyObject* pybullet_resetVisualShapeData(PyObject* self, PyObject* args,PyObject* keywds) { - int size = PySequence_Size(args); + int objectUniqueId = -1; int jointIndex = -1; int shapeIndex = -1; @@ -2289,18 +2627,23 @@ static PyObject* pybullet_resetVisualShapeData(PyObject* self, PyObject* args) b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryStatusHandle statusHandle; int statusType; - - if (0 == sm) { + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + static char *kwlist[] = { "objectUniqueId", "jointIndex", "shapeIndex", "textureUniqueId", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiii|i", kwlist, &objectUniqueId, &jointIndex, &shapeIndex, &textureUniqueId, &physicsClientId)) + { + return NULL; + } + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } + sm = sPhysicsClients[physicsClientId]; + - if (size == 4) + { - if (!PyArg_ParseTuple(args, "iiii", &objectUniqueId, &jointIndex, &shapeIndex, &textureUniqueId)) { - PyErr_SetString(SpamError, "Error parsing object unique id, or joint index, or shape index, or texture unique id"); - return NULL; - } commandHandle = b3InitUpdateVisualShape(sm, objectUniqueId, jointIndex, shapeIndex, textureUniqueId); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); @@ -2314,17 +2657,12 @@ static PyObject* pybullet_resetVisualShapeData(PyObject* self, PyObject* args) return NULL; } } - else - { - PyErr_SetString(SpamError, "setVisualShapeData requires 4 argument"); - return NULL; - } Py_INCREF(Py_None); return Py_None; } -static PyObject* pybullet_loadTexture(PyObject* self, PyObject* args) +static PyObject* pybullet_loadTexture(PyObject* self, PyObject* args, PyObject* keywds) { int size = PySequence_Size(args); const char* filename = 0; @@ -2332,17 +2670,21 @@ static PyObject* pybullet_loadTexture(PyObject* self, PyObject* args) b3SharedMemoryStatusHandle statusHandle; int statusType; - if (0 == sm) { + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + static char *kwlist[] = { "textureFilename", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist,&filename, &physicsClientId)) + { + return NULL; + } + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - - if (size == 1) + sm = sPhysicsClients[physicsClientId]; + { - if (!PyArg_ParseTuple(args, "s", &filename)) { - PyErr_SetString(SpamError, "Error parsing file name"); - return NULL; - } commandHandle = b3InitLoadTexture(sm, filename); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); @@ -2356,11 +2698,6 @@ static PyObject* pybullet_loadTexture(PyObject* self, PyObject* args) return NULL; } } - else - { - PyErr_SetString(SpamError, "loadTexture requires 1 argument"); - return NULL; - } Py_INCREF(Py_None); return Py_None; @@ -2477,16 +2814,22 @@ static PyObject* pybullet_getOverlappingObjects(PyObject* self, PyObject* args, b3SharedMemoryStatusHandle statusHandle; struct b3AABBOverlapData overlapData; int i; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + static char *kwlist[] = { "aabbMin", "aabbMax", "physicsClientId",NULL }; + - static char *kwlist[] = { "aabbMin", "aabbMax", NULL }; - if (0 == sm) { + if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|i", kwlist, + &aabbMinOb, &aabbMaxOb,&physicsClientId)) + return NULL; + + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } + sm = sPhysicsClients[physicsClientId]; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO", kwlist, - &aabbMinOb, &aabbMaxOb)) - return NULL; pybullet_internalSetVectord(aabbMinOb, aabbMin); pybullet_internalSetVectord(aabbMaxOb, aabbMax); @@ -2537,19 +2880,22 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py b3SharedMemoryStatusHandle statusHandle; int statusType; PyObject* pyResultList = 0; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + static char *kwlist[] = { "bodyA", "bodyB", "distance", "linkIndexA","linkIndexB","physicsClientId", NULL }; + - static char *kwlist[] = { "bodyA", "bodyB", "distance", "linkIndexA","linkIndexB",NULL }; - - if (0 == sm) { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - if (!PyArg_ParseTupleAndKeywords(args, keywds, "iid|ii", kwlist, - &bodyUniqueIdA, &bodyUniqueIdB, &distanceThreshold, &linkIndexA, &linkIndexB)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iid|iii", kwlist, + &bodyUniqueIdA, &bodyUniqueIdB, &distanceThreshold, &linkIndexA, &linkIndexB,&physicsClientId)) return NULL; + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + sm = sPhysicsClients[physicsClientId]; commandHandle = b3InitClosestDistanceQuery(sm); b3SetClosestDistanceFilterBodyA(commandHandle, bodyUniqueIdA); @@ -2581,25 +2927,26 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py static PyObject* pybullet_removeUserConstraint(PyObject* self, PyObject* args, PyObject *keywds) { - static char *kwlist[] = { "userConstraintUniqueId",NULL}; + static char *kwlist[] = { "userConstraintUniqueId","physicsClientId", NULL}; int userConstraintUniqueId=-1; b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryStatusHandle statusHandle; int statusType; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,&userConstraintUniqueId,&physicsClientId)) + { + return NULL; + } - - if (0 == sm) + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } + sm = sPhysicsClients[physicsClientId]; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i", kwlist,&userConstraintUniqueId)) - { - return NULL; - } - commandHandle = b3InitRemoveUserConstraintCommand(sm,userConstraintUniqueId); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); @@ -2642,8 +2989,8 @@ static PyObject* pybullet_createUserConstraint(PyObject* self, PyObject* args, P b3SharedMemoryStatusHandle statusHandle; int statusType; PyObject* pyResultList = 0; - - + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; static char *kwlist[] = { "parentBodyUniqueId", "parentLinkIndex", "childBodyUniqueId", "childLinkIndex", "jointType", @@ -2651,27 +2998,33 @@ static PyObject* pybullet_createUserConstraint(PyObject* self, PyObject* args, P "parentFramePosition", "childFramePosition", "parentFrameOrientation", - "childFrameOrientation", - NULL }; + "childFrameOrientation", + "physicsClientId", + NULL }; - if (0 == sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } + - if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiiiiOOO|OO", kwlist,&parentBodyUniqueId,&parentLinkIndex, + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiiiiOOO|OOi", kwlist,&parentBodyUniqueId,&parentLinkIndex, &childBodyUniqueId,&childLinkIndex, &jointType,&jointAxisObj, &parentFramePositionObj, &childFramePositionObj, &parentFrameOrientationObj, - &childFrameOrientationObj + &childFrameOrientationObj, + &physicsClientId )) { return NULL; } + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + sm = sPhysicsClients[physicsClientId]; + + pybullet_internalSetVectord(jointAxisObj,jointAxis); pybullet_internalSetVectord(parentFramePositionObj,parentFramePosition); pybullet_internalSetVectord(childFramePositionObj,childFramePosition); @@ -2730,16 +3083,21 @@ static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args, Py PyObject* pyResultList = 0; - static char *kwlist[] = { "bodyA", "bodyB", NULL }; + static char *kwlist[] = { "bodyA", "bodyB","physicsClientId", NULL }; - if (0 == sm) { - PyErr_SetString(SpamError, "Not connected to physics server."); + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|iii", kwlist, + &bodyUniqueIdA, &bodyUniqueIdB,&physicsClientId)) return NULL; - } - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|ii", kwlist, - &bodyUniqueIdA, &bodyUniqueIdB)) - return NULL; + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + sm = sPhysicsClients[physicsClientId]; + commandHandle = b3InitRequestContactPointInformation(sm); @@ -2783,22 +3141,23 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec float lightSpecularCoeff = 0.05; // inialize cmd b3SharedMemoryCommandHandle command; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + // set camera resolution, optionally view, projection matrix, light direction, light color, light distance, shadow + static char *kwlist[] = { "width", "height", "viewMatrix", "projectionMatrix", "lightDirection", "lightColor", "lightDistance", "shadow", "lightAmbientCoeff", "lightDiffuseCoeff", "lightSpecularCoeff", "physicsClientId", NULL }; - if (0 == sm) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|OOOOfifffi", kwlist, &width, &height, &objViewMat, &objProjMat, &lightDirObj, &lightColorObj, &lightDist, &hasShadow, &lightAmbientCoeff, &lightDiffuseCoeff, &lightSpecularCoeff,&physicsClientId)) + { + return NULL; + } + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } + sm = sPhysicsClients[physicsClientId]; command = b3InitRequestCameraImage(sm); - - // set camera resolution, optionally view, projection matrix, light direction, light color, light distance, shadow - static char *kwlist[] = { "width", "height", "viewMatrix", "projectionMatrix", "lightDirection", "lightColor", "lightDistance", "shadow", "lightAmbientCoeff", "lightDiffuseCoeff", "lightSpecularCoeff", NULL }; - - if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|OOOOfifff", kwlist, &width, &height, &objViewMat, &objProjMat, &lightDirObj, &lightColorObj, &lightDist, &hasShadow, &lightAmbientCoeff, &lightDiffuseCoeff, &lightSpecularCoeff)) - { - return NULL; - } b3RequestCameraImageSetPixelResolution(command, width, height); // set camera matrices only if set matrix function succeeds @@ -3119,6 +3478,15 @@ static PyObject* pybullet_renderImageObsolete(PyObject* self, PyObject* args) { // inialize cmd b3SharedMemoryCommandHandle command; + int physicsClientId = 0; + b3PhysicsClientHandle sm=0; + if (0 == sPhysicsClients[physicsClientId]) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + sm = sPhysicsClients[physicsClientId]; + if (0 == sm) { PyErr_SetString(SpamError, "Not connected to physics server."); @@ -3336,11 +3704,8 @@ static PyObject* pybullet_renderImageObsolete(PyObject* self, PyObject* args) { return Py_None; } -static PyObject* pybullet_applyExternalForce(PyObject* self, PyObject* args) { - if (0 == sm) { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } +static PyObject* pybullet_applyExternalForce(PyObject* self, PyObject* args,PyObject* keywds) { + { int objectUniqueId, linkIndex, flags; double force[3]; @@ -3350,22 +3715,22 @@ static PyObject* pybullet_applyExternalForce(PyObject* self, PyObject* args) { b3SharedMemoryCommandHandle command; b3SharedMemoryStatusHandle statusHandle; int size = PySequence_Size(args); + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + static char *kwlist[] = { "objectUniqueId", "linkIndex", + "forceObj", "posObj", "flags", "physicsClientId", NULL }; - if (size == 5) { - if (!PyArg_ParseTuple(args, "iiOOi", &objectUniqueId, &linkIndex, - &forceObj, &posObj, &flags)) { - PyErr_SetString(SpamError, "applyBaseForce couldn't parse arguments"); - return NULL; - } - } else { - PyErr_SetString(SpamError, - "applyBaseForce needs 5 arguments: objectUniqueId, " - "linkIndex (-1 for base/root link), force [x,y,z], " - "position [x,y,z], flags"); - - return NULL; - } - + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiOOi|i", kwlist,&objectUniqueId, &linkIndex, + &forceObj, &posObj, &flags, &physicsClientId)) + { + return NULL; + } + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + sm = sPhysicsClients[physicsClientId]; { PyObject* seq; int len, i; @@ -3412,18 +3777,30 @@ static PyObject* pybullet_applyExternalForce(PyObject* self, PyObject* args) { return Py_None; } -static PyObject* pybullet_applyExternalTorque(PyObject* self, PyObject* args) { - if (0 == sm) { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } +static PyObject* pybullet_applyExternalTorque(PyObject* self, PyObject* args,PyObject* keywds) { + { int objectUniqueId, linkIndex, flags; double torque[3]; PyObject* torqueObj; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + static char *kwlist[] = { "objectUniqueId", "linkIndex", "torqueObj", + "flags", "physicsClientId", NULL }; - if (PyArg_ParseTuple(args, "iiOi", &objectUniqueId, &linkIndex, &torqueObj, - &flags)) { + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiOi|i", kwlist,&objectUniqueId, &linkIndex, &torqueObj, + &flags, &physicsClientId)) + { + return NULL; + } + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + sm = sPhysicsClients[physicsClientId]; + + { PyObject* seq; int len, i; seq = PySequence_Fast(torqueObj, "expected a sequence"); @@ -3589,81 +3966,80 @@ static PyObject* pybullet_getEulerFromQuaternion(PyObject* self, ///Inverse Kinematics binding static PyObject* pybullet_calculateInverseKinematics(PyObject* self, - PyObject* args) { - int size; - if (0 == sm) { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - - size = PySequence_Size(args); - if (size == 2) - { + PyObject* args, PyObject* keywds) + + { int bodyIndex; int endEffectorLinkIndex; PyObject* targetPosObj; PyObject* targetOrnObj; - if (PyArg_ParseTuple(args, "iiOO", &bodyIndex, &endEffectorLinkIndex, &targetPosObj,&targetOrnObj)) - { - double pos[3]; - double ori[4]={0,1.0,0,0}; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + static char *kwlist[] = { "bodyIndex", "endEffectorLinkIndex", "targetPosition", "targetOrientation","physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiOO|i", kwlist, &bodyIndex, &endEffectorLinkIndex, &targetPosObj,&targetOrnObj,&physicsClientId)) + { + return NULL; + } + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + sm = sPhysicsClients[physicsClientId]; + { + double pos[3]; + double ori[4]={0,1.0,0,0}; - if (pybullet_internalSetVectord(targetPosObj,pos) && pybullet_internalSetVector4d(targetOrnObj,ori)) - { - b3SharedMemoryStatusHandle statusHandle; - int numPos=0; - int resultBodyIndex; - int result; + if (pybullet_internalSetVectord(targetPosObj,pos) && pybullet_internalSetVector4d(targetOrnObj,ori)) + { + b3SharedMemoryStatusHandle statusHandle; + int numPos=0; + int resultBodyIndex; + int result; - b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm,bodyIndex); - b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command,6,pos,ori); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm,bodyIndex); + b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command,6,pos,ori); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); - result = b3GetStatusInverseKinematicsJointPositions(statusHandle, - &resultBodyIndex, - &numPos, - 0); - if (result && numPos) - { - int i; - PyObject* pylist; - double* ikOutPutJointPos = (double*)malloc(numPos*sizeof(double)); - result = b3GetStatusInverseKinematicsJointPositions(statusHandle, - &resultBodyIndex, - &numPos, - ikOutPutJointPos); - pylist = PyTuple_New(numPos); - for (i = 0; i < numPos; i++) - { - PyTuple_SetItem(pylist, i, - PyFloat_FromDouble(ikOutPutJointPos[i])); - } + result = b3GetStatusInverseKinematicsJointPositions(statusHandle, + &resultBodyIndex, + &numPos, + 0); + if (result && numPos) + { + int i; + PyObject* pylist; + double* ikOutPutJointPos = (double*)malloc(numPos*sizeof(double)); + result = b3GetStatusInverseKinematicsJointPositions(statusHandle, + &resultBodyIndex, + &numPos, + ikOutPutJointPos); + pylist = PyTuple_New(numPos); + for (i = 0; i < numPos; i++) + { + PyTuple_SetItem(pylist, i, + PyFloat_FromDouble(ikOutPutJointPos[i])); + } - free(ikOutPutJointPos); - return pylist; - } - else - { - PyErr_SetString(SpamError, - "Error in calculateInverseKinematics"); - return NULL; - } - } else - { - PyErr_SetString(SpamError, - "calculateInverseKinematics couldn't extract position vector3"); - return NULL; - } - } - } else - { - PyErr_SetString(SpamError, - "calculateInverseKinematics expects 2 arguments, body index, " - "and target position for end effector"); - return NULL; - } + free(ikOutPutJointPos); + return pylist; + } + else + { + PyErr_SetString(SpamError, + "Error in calculateInverseKinematics"); + return NULL; + } + } else + { + PyErr_SetString(SpamError, + "calculateInverseKinematics couldn't extract position vector3"); + return NULL; + } + } + Py_INCREF(Py_None); return Py_None; } @@ -3674,22 +4050,31 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, /// accelerations, /// compute the joint forces using Inverse Dynamics static PyObject* pybullet_calculateInverseDynamics(PyObject* self, - PyObject* args) { - int size; - if (0 == sm) { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } + PyObject* args,PyObject* keywds) +{ + - size = PySequence_Size(args); - if (size == 4) { + { int bodyIndex; PyObject* objPositionsQ; PyObject* objVelocitiesQdot; PyObject* objAccelerations; - - if (PyArg_ParseTuple(args, "iOOO", &bodyIndex, &objPositionsQ, - &objVelocitiesQdot, &objAccelerations)) { + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + static char *kwlist[] = { "bodyIndex", "objPositions", "objVelocities", "objAccelerations","physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOOO|i", kwlist, &bodyIndex, &objPositionsQ, + &objVelocitiesQdot, &objAccelerations,&physicsClientId)) + { + return NULL; + } + if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + sm = sPhysicsClients[physicsClientId]; + + { int szObPos = PySequence_Size(objPositionsQ); int szObVel = PySequence_Size(objVelocitiesQdot); int szObAcc = PySequence_Size(objAccelerations); @@ -3764,19 +4149,7 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, return NULL; } - } else { - PyErr_SetString(SpamError, - "calculateInverseDynamics expects 4 arguments, body " - "index, [joint positions], [joint velocities], [joint " - "accelerations]."); - return NULL; } - } else { - PyErr_SetString(SpamError, - "calculateInverseDynamics expects 4 arguments, body index, " - "[joint positions], [joint velocities], [joint " - "accelerations]."); - return NULL; } Py_INCREF(Py_None); return Py_None; @@ -3784,56 +4157,56 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, static PyMethodDef SpamMethods[] = { - {"connect", pybullet_connectPhysicsServer, METH_VARARGS, + {"connect",(PyCFunction)pybullet_connectPhysicsServer, METH_VARARGS|METH_KEYWORDS, "Connect to an existing physics server (using shared memory by default)."}, - {"disconnect", pybullet_disconnectPhysicsServer, METH_VARARGS, + {"disconnect", (PyCFunction)pybullet_disconnectPhysicsServer, METH_VARARGS|METH_KEYWORDS, "Disconnect from the physics server."}, - {"resetSimulation", pybullet_resetSimulation, METH_VARARGS, + {"resetSimulation", (PyCFunction)pybullet_resetSimulation, METH_VARARGS|METH_KEYWORDS, "Reset the simulation: remove all objects and start from an empty world."}, - {"stepSimulation", pybullet_stepSimulation, METH_VARARGS, + {"stepSimulation", (PyCFunction)pybullet_stepSimulation, METH_VARARGS|METH_KEYWORDS, "Step the simulation using forward dynamics."}, - {"setGravity", pybullet_setGravity, METH_VARARGS, + {"setGravity", (PyCFunction)pybullet_setGravity, METH_VARARGS|METH_KEYWORDS, "Set the gravity acceleration (x,y,z)."}, - {"setTimeStep", pybullet_setTimeStep, METH_VARARGS, + {"setTimeStep",(PyCFunction) pybullet_setTimeStep, METH_VARARGS|METH_KEYWORDS, "Set the amount of time to proceed at each call to stepSimulation. (unit " "is seconds, typically range is 0.01 or 0.001)"}, - {"setDefaultContactERP", pybullet_setDefaultContactERP, METH_VARARGS, + {"setDefaultContactERP", (PyCFunction) pybullet_setDefaultContactERP, METH_VARARGS| METH_KEYWORDS, "Set the amount of contact penetration Error Recovery Paramater " "(ERP) in each time step. \ This is an tuning parameter to control resting contact stability. " "This value depends on the time step."}, - { "setRealTimeSimulation", pybullet_setRealTimeSimulation, METH_VARARGS, + { "setRealTimeSimulation",(PyCFunction) pybullet_setRealTimeSimulation, METH_VARARGS| METH_KEYWORDS, "Enable or disable real time simulation (using the real time clock," " RTC) in the physics server. Expects one integer argument, 0 or 1" }, { "setPhysicsEngineParameter", (PyCFunction)pybullet_setPhysicsEngineParameter, METH_VARARGS | METH_KEYWORDS, "Set some internal physics engine parameter, such as cfm or erp etc." }, - { "setInternalSimFlags", pybullet_setInternalSimFlags, METH_VARARGS, + { "setInternalSimFlags", (PyCFunction)pybullet_setInternalSimFlags, METH_VARARGS| METH_KEYWORDS, "This is for experimental purposes, use at own risk, magic may or not happen"}, {"loadURDF", (PyCFunction) pybullet_loadURDF, METH_VARARGS | METH_KEYWORDS, "Create a multibody by loading a URDF file."}, - {"loadSDF", pybullet_loadSDF, METH_VARARGS, + {"loadSDF", (PyCFunction)pybullet_loadSDF, METH_VARARGS| METH_KEYWORDS, "Load multibodies from an SDF file."}, - { "loadBullet", pybullet_loadBullet, METH_VARARGS, + { "loadBullet", (PyCFunction)pybullet_loadBullet, METH_VARARGS| METH_KEYWORDS, "Restore the full state of the world from a .bullet file." }, - { "saveBullet", pybullet_saveBullet, METH_VARARGS, + { "saveBullet", (PyCFunction)pybullet_saveBullet, METH_VARARGS| METH_KEYWORDS, "Save the full state of the world to a .bullet file." }, - { "loadMJCF", pybullet_loadMJCF, METH_VARARGS, + { "loadMJCF",(PyCFunction) pybullet_loadMJCF, METH_VARARGS| METH_KEYWORDS, "Load multibodies from an MJCF file." }, {"createConstraint", (PyCFunction)pybullet_createUserConstraint, METH_VARARGS | METH_KEYWORDS, @@ -3844,32 +4217,32 @@ static PyMethodDef SpamMethods[] = { "Remove a constraint using its unique id." }, - {"saveWorld", pybullet_saveWorld, METH_VARARGS, + {"saveWorld", (PyCFunction)pybullet_saveWorld, METH_VARARGS| METH_KEYWORDS, "Save a approximate Python file to reproduce the current state of the world: saveWorld" "(filename). (very preliminary and approximately)"}, - {"getNumBodies", pybullet_getNumBodies, METH_VARARGS, + {"getNumBodies", (PyCFunction)pybullet_getNumBodies, METH_VARARGS| METH_KEYWORDS, "Get the number of bodies in the simulation."}, - {"getBodyUniqueId", pybullet_getBodyUniqueId, METH_VARARGS, + {"getBodyUniqueId", (PyCFunction)pybullet_getBodyUniqueId, METH_VARARGS| METH_KEYWORDS, "Get the unique id of the body, given a integer serial index in range [0.. number of bodies)."}, - {"getBodyInfo", pybullet_getBodyInfo, METH_VARARGS, + {"getBodyInfo",(PyCFunction) pybullet_getBodyInfo, METH_VARARGS | METH_KEYWORDS, "Get the body info, given a body unique id."}, - {"getBasePositionAndOrientation", pybullet_getBasePositionAndOrientation, - METH_VARARGS, + {"getBasePositionAndOrientation",(PyCFunction) pybullet_getBasePositionAndOrientation, + METH_VARARGS | METH_KEYWORDS, "Get the world position and orientation of the base of the object. " "(x,y,z) position vector and (x,y,z,w) quaternion orientation."}, {"resetBasePositionAndOrientation", - pybullet_resetBasePositionAndOrientation, METH_VARARGS, + (PyCFunction) pybullet_resetBasePositionAndOrientation, METH_VARARGS| METH_KEYWORDS, "Reset the world position and orientation of the base of the object " "instantaneously, not through physics simulation. (x,y,z) position vector " "and (x,y,z,w) quaternion orientation."}, - { "getBaseVelocity", pybullet_getBaseVelocity, - METH_VARARGS, + { "getBaseVelocity", (PyCFunction)pybullet_getBaseVelocity, + METH_VARARGS| METH_KEYWORDS, "Get the linear and angular velocity of the base of the object " " in world space coordinates. " "(x,y,z) linear velocity vector and (x,y,z) angular velocity vector." }, @@ -3879,36 +4252,39 @@ static PyMethodDef SpamMethods[] = { " in world space coordinates. " "linearVelocity (x,y,z) and angularVelocity (x,y,z)." }, - - - {"getNumJoints", pybullet_getNumJoints, METH_VARARGS, + {"getNumJoints", (PyCFunction)pybullet_getNumJoints, METH_VARARGS| METH_KEYWORDS, "Get the number of joints for an object."}, - {"getJointInfo", pybullet_getJointInfo, METH_VARARGS, + {"getJointInfo", (PyCFunction)pybullet_getJointInfo, METH_VARARGS| METH_KEYWORDS, "Get the name and type info for a joint on a body."}, - {"getJointState", pybullet_getJointState, METH_VARARGS, + {"getJointState",(PyCFunction) pybullet_getJointState, METH_VARARGS| METH_KEYWORDS, "Get the state (position, velocity etc) for a joint on a body."}, - {"getLinkState", pybullet_getLinkState, METH_VARARGS, + {"getLinkState", (PyCFunction)pybullet_getLinkState, METH_VARARGS| METH_KEYWORDS, "Provides extra information such as the Cartesian world coordinates" " center of mass (COM) of the link, relative to the world reference" " frame."}, - {"resetJointState", pybullet_resetJointState, METH_VARARGS, + {"resetJointState",(PyCFunction) pybullet_resetJointState, METH_VARARGS| METH_KEYWORDS, "Reset the state (position, velocity etc) for a joint on a body " "instantaneously, not through physics simulation."}, - {"setJointMotorControl", pybullet_setJointMotorControl, METH_VARARGS, + {"setJointMotorControl",(PyCFunction) pybullet_setJointMotorControl, METH_VARARGS, + "This (obsolete) method cannot select non-zero physicsClientId, use setJointMotorControl2 instead." + "Set a single joint motor control mode and desired target value. There is " + "no immediate state change, stepSimulation will process the motors."}, + + {"setJointMotorControl2",(PyCFunction) pybullet_setJointMotorControl2, METH_VARARGS| METH_KEYWORDS, "Set a single joint motor control mode and desired target value. There is " "no immediate state change, stepSimulation will process the motors."}, - {"applyExternalForce", pybullet_applyExternalForce, METH_VARARGS, + {"applyExternalForce",(PyCFunction) pybullet_applyExternalForce, METH_VARARGS, "for objectUniqueId, linkIndex (-1 for base/root link), apply a force " "[x,y,z] at the a position [x,y,z], flag to select FORCE_IN_LINK_FRAME or " "FORCE_IN_WORLD_FRAME coordinates"}, - {"applyExternalTorque", pybullet_applyExternalTorque, METH_VARARGS, + {"applyExternalTorque", (PyCFunction)pybullet_applyExternalTorque, METH_VARARGS| METH_KEYWORDS, "for objectUniqueId, linkIndex (-1 for base/root link) apply a torque " "[x,y,z] in Cartesian coordinates, flag to select TORQUE_IN_LINK_FRAME or " "TORQUE_IN_WORLD_FRAME coordinates"}, @@ -3985,13 +4361,13 @@ static PyMethodDef SpamMethods[] = { }, - {"getVisualShapeData", pybullet_getVisualShapeData, METH_VARARGS, + {"getVisualShapeData", (PyCFunction)pybullet_getVisualShapeData, METH_VARARGS| METH_KEYWORDS, "Return the visual shape information for one object." }, - {"resetVisualShapeData", pybullet_resetVisualShapeData, METH_VARARGS, + {"resetVisualShapeData", (PyCFunction)pybullet_resetVisualShapeData, METH_VARARGS| METH_KEYWORDS, "Reset part of the visual shape information for one object." }, - {"loadTexture", pybullet_loadTexture, METH_VARARGS, + {"loadTexture", (PyCFunction)pybullet_loadTexture, METH_VARARGS| METH_KEYWORDS, "Load texture file." }, {"getQuaternionFromEuler", pybullet_getQuaternionFromEuler, METH_VARARGS, @@ -4005,13 +4381,12 @@ static PyMethodDef SpamMethods[] = { {"getMatrixFromQuaterion", pybullet_getMatrixFromQuaterion,METH_VARARGS, "Compute the 3x3 matrix from a quaternion, as a list of 9 values (row-major)"}, - {"calculateInverseDynamics", pybullet_calculateInverseDynamics, - METH_VARARGS, + {"calculateInverseDynamics", (PyCFunction)pybullet_calculateInverseDynamics, METH_VARARGS| METH_KEYWORDS, "Given an object id, joint positions, joint velocities and joint " "accelerations, compute the joint forces using Inverse Dynamics"}, - {"calculateInverseKinematics", pybullet_calculateInverseKinematics, - METH_VARARGS, + {"calculateInverseKinematics", (PyCFunction)pybullet_calculateInverseKinematics, + METH_VARARGS| METH_KEYWORDS, "Inverse Kinematics bindings: Given an object id, " "current joint positions and target position" " for the end effector," From 82995a8343b4c70950168405c95b7589e6171397 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 28 Dec 2016 21:51:54 -0800 Subject: [PATCH 2/5] pybullet, more robust multi-server connections Windows shared memory: allow to use custom key. Improve GUI performance on Windows, submit letters in text as a batch (fewer draw-calls) quadruped.py: first try to connect to SHARED_MEMORY, if it fails (<0) use GUI increase Chrome about://tracing json export capacity (press 'p' in Example Browser) UDP physics server: add --port and --sharedMemoryKey command-line arguments PhysicsServerExample: add --sharedMemoryKey command-line option (for VR example too) ExampleBrowser: sleep a few milliseconds if rendering is too fast, use --minUpdateTimeMicroSecs=0 to disable --- .../InProcessExampleBrowser.cpp | 18 +- .../ExampleBrowser/OpenGLExampleBrowser.cpp | 37 +- examples/ExampleBrowser/main.cpp | 19 +- .../ImportMeshUtility/b3ImportMeshUtility.cpp | 1 - .../ImportURDFDemo/BulletUrdfImporter.cpp | 10 +- examples/OpenGLWindow/GLPrimInternalData.h | 7 +- examples/OpenGLWindow/GLPrimitiveRenderer.cpp | 217 +++++++++++- examples/OpenGLWindow/GLPrimitiveRenderer.h | 25 +- .../OpenGLWindow/GwenOpenGL3CoreRenderer.h | 28 +- .../PhysicsClientSharedMemory.cpp | 2 +- .../PhysicsServerCommandProcessor.cpp | 1 + .../SharedMemory/PhysicsServerExample.cpp | 324 ++++++++++-------- examples/SharedMemory/Win32SharedMemory.cpp | 23 +- examples/SharedMemory/main.cpp | 1 + examples/SharedMemory/udp/main.cpp | 28 +- .../StandaloneMain/hellovr_opengl_main.cpp | 8 +- .../main_opengl_single_example.cpp | 1 + examples/pybullet/pybullet.c | 292 ++++++++-------- examples/pybullet/quadruped.py | 9 +- 19 files changed, 723 insertions(+), 328 deletions(-) diff --git a/examples/ExampleBrowser/InProcessExampleBrowser.cpp b/examples/ExampleBrowser/InProcessExampleBrowser.cpp index 4924a5da9..0732d39b3 100644 --- a/examples/ExampleBrowser/InProcessExampleBrowser.cpp +++ b/examples/ExampleBrowser/InProcessExampleBrowser.cpp @@ -224,6 +224,8 @@ enum TestExampleBrowserCommunicationEnums eExampleBrowserHasTerminated }; +static double gMinUpdateTimeMicroSecs = 4000.; + void ExampleBrowserThreadFunc(void* userPtr,void* lsMemory) { printf("ExampleBrowserThreadFunc started\n"); @@ -254,8 +256,20 @@ void ExampleBrowserThreadFunc(void* userPtr,void* lsMemory) do { float deltaTimeInSeconds = clock.getTimeMicroseconds()/1000000.f; - clock.reset(); - exampleBrowser->update(deltaTimeInSeconds); + { + if (deltaTimeInSeconds > 0.1) + { + deltaTimeInSeconds = 0.1; + } + if (deltaTimeInSeconds < (gMinUpdateTimeMicroSecs/1e6)) + { + clock.usleep(gMinUpdateTimeMicroSecs/10.); + } else + { + clock.reset(); + exampleBrowser->update(deltaTimeInSeconds); + } + } } while (!exampleBrowser->requestedExit() && (args->m_cs->getSharedParam(0)!=eRequestTerminateExampleBrowser)); } else diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index ff1b9588a..b5af22122 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -163,7 +163,7 @@ FILE* gTimingFile = 0; #define __STDC_FORMAT_MACROS #endif //__STDC_FORMAT_MACROS #include -#define BT_TIMING_CAPACITY 65536 +#define BT_TIMING_CAPACITY 16*65536 static bool m_firstTiming = true; @@ -266,7 +266,10 @@ struct btTimings return; } - + if (m_timings[0].size()==0) + { + m_timings[0].resize(BT_TIMING_CAPACITY); + } int slot = m_numTimings++; @@ -279,7 +282,7 @@ struct btTimings int m_numTimings; int m_activeBuffer; - btTiming m_timings[2][BT_TIMING_CAPACITY]; + btAlignedObjectArray m_timings[1]; }; btTimings gTimings[BT_MAX_THREAD_COUNT]; @@ -729,11 +732,13 @@ void MyComboBoxCallback(int comboId, const char* item) } +//in case of multi-threading, don't submit messages while the GUI is rendering (causing crashes) +static bool gBlockGuiMessages = false; void MyGuiPrintf(const char* msg) { printf("b3Printf: %s\n",msg); - if (!gDisableDemoSelection) + if (!gDisableDemoSelection && !gBlockGuiMessages) { gui2->textOutput(msg); gui2->forceUpdateScrollBars(); @@ -745,7 +750,7 @@ void MyGuiPrintf(const char* msg) void MyStatusBarPrintf(const char* msg) { printf("b3Printf: %s\n", msg); - if (!gDisableDemoSelection) + if (!gDisableDemoSelection && !gBlockGuiMessages) { bool isLeft = true; gui2->setStatusBarMessage(msg,isLeft); @@ -756,7 +761,7 @@ void MyStatusBarPrintf(const char* msg) void MyStatusBarError(const char* msg) { printf("Warning: %s\n", msg); - if (!gDisableDemoSelection) + if (!gDisableDemoSelection && !gBlockGuiMessages) { bool isLeft = false; gui2->setStatusBarMessage(msg,isLeft); @@ -1016,7 +1021,22 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[]) b3CommandLineArgs args(argc,argv); loadCurrentSettings(startFileName, args); + if (args.CheckCmdLineFlag("nogui")) + { + renderGrid = false; + renderGui = false; + } + if (args.CheckCmdLineFlag("tracing")) + { + m_firstTiming = true; + gProfileDisabled = false;//true; + b3SetCustomEnterProfileZoneFunc(MyEnterProfileZoneFunc); + b3SetCustomLeaveProfileZoneFunc(MyLeaveProfileZoneFunc); + //also for Bullet 2.x API + btSetCustomEnterProfileZoneFunc(MyEnterProfileZoneFunc); + btSetCustomLeaveProfileZoneFunc(MyLeaveProfileZoneFunc); + } args.GetCmdLineArgument("fixed_timestep",gFixedTimeStep); args.GetCmdLineArgument("png_skip_frames", gPngSkipFrames); ///The OpenCL rigid body pipeline is experimental and @@ -1029,6 +1049,7 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[]) enable_experimental_opencl = true; gAllExamples->initOpenCLExampleEntries(); } + if (args.CheckCmdLineFlag("disable_retina")) { gAllowRetina = false; @@ -1456,7 +1477,11 @@ void OpenGLExampleBrowser::update(float deltaTime) if (m_internalData->m_gui) { + gBlockGuiMessages = true; m_internalData->m_gui->draw(s_instancingRenderer->getScreenWidth(), s_instancingRenderer->getScreenHeight()); + + + gBlockGuiMessages = false; } if (sUseOpenGL2) diff --git a/examples/ExampleBrowser/main.cpp b/examples/ExampleBrowser/main.cpp index 05985d41e..0395fcbd9 100644 --- a/examples/ExampleBrowser/main.cpp +++ b/examples/ExampleBrowser/main.cpp @@ -19,13 +19,15 @@ #include "LinearMath/btAlignedAllocator.h" +static double gMinUpdateTimeMicroSecs = 1000.; + int main(int argc, char* argv[]) { { b3CommandLineArgs args(argc, argv); b3Clock clock; - + args.GetCmdLineArgument("minUpdateTimeMicroSecs",gMinUpdateTimeMicroSecs); ExampleEntriesAll examples; examples.initExampleEntries(); @@ -45,9 +47,18 @@ int main(int argc, char* argv[]) do { float deltaTimeInSeconds = clock.getTimeMicroseconds() / 1000000.f; - clock.reset(); - exampleBrowser->update(deltaTimeInSeconds); - + if (deltaTimeInSeconds > 0.1) + { + deltaTimeInSeconds = 0.1; + } + if (deltaTimeInSeconds < (gMinUpdateTimeMicroSecs/1e6)) + { + b3Clock::usleep(gMinUpdateTimeMicroSecs/10.); + } else + { + clock.reset(); + exampleBrowser->update(deltaTimeInSeconds); + } } while (!exampleBrowser->requestedExit()); } delete exampleBrowser; diff --git a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp index c042c42c0..313ae041a 100644 --- a/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp +++ b/examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp @@ -8,7 +8,6 @@ #include "Bullet3Common/b3FileUtils.h" #include "../../ThirdPartyLibs/stb_image/stb_image.h" - bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& fileName, b3ImportMeshData& meshData) { diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index dd9159335..c93dc2cb3 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -447,6 +447,8 @@ static btCollisionShape* createConvexHullFromShapes(std::vectorm_geometry.m_type) @@ -677,6 +679,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH) { + BT_PROFILE("convert trimesh"); btTriangleMesh* meshInterface = new btTriangleMesh(); for (int i=0;im_numIndices/3;i++) { @@ -692,6 +695,8 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co shape = trimesh; } else { + BT_PROFILE("convert btConvexHullShape"); + btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3)); convexHull->optimizeConvexHull(); //convexHull->initializePolyhedralFeatures(); @@ -727,6 +732,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray& verticesOut, btAlignedObjectArray& indicesOut, btAlignedObjectArray& texturesOut) { + BT_PROFILE("convertURDFToVisualShapeInternal"); GLInstanceGraphicsShape* glmesh = 0; @@ -981,6 +987,8 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha //if we have a convex, tesselate into localVertices/localIndices if ((glmesh==0) && convexColShape) { + BT_PROFILE("convexColShape"); + btShapeHull* hull = new btShapeHull(convexColShape); hull->buildHull(0.0); { @@ -1029,7 +1037,7 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha if (glmesh && glmesh->m_numIndices>0 && glmesh->m_numvertices >0) { - + BT_PROFILE("glmesh"); int baseIndex = verticesOut.size(); diff --git a/examples/OpenGLWindow/GLPrimInternalData.h b/examples/OpenGLWindow/GLPrimInternalData.h index 5fdc62991..fcf2e666f 100644 --- a/examples/OpenGLWindow/GLPrimInternalData.h +++ b/examples/OpenGLWindow/GLPrimInternalData.h @@ -13,8 +13,13 @@ struct PrimInternalData GLint m_positionAttribute; GLint m_textureAttribute; GLuint m_vertexBuffer; - GLuint m_vertexArrayObject; + GLuint m_vertexBuffer2; + + GLuint m_vertexArrayObject; + GLuint m_vertexArrayObject2; + GLuint m_indexBuffer; + GLuint m_indexBuffer2; GLuint m_texturehandle; }; diff --git a/examples/OpenGLWindow/GLPrimitiveRenderer.cpp b/examples/OpenGLWindow/GLPrimitiveRenderer.cpp index 314066851..ba7c217d5 100644 --- a/examples/OpenGLWindow/GLPrimitiveRenderer.cpp +++ b/examples/OpenGLWindow/GLPrimitiveRenderer.cpp @@ -48,10 +48,20 @@ static const char* fragmentShader3D= \ static unsigned int s_indexData[6] = {0,1,2,0,2,3}; +#define MAX_VERTICES2 8192 +struct PrimInternalData2 +{ + PrimInternalData2() + :m_numVerticesText(0), + m_numVerticesRect(0) + { + } + int m_numVerticesText; + int m_numVerticesRect; + PrimVertex m_verticesText[MAX_VERTICES2]; + PrimVertex m_verticesRect[MAX_VERTICES2]; - - - +}; GLPrimitiveRenderer::GLPrimitiveRenderer(int screenWidth, int screenHeight) :m_screenWidth(screenWidth), @@ -59,6 +69,7 @@ m_screenHeight(screenHeight) { m_data = new PrimInternalData; + m_data2 = new PrimInternalData2; m_data->m_shaderProg = gltLoadShaderPair(vertexShader3D,fragmentShader3D); @@ -109,6 +120,13 @@ void GLPrimitiveRenderer::loadBufferData() glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vertexBuffer); glBufferData(GL_ARRAY_BUFFER, 4 * sizeof(PrimVertex), vertexData, GL_DYNAMIC_DRAW); + glGenVertexArrays(1, &m_data->m_vertexArrayObject2); + glBindVertexArray(m_data->m_vertexArrayObject2); + glGenBuffers(1, &m_data->m_vertexBuffer2); + glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vertexBuffer2); + glBufferData(GL_ARRAY_BUFFER, MAX_VERTICES2 * sizeof(PrimVertex), 0, GL_DYNAMIC_DRAW); + + assert(glGetError()==GL_NO_ERROR); @@ -116,7 +134,23 @@ void GLPrimitiveRenderer::loadBufferData() glGenBuffers(1, &m_data->m_indexBuffer); glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_data->m_indexBuffer); glBufferData(GL_ELEMENT_ARRAY_BUFFER,6*sizeof(int), s_indexData,GL_STATIC_DRAW); - + + unsigned int indexData[MAX_VERTICES2*2]; + int count=0; + for (int i=0;im_indexBuffer2); + glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_data->m_indexBuffer2); + glBufferData(GL_ELEMENT_ARRAY_BUFFER,count*sizeof(int), indexData,GL_STATIC_DRAW); + glEnableVertexAttribArray(m_data->m_positionAttribute); glEnableVertexAttribArray(m_data->m_colourAttribute); assert(glGetError()==GL_NO_ERROR); @@ -182,6 +216,7 @@ GLPrimitiveRenderer::~GLPrimitiveRenderer() glBindTexture(GL_TEXTURE_2D,0); glDeleteProgram(m_data->m_shaderProg); delete m_data; + delete m_data2; } void GLPrimitiveRenderer::drawLine() @@ -299,6 +334,180 @@ void GLPrimitiveRenderer::drawTexturedRect3D(const PrimVertex& v0,const PrimVert } +void GLPrimitiveRenderer::drawTexturedRect3D2Text( bool useRGBA) +{ + drawTexturedRect3D2(&m_data2->m_verticesText[0],m_data2->m_numVerticesText,useRGBA); + m_data2->m_numVerticesText = 0; +} + +void GLPrimitiveRenderer::drawTexturedRect3D2( PrimVertex* vertices, int numVertices, bool useRGBA) +{ + //B3_PROFILE("drawTexturedRect3D2"); + if (numVertices==0) + { + return; + } + //B3_PROFILE("GLPrimitiveRenderer::drawTexturedRect3D"); + + assert(glGetError()==GL_NO_ERROR); + float identity[16]={1,0,0,0, + 0,1,0,0, + 0,0,1,0, + 0,0,0,1}; + + glUseProgram(m_data->m_shaderProg); + + glUniformMatrix4fv(m_data->m_viewmatUniform, 1, false, identity); + glUniformMatrix4fv(m_data->m_projMatUniform, 1, false, identity); + + assert(glGetError()==GL_NO_ERROR); + + glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vertexBuffer2); + glBindVertexArray(m_data->m_vertexArrayObject2); + + bool useFiltering = false; + if (useFiltering) + { + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); + } else + { + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); + } + + /* PrimVertex vertexData[4] = { + v0,v1,v2,v3 + }; + */ + + glBufferSubData(GL_ARRAY_BUFFER, 0,numVertices * sizeof(PrimVertex), vertices); + + + + + + + assert(glGetError()==GL_NO_ERROR); + + PrimVec2 p( 0.f,0.f);//?b?0.5f * sinf(timeValue), 0.5f * cosf(timeValue) ); + if (useRGBA) + { + p.p[0] = 1.f; + p.p[1] = 1.f; + } + + glUniform2fv(m_data->m_positionUniform, 1, (const GLfloat *)&p); + + glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0); + + assert(glGetError()==GL_NO_ERROR); + + glEnableVertexAttribArray(m_data->m_positionAttribute); + assert(glGetError()==GL_NO_ERROR); + + glEnableVertexAttribArray(m_data->m_colourAttribute); + assert(glGetError()==GL_NO_ERROR); + + glEnableVertexAttribArray(m_data->m_textureAttribute); + + glVertexAttribPointer(m_data->m_positionAttribute, 4, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)0); + glVertexAttribPointer(m_data->m_colourAttribute , 4, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)sizeof(PrimVec4)); + glVertexAttribPointer(m_data->m_textureAttribute , 2, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)(sizeof(PrimVec4)+sizeof(PrimVec4))); + assert(glGetError()==GL_NO_ERROR); + + glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_data->m_indexBuffer2); + + //glDrawArrays(GL_TRIANGLE_FAN, 0, 4); + int indexCount = (numVertices/4)*6; + assert(glGetError()==GL_NO_ERROR); + + + glDrawElements(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, 0); + assert(glGetError()==GL_NO_ERROR); + + + glBindVertexArray(0); + assert(glGetError()==GL_NO_ERROR); + + glBindBuffer(GL_ARRAY_BUFFER, 0); + assert(glGetError()==GL_NO_ERROR); + + glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0); + assert(glGetError()==GL_NO_ERROR); + + //glDisableVertexAttribArray(m_data->m_textureAttribute); + assert(glGetError()==GL_NO_ERROR); + + glUseProgram(0); + + assert(glGetError()==GL_NO_ERROR); + + +} + +void GLPrimitiveRenderer::drawTexturedRect2a(float x0, float y0, float x1, float y1, float color[4], float u0,float v0, float u1, float v1, int useRGBA) +{ + + PrimVertex vertexData[4] = { + { PrimVec4(-1.f+2.f*x0/float(m_screenWidth), 1.f-2.f*y0/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v0)}, + { PrimVec4(-1.f+2.f*x0/float(m_screenWidth), 1.f-2.f*y1/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v1)}, + { PrimVec4( -1.f+2.f*x1/float(m_screenWidth), 1.f-2.f*y1/float(m_screenHeight), 0.f, 1.f ), PrimVec4(color[0], color[1], color[2], color[3]) ,PrimVec2(u1,v1)}, + { PrimVec4( -1.f+2.f*x1/float(m_screenWidth), 1.f-2.f*y0/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u1,v0)} + }; + + int sz = m_data2->m_numVerticesText; + + m_data2->m_verticesRect[m_data2->m_numVerticesRect++]=vertexData[0]; + m_data2->m_verticesRect[m_data2->m_numVerticesRect++]=vertexData[1]; + m_data2->m_verticesRect[m_data2->m_numVerticesRect++]=vertexData[2]; + m_data2->m_verticesRect[m_data2->m_numVerticesRect++]=vertexData[3]; + + + if (m_data2->m_numVerticesRect>=MAX_VERTICES2) + { + flushBatchedRects(); + } + +} + +void GLPrimitiveRenderer::flushBatchedRects() +{ + if (m_data2->m_numVerticesRect==0) + return; + + glActiveTexture(GL_TEXTURE0); + assert(glGetError()==GL_NO_ERROR); + glBindTexture(GL_TEXTURE_2D,m_data->m_texturehandle); + drawTexturedRect3D2(m_data2->m_verticesRect, m_data2->m_numVerticesRect,0); + m_data2->m_numVerticesRect=0; +} +void GLPrimitiveRenderer::drawTexturedRect2(float x0, float y0, float x1, float y1, float color[4], float u0,float v0, float u1, float v1, int useRGBA) +{ + + PrimVertex vertexData[4] = { + { PrimVec4(-1.f+2.f*x0/float(m_screenWidth), 1.f-2.f*y0/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v0)}, + { PrimVec4(-1.f+2.f*x0/float(m_screenWidth), 1.f-2.f*y1/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v1)}, + { PrimVec4( -1.f+2.f*x1/float(m_screenWidth), 1.f-2.f*y1/float(m_screenHeight), 0.f, 1.f ), PrimVec4(color[0], color[1], color[2], color[3]) ,PrimVec2(u1,v1)}, + { PrimVec4( -1.f+2.f*x1/float(m_screenWidth), 1.f-2.f*y0/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u1,v0)} + }; + + int sz = m_data2->m_numVerticesText; + + m_data2->m_verticesText[m_data2->m_numVerticesText++]=vertexData[0]; + m_data2->m_verticesText[m_data2->m_numVerticesText++]=vertexData[1]; + m_data2->m_verticesText[m_data2->m_numVerticesText++]=vertexData[2]; + m_data2->m_verticesText[m_data2->m_numVerticesText++]=vertexData[3]; + + + if (m_data2->m_numVerticesText>=MAX_VERTICES2) + { + drawTexturedRect3D2(m_data2->m_verticesText, m_data2->m_numVerticesText,useRGBA); + m_data2->m_numVerticesText=0; + } + +} + void GLPrimitiveRenderer::drawTexturedRect(float x0, float y0, float x1, float y1, float color[4], float u0,float v0, float u1, float v1, int useRGBA) { diff --git a/examples/OpenGLWindow/GLPrimitiveRenderer.h b/examples/OpenGLWindow/GLPrimitiveRenderer.h index 1c4f2e3f3..23cd2e8b9 100644 --- a/examples/OpenGLWindow/GLPrimitiveRenderer.h +++ b/examples/OpenGLWindow/GLPrimitiveRenderer.h @@ -5,6 +5,8 @@ struct PrimVec2 { + PrimVec2() + {} PrimVec2(float x, float y) { p[0] = x; @@ -28,12 +30,21 @@ struct PrimVec4 float p[4]; }; -typedef struct +struct PrimVertex { + PrimVertex(const PrimVec4 & p, const PrimVec4 & c, const PrimVec2& u) + :position(p), + colour(c), + uv(u) + { + } + + PrimVertex() + {} PrimVec4 position; PrimVec4 colour; PrimVec2 uv; -} PrimVertex; +} ; class GLPrimitiveRenderer @@ -42,7 +53,7 @@ class GLPrimitiveRenderer int m_screenHeight; struct PrimInternalData* m_data; - + struct PrimInternalData2* m_data2; void loadBufferData(); public: @@ -56,6 +67,14 @@ public: void drawLine();//float from[4], float to[4], float color[4]); void setScreenSize(int width, int height); + void drawTexturedRect2(float x0, float y0, float x1, float y1, float color[4], float u0,float v0, float u1, float v1, int useRGBA=0); + void drawTexturedRect2a(float x0, float y0, float x1, float y1, float color[4], float u0,float v0, float u1, float v1, int useRGBA=0); + void flushBatchedRects(); + + + void drawTexturedRect3D2Text(bool useRGBA = true); + void drawTexturedRect3D2(PrimVertex* vertices, int numVertices, bool useRGBA = true); + PrimInternalData* getData() { return m_data; diff --git a/examples/OpenGLWindow/GwenOpenGL3CoreRenderer.h b/examples/OpenGLWindow/GwenOpenGL3CoreRenderer.h index 2bc1a01de..0dc19ade6 100644 --- a/examples/OpenGLWindow/GwenOpenGL3CoreRenderer.h +++ b/examples/OpenGLWindow/GwenOpenGL3CoreRenderer.h @@ -161,6 +161,7 @@ public: virtual void StartClip() { + if (m_useTrueTypeFont) sth_flush_draw(m_font); Gwen::Rect rect = ClipRegion(); @@ -181,6 +182,7 @@ public: virtual void EndClip() { + if (m_useTrueTypeFont) sth_flush_draw(m_font); glDisable( GL_SCISSOR_TEST ); @@ -196,16 +198,23 @@ public: } virtual void DrawFilledRect( Gwen::Rect rect ) { +// BT_PROFILE("GWEN_DrawFilledRect"); Translate( rect ); - m_primitiveRenderer->drawRect(rect.x, rect.y+m_yOffset, rect.x+rect.w, rect.y+rect.h+m_yOffset, m_currentColor); + m_primitiveRenderer->drawRect(rect.x, rect.y+m_yOffset, + rect.x+rect.w, rect.y+rect.h+m_yOffset, m_currentColor); + + +// m_primitiveRenderer->drawTexturedRect2a(rect.x, rect.y+m_yOffset, + // rect.x+rect.w, rect.y+rect.h+m_yOffset, m_currentColor,0,0,1,1); // m_yOffset+=rect.h+10; } void RenderText( Gwen::Font* pFont, Gwen::Point rasterPos, const Gwen::UnicodeString& text ) { - +// BT_PROFILE("GWEN_RenderText"); + Gwen::String str = Gwen::Utility::UnicodeToString(text); const char* unicodeText = (const char*)str.c_str(); @@ -246,32 +255,33 @@ public: glBindTexture(GL_TEXTURE_2D,m_fontTextureId); float width = r.x; + while (unicodeText[pos]) { + int c = unicodeText[pos]; r.h = m_currentFont->m_CharHeight; r.w = m_currentFont->m_CharWidth[c]+extraSpacing; Gwen::Rect rect = r; Translate( rect ); - m_primitiveRenderer->drawTexturedRect(rect.x, rect.y+m_yOffset, rect.x+rect.w, rect.y+rect.h+m_yOffset, m_currentColor,m_currentFont->m_CharU0[c],m_currentFont->m_CharV0[c],m_currentFont->m_CharU1[c],m_currentFont->m_CharV1[c]); + m_primitiveRenderer->drawTexturedRect2(rect.x, rect.y+m_yOffset, rect.x+rect.w, rect.y+rect.h+m_yOffset, m_currentColor,m_currentFont->m_CharU0[c],m_currentFont->m_CharV0[c],m_currentFont->m_CharU1[c],m_currentFont->m_CharV1[c]); - //DrawTexturedRect(0,r,m_currentFont->m_CharU0[c],m_currentFont->m_CharV0[c],m_currentFont->m_CharU1[c],m_currentFont->m_CharV1[c]); - // DrawFilledRect(r); - - - width += r.w; r.x = width; pos++; } + { + m_primitiveRenderer->drawTexturedRect3D2Text(false); + } glBindTexture(GL_TEXTURE_2D,0); } } Gwen::Point MeasureText( Gwen::Font* pFont, const Gwen::UnicodeString& text ) { +// BT_PROFILE("GWEN_MeasureText"); Gwen::String str = Gwen::Utility::UnicodeToString(text); const char* unicodeText = (const char*)str.c_str(); @@ -341,7 +351,7 @@ public: virtual void DrawTexturedRect( Gwen::Texture* pTexture, Gwen::Rect rect, float u1=0.0f, float v1=0.0f, float u2=1.0f, float v2=1.0f ) { - +// BT_PROFILE("DrawTexturedRect"); Translate( rect ); //float eraseColor[4] = {0,0,0,0}; diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index ae0ab604b..61d2390bd 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -207,7 +207,7 @@ bool PhysicsClientSharedMemory::connect() { m_data->m_isConnected = true; } } else { - b3Error("Cannot connect to shared memory"); + b3Warning("Cannot connect to shared memory"); return false; } #if 0 diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 93bb085f9..8448d98f4 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1044,6 +1044,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn, bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes) { + BT_PROFILE("loadURDF"); btAssert(m_data->m_dynamicsWorld); if (!m_data->m_dynamicsWorld) { diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index d176e5d07..c45a2461d 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -239,6 +239,10 @@ struct MotionArgs } } b3CriticalSection* m_cs; + b3CriticalSection* m_cs2; + b3CriticalSection* m_cs3; + b3CriticalSection* m_csGUI; + btAlignedObjectArray m_mouseCommands; @@ -289,16 +293,21 @@ void MotionThreadFunc(void* userPtr,void* lsMemory) do { + BT_PROFILE("loop"); deltaTimeInSeconds+= double(clock.getTimeMicroseconds())/1000000.; + clock.reset(); if (deltaTimeInSeconds<(1./5000.)) { skip++; skip1++; + //if (skip1>105) if (skip1>5) { + BT_PROFILE("b3Clock::usleep(250)"); b3Clock::usleep(250); + skip1 = 0; } } else { @@ -358,9 +367,9 @@ void MotionThreadFunc(void* userPtr,void* lsMemory) //b3Warning("Clamp deltaTime from %f to %f",deltaTimeInSeconds, clampedDeltaTime); } - clock.reset(); + - args->m_cs->lock(); + args->m_csGUI->lock(); int numSendVrControllers = 0; for (int i=0;im_cs->unlock(); - - args->m_physicsServerPtr->stepSimulationRealTime(deltaTimeInSeconds, args->m_sendVrControllerEvents,numSendVrControllers); + args->m_csGUI->unlock(); + { + BT_PROFILE("stepSimulationRealTime"); + args->m_physicsServerPtr->stepSimulationRealTime(deltaTimeInSeconds, args->m_sendVrControllerEvents,numSendVrControllers); + } deltaTimeInSeconds = 0; } - args->m_cs->lock(); + args->m_csGUI->lock(); for (int i = 0; i < args->m_mouseCommands.size(); i++) { switch (args->m_mouseCommands[i].m_type) @@ -418,10 +429,12 @@ void MotionThreadFunc(void* userPtr,void* lsMemory) } } args->m_mouseCommands.clear(); - args->m_cs->unlock(); + args->m_csGUI->unlock(); - - args->m_physicsServerPtr->processClientCommands(); + { + BT_PROFILE("processClientCommands"); + args->m_physicsServerPtr->processClientCommands(); + } } while (args->m_cs->getSharedParam(0)!=eRequestTerminateMotion); } else @@ -478,9 +491,10 @@ class MultiThreadedOpenGLGuiHelper : public GUIHelperInterface CommonGraphicsApp* m_app; b3CriticalSection* m_cs; - + b3CriticalSection* m_cs2; + b3CriticalSection* m_cs3; + b3CriticalSection* m_csGUI; - public: @@ -506,11 +520,43 @@ public: int m_textureId; int m_instanceId; - + void mainThreadRelease() + { + BT_PROFILE("mainThreadRelease"); + + getCriticalSection()->setSharedParam(1,eGUIHelperIdle); + getCriticalSection3()->lock(); + getCriticalSection2()->unlock(); + getCriticalSection()->lock(); + getCriticalSection2()->lock(); + getCriticalSection()->unlock(); + getCriticalSection3()->unlock(); + + } + + void workerThreadWait() + { + BT_PROFILE("workerThreadWait"); + m_cs2->lock(); + m_cs->unlock(); + m_cs2->unlock(); + m_cs3->lock(); + m_cs3->unlock(); + + + + while (m_cs->getSharedParam(1)!=eGUIHelperIdle) + { + b3Clock::usleep(100); + } + } MultiThreadedOpenGLGuiHelper(CommonGraphicsApp* app, GUIHelperInterface* guiHelper) :m_app(app) ,m_cs(0), + m_cs2(0), + m_cs3(0), + m_csGUI(0), m_uidGenerator(0), m_texels(0), m_textureId(-1) @@ -534,6 +580,32 @@ public: return m_cs; } + + void setCriticalSection2(b3CriticalSection* cs) + { + m_cs2 = cs; + } + + b3CriticalSection* getCriticalSection2() + { + return m_cs2; + } + + void setCriticalSection3(b3CriticalSection* cs) + { + m_cs3 = cs; + } + + void setCriticalSectionGUI(b3CriticalSection* cs) + { + m_csGUI = cs; + } + + + b3CriticalSection* getCriticalSection3() + { + return m_cs3; + } btRigidBody* m_body; btVector3 m_color3; virtual void createRigidBodyGraphicsObject(btRigidBody* body,const btVector3& color) @@ -542,11 +614,8 @@ public: m_color3 = color; m_cs->lock(); m_cs->setSharedParam(1,eGUIHelperCreateRigidBodyGraphicsObject); - m_cs->unlock(); - while (m_cs->getSharedParam(1)!=eGUIHelperIdle) - { - b3Clock::usleep(1000); - } + workerThreadWait(); + } btCollisionObject* m_obj; @@ -558,11 +627,7 @@ public: m_color2 = color; m_cs->lock(); m_cs->setSharedParam(1,eGUIHelperCreateCollisionObjectGraphicsObject); - m_cs->unlock(); - while (m_cs->getSharedParam(1)!=eGUIHelperIdle) - { - b3Clock::usleep(1000); - } + workerThreadWait(); } @@ -572,11 +637,7 @@ public: m_colShape = collisionShape; m_cs->lock(); m_cs->setSharedParam(1,eGUIHelperCreateCollisionShapeGraphicsObject); - m_cs->unlock(); - while (m_cs->getSharedParam(1)!=eGUIHelperIdle) - { - b3Clock::usleep(1000); - } + workerThreadWait(); } @@ -584,7 +645,7 @@ public: { //this check is to prevent a crash, in case we removed all graphics instances, but there are still physics objects. //the check will be obsolete, once we have a better/safer way of synchronizing physics->graphics transforms - if ( m_childGuiHelper->getRenderInterface()->getTotalNumInstances()>0) + if ( m_childGuiHelper->getRenderInterface() && m_childGuiHelper->getRenderInterface()->getTotalNumInstances()>0) { m_childGuiHelper->syncPhysicsToGraphics(rbWorld); } @@ -608,11 +669,10 @@ public: m_cs->lock(); m_cs->setSharedParam(1,eGUIHelperRegisterTexture); - m_cs->unlock(); - while (m_cs->getSharedParam(1)!=eGUIHelperIdle) - { - b3Clock::usleep(1000); - } + + workerThreadWait(); + + return m_textureId; } virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId) @@ -626,11 +686,8 @@ public: m_cs->lock(); m_cs->setSharedParam(1,eGUIHelperRegisterGraphicsShape); - m_cs->unlock(); - while (m_cs->getSharedParam(1)!=eGUIHelperIdle) - { - b3Clock::usleep(1000); - } + workerThreadWait(); + return m_shapeIndex; } virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling) @@ -643,11 +700,7 @@ public: m_cs->lock(); m_cs->setSharedParam(1,eGUIHelperRegisterGraphicsInstance); - m_cs->unlock(); - while (m_cs->getSharedParam(1)!=eGUIHelperIdle) - { - b3Clock::usleep(1000); - } + workerThreadWait(); return m_instanceId; } @@ -655,11 +708,7 @@ public: { m_cs->lock(); m_cs->setSharedParam(1,eGUIHelperRemoveAllGraphicsInstances); - m_cs->unlock(); - while (m_cs->getSharedParam(1)!=eGUIHelperIdle) - { - b3Clock::usleep(1000); - } + workerThreadWait(); } virtual Common2dCanvasInterface* get2dCanvasInterface() @@ -730,11 +779,7 @@ public: m_numPixelsCopied = numPixelsCopied; m_cs->setSharedParam(1,eGUIHelperCopyCameraImageData); - m_cs->unlock(); - while (m_cs->getSharedParam(1)!=eGUIHelperIdle) - { - b3Clock::usleep(1000); - } + workerThreadWait(); } @@ -745,11 +790,7 @@ public: m_dynamicsWorld = rbWorld; m_cs->lock(); m_cs->setSharedParam(1, eGUIHelperAutogenerateGraphicsObjects); - m_cs->unlock(); - while (m_cs->getSharedParam(1) != eGUIHelperIdle) - { - b3Clock::usleep(1000); - } + workerThreadWait(); } virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size) @@ -780,11 +821,7 @@ public: m_cs->lock(); m_cs->setSharedParam(1, eGUIUserDebugAddText); - m_cs->unlock(); - while (m_cs->getSharedParam(1) != eGUIHelperIdle) - { - b3Clock::usleep(150); - } + workerThreadWait(); return m_userDebugText[m_userDebugText.size()-1].m_itemUniqueId; } @@ -811,11 +848,7 @@ public: m_cs->lock(); m_cs->setSharedParam(1, eGUIUserDebugAddLine); - m_cs->unlock(); - while (m_cs->getSharedParam(1) != eGUIHelperIdle) - { - b3Clock::usleep(150); - } + workerThreadWait(); return m_userDebugLines[m_userDebugLines.size()-1].m_itemUniqueId; } @@ -826,22 +859,14 @@ public: m_removeDebugItemUid = debugItemUniqueId; m_cs->lock(); m_cs->setSharedParam(1, eGUIUserDebugRemoveItem); - m_cs->unlock(); - while (m_cs->getSharedParam(1) != eGUIHelperIdle) - { - b3Clock::usleep(150); - } + workerThreadWait(); } virtual void removeAllUserDebugItems( ) { m_cs->lock(); m_cs->setSharedParam(1, eGUIUserDebugRemoveAllItems); - m_cs->unlock(); - while (m_cs->getSharedParam(1) != eGUIHelperIdle) - { - b3Clock::usleep(150); - } + workerThreadWait(); } @@ -934,9 +959,10 @@ public: cmd.m_rayFrom = rayFrom; cmd.m_rayTo = rayTo; cmd.m_type = MyMouseMove; - m_args[0].m_cs->lock(); + m_args[0].m_csGUI->lock(); m_args[0].m_mouseCommands.push_back(cmd); - m_args[0].m_cs->unlock(); + m_args[0].m_csGUI->unlock(); + return false; }; @@ -969,10 +995,11 @@ public: cmd.m_rayFrom = rayFrom; cmd.m_rayTo = rayTo; cmd.m_type = MyMouseButtonDown; - m_args[0].m_cs->lock(); + + m_args[0].m_csGUI->lock(); m_args[0].m_mouseCommands.push_back(cmd); - m_args[0].m_cs->unlock(); - + m_args[0].m_csGUI->unlock(); + } } else @@ -984,9 +1011,10 @@ public: cmd.m_rayFrom.setValue(0,0,0); cmd.m_rayTo.setValue(0, 0, 0); cmd.m_type = MyMouseButtonUp; - m_args[0].m_cs->lock(); + + m_args[0].m_csGUI->lock(); m_args[0].m_mouseCommands.push_back(cmd); - m_args[0].m_cs->unlock(); + m_args[0].m_csGUI->unlock(); //remove p2p } } @@ -1005,6 +1033,13 @@ public: { b3CommandLineArgs args(argc,argv); loadCurrentSettingsVR(args); + int shmemKey; + + if (args.GetCmdLineArgument("sharedMemoryKey", shmemKey)) + { + setSharedMemoryKey(shmemKey); + } + if (args.GetCmdLineArgument("camPosX", gVRTeleportPos1[0])) { printf("camPosX=%f\n", gVRTeleportPos1[0]); @@ -1035,7 +1070,7 @@ public: if (args.CheckCmdLineFlag("norobotassets")) { - gCreateDefaultRobotAssets = false; +// gCreateDefaultRobotAssets = false; } @@ -1149,6 +1184,10 @@ void PhysicsServerExample::initPhysics() for (int w=0;wcreateCriticalSection(); + m_args[w].m_cs2 = m_threadSupport->createCriticalSection(); + m_args[w].m_cs3 = m_threadSupport->createCriticalSection(); + m_args[w].m_csGUI = m_threadSupport->createCriticalSection(); + m_args[w].m_cs->setSharedParam(0,eMotionIsUnInitialized); int numMoving = 0; m_args[w].m_positions.resize(numMoving); @@ -1165,6 +1204,13 @@ void PhysicsServerExample::initPhysics() m_args[0].m_cs->setSharedParam(1,eGUIHelperIdle); m_multiThreadedHelper->setCriticalSection(m_args[0].m_cs); + m_multiThreadedHelper->setCriticalSection2(m_args[0].m_cs2); + m_multiThreadedHelper->setCriticalSection3(m_args[0].m_cs3); + m_multiThreadedHelper->setCriticalSectionGUI(m_args[0].m_csGUI); + + m_args[0].m_cs2->lock(); + + m_isConnected = m_physicsServer.connectSharedMemory( m_guiHelper); } @@ -1213,8 +1259,35 @@ bool PhysicsServerExample::wantsTermination() void PhysicsServerExample::stepSimulation(float deltaTime) { + BT_PROFILE("PhysicsServerExample::stepSimulation"); + //this->m_physicsServer.processClientCommands(); + for (int i = m_multiThreadedHelper->m_userDebugLines.size()-1;i>=0;i--) + { + if (m_multiThreadedHelper->m_userDebugLines[i].m_lifeTime) + { + m_multiThreadedHelper->m_userDebugLines[i].m_lifeTime -= deltaTime; + if (m_multiThreadedHelper->m_userDebugLines[i].m_lifeTime<=0) + { + m_multiThreadedHelper->m_userDebugLines.swap(i,m_multiThreadedHelper->m_userDebugLines.size()-1); + m_multiThreadedHelper->m_userDebugLines.pop_back(); + } + } + } + + for (int i = m_multiThreadedHelper->m_userDebugText.size()-1;i>=0;i--) + { + if (m_multiThreadedHelper->m_userDebugText[i].m_lifeTime) + { + m_multiThreadedHelper->m_userDebugText[i].m_lifeTime -= deltaTime; + if (m_multiThreadedHelper->m_userDebugText[i].m_lifeTime<=0) + { + m_multiThreadedHelper->m_userDebugText.swap(i,m_multiThreadedHelper->m_userDebugText.size()-1); + m_multiThreadedHelper->m_userDebugText.pop_back(); + } + } + } //check if any graphics related tasks are requested switch (m_multiThreadedHelper->getCriticalSection()->getSharedParam(1)) @@ -1222,27 +1295,21 @@ void PhysicsServerExample::stepSimulation(float deltaTime) case eGUIHelperCreateCollisionShapeGraphicsObject: { m_multiThreadedHelper->m_childGuiHelper->createCollisionShapeGraphicsObject(m_multiThreadedHelper->m_colShape); - m_multiThreadedHelper->getCriticalSection()->lock(); - m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle); - m_multiThreadedHelper->getCriticalSection()->unlock(); - + m_multiThreadedHelper->mainThreadRelease(); break; } case eGUIHelperCreateCollisionObjectGraphicsObject: { m_multiThreadedHelper->m_childGuiHelper->createCollisionObjectGraphicsObject(m_multiThreadedHelper->m_obj, m_multiThreadedHelper->m_color2); - m_multiThreadedHelper->getCriticalSection()->lock(); - m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle); - m_multiThreadedHelper->getCriticalSection()->unlock(); + m_multiThreadedHelper->mainThreadRelease(); + break; } case eGUIHelperCreateRigidBodyGraphicsObject: { m_multiThreadedHelper->m_childGuiHelper->createRigidBodyGraphicsObject(m_multiThreadedHelper->m_body,m_multiThreadedHelper->m_color3); - m_multiThreadedHelper->getCriticalSection()->lock(); - m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle); - m_multiThreadedHelper->getCriticalSection()->unlock(); + m_multiThreadedHelper->mainThreadRelease(); break; } case eGUIHelperRegisterTexture: @@ -1250,11 +1317,7 @@ void PhysicsServerExample::stepSimulation(float deltaTime) m_multiThreadedHelper->m_textureId = m_multiThreadedHelper->m_childGuiHelper->registerTexture(m_multiThreadedHelper->m_texels, m_multiThreadedHelper->m_textureWidth,m_multiThreadedHelper->m_textureHeight); - - m_multiThreadedHelper->getCriticalSection()->lock(); - m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle); - m_multiThreadedHelper->getCriticalSection()->unlock(); - + m_multiThreadedHelper->mainThreadRelease(); break; } case eGUIHelperRegisterGraphicsShape: @@ -1266,10 +1329,7 @@ void PhysicsServerExample::stepSimulation(float deltaTime) m_multiThreadedHelper->m_numIndices, m_multiThreadedHelper->m_primitiveType, m_multiThreadedHelper->m_textureId); - - m_multiThreadedHelper->getCriticalSection()->lock(); - m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle); - m_multiThreadedHelper->getCriticalSection()->unlock(); + m_multiThreadedHelper->mainThreadRelease(); break; } case eGUIHelperRegisterGraphicsInstance: @@ -1280,21 +1340,19 @@ void PhysicsServerExample::stepSimulation(float deltaTime) m_multiThreadedHelper->m_quaternion, m_multiThreadedHelper->m_color, m_multiThreadedHelper->m_scaling); - - m_multiThreadedHelper->getCriticalSection()->lock(); - m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle); - m_multiThreadedHelper->getCriticalSection()->unlock(); + m_multiThreadedHelper->mainThreadRelease(); break; } case eGUIHelperRemoveAllGraphicsInstances: { m_multiThreadedHelper->m_childGuiHelper->removeAllGraphicsInstances(); - int numRenderInstances = m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->getTotalNumInstances(); - b3Assert(numRenderInstances==0); + if (m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()) + { + int numRenderInstances = m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->getTotalNumInstances(); + b3Assert(numRenderInstances==0); + } + m_multiThreadedHelper->mainThreadRelease(); - m_multiThreadedHelper->getCriticalSection()->lock(); - m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle); - m_multiThreadedHelper->getCriticalSection()->unlock(); break; } @@ -1312,34 +1370,26 @@ void PhysicsServerExample::stepSimulation(float deltaTime) m_multiThreadedHelper->m_destinationWidth, m_multiThreadedHelper->m_destinationHeight, m_multiThreadedHelper->m_numPixelsCopied); - m_multiThreadedHelper->getCriticalSection()->lock(); - m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle); - m_multiThreadedHelper->getCriticalSection()->unlock(); + m_multiThreadedHelper->mainThreadRelease(); break; } case eGUIHelperAutogenerateGraphicsObjects: { m_multiThreadedHelper->m_childGuiHelper->autogenerateGraphicsObjects(m_multiThreadedHelper->m_dynamicsWorld); - m_multiThreadedHelper->getCriticalSection()->lock(); - m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperIdle); - m_multiThreadedHelper->getCriticalSection()->unlock(); + m_multiThreadedHelper->mainThreadRelease(); break; } case eGUIUserDebugAddText: { m_multiThreadedHelper->m_userDebugText.push_back(m_multiThreadedHelper->m_tmpText); - m_multiThreadedHelper->getCriticalSection()->lock(); - m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperIdle); - m_multiThreadedHelper->getCriticalSection()->unlock(); + m_multiThreadedHelper->mainThreadRelease(); break; } case eGUIUserDebugAddLine: { m_multiThreadedHelper->m_userDebugLines.push_back(m_multiThreadedHelper->m_tmpLine); - m_multiThreadedHelper->getCriticalSection()->lock(); - m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperIdle); - m_multiThreadedHelper->getCriticalSection()->unlock(); + m_multiThreadedHelper->mainThreadRelease(); break; } case eGUIUserDebugRemoveItem: @@ -1365,9 +1415,7 @@ void PhysicsServerExample::stepSimulation(float deltaTime) } } - m_multiThreadedHelper->getCriticalSection()->lock(); - m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperIdle); - m_multiThreadedHelper->getCriticalSection()->unlock(); + m_multiThreadedHelper->mainThreadRelease(); break; } case eGUIUserDebugRemoveAllItems: @@ -1375,9 +1423,7 @@ void PhysicsServerExample::stepSimulation(float deltaTime) m_multiThreadedHelper->m_userDebugLines.clear(); m_multiThreadedHelper->m_userDebugText.clear(); m_multiThreadedHelper->m_uidGenerator = 0; - m_multiThreadedHelper->getCriticalSection()->lock(); - m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperIdle); - m_multiThreadedHelper->getCriticalSection()->unlock(); + m_multiThreadedHelper->mainThreadRelease(); break; } case eGUIHelperIdle: @@ -1670,9 +1716,11 @@ void PhysicsServerExample::renderScene() vrOffset[13]= trInv.getOrigin()[1]; vrOffset[14]= trInv.getOrigin()[2]; - this->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()-> + if (m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()) + { + m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()-> getActiveCamera()->setVRCameraOffsetTransform(vrOffset); - + } m_physicsServer.renderScene(); for (int i=0;ilock(); + m_args[0].m_csGUI->lock(); m_args[0].m_vrControllerEvents[controllerId].m_controllerId = controllerId; m_args[0].m_vrControllerEvents[controllerId].m_pos[0] = trTotal.getOrigin()[0]; m_args[0].m_vrControllerEvents[controllerId].m_pos[1] = trTotal.getOrigin()[1]; @@ -1957,7 +2005,7 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt m_args[0].m_vrControllerEvents[controllerId].m_buttons[button]|=eButtonReleased; m_args[0].m_vrControllerEvents[controllerId].m_buttons[button] &= ~eButtonIsDown; } - m_args[0].m_cs->unlock(); + m_args[0].m_csGUI->unlock(); } @@ -2009,7 +2057,7 @@ void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[ m_args[0].m_vrControllerOrn[controllerId] = trTotal.getRotation(); } - m_args[0].m_cs->lock(); + m_args[0].m_csGUI->lock(); m_args[0].m_vrControllerEvents[controllerId].m_controllerId = controllerId; m_args[0].m_vrControllerEvents[controllerId].m_pos[0] = trTotal.getOrigin()[0]; m_args[0].m_vrControllerEvents[controllerId].m_pos[1] = trTotal.getOrigin()[1]; @@ -2020,7 +2068,7 @@ 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; - m_args[0].m_cs->unlock(); + m_args[0].m_csGUI->unlock(); } B3_STANDALONE_EXAMPLE(PhysicsServerCreateFunc) diff --git a/examples/SharedMemory/Win32SharedMemory.cpp b/examples/SharedMemory/Win32SharedMemory.cpp index e030104fc..038705364 100644 --- a/examples/SharedMemory/Win32SharedMemory.cpp +++ b/examples/SharedMemory/Win32SharedMemory.cpp @@ -4,18 +4,17 @@ #include "Bullet3Common/b3Scalar.h" #include - +#include //see also https://msdn.microsoft.com/en-us/library/windows/desktop/aa366551%28v=vs.85%29.aspx -//TCHAR szName[]=TEXT("Global\\MyFileMappingObject2"); -TCHAR szName[]=TEXT("MyFileMappingObject2"); struct Win32SharedMemoryInteralData { HANDLE m_hMapFile; void* m_buf; - + TCHAR m_szName[1024]; + Win32SharedMemoryInteralData() :m_hMapFile(0), m_buf(0) @@ -36,10 +35,18 @@ void* Win32SharedMemory::allocateSharedMemory(int key, int size, bool allowCre { b3Assert(m_internalData->m_buf==0); +#ifdef UNICODE + swprintf_s (m_internalData->m_szName,"MyFileMappingObject%d",key); +#else + + sprintf(m_internalData->m_szName,"MyFileMappingObject%d",key); +#endif + + m_internalData->m_hMapFile = OpenFileMapping( FILE_MAP_ALL_ACCESS, // read/write access FALSE, // do not inherit the name - szName); // name of mapping object + m_internalData->m_szName); // name of mapping object if (m_internalData->m_hMapFile==NULL) { @@ -51,10 +58,10 @@ void* Win32SharedMemory::allocateSharedMemory(int key, int size, bool allowCre PAGE_READWRITE, // read/write access 0, // maximum object size (high-order DWORD) size, // maximum object size (low-order DWORD) - szName); // name of mapping object + m_internalData->m_szName); // name of mapping object } else { - b3Error("Could not create file mapping object (%d).\n",GetLastError()); + b3Warning("Could not create file mapping object (%d).\n",GetLastError()); return 0; } @@ -68,7 +75,7 @@ void* Win32SharedMemory::allocateSharedMemory(int key, int size, bool allowCre if (m_internalData->m_buf == NULL) { - b3Error("Could not map view of file (%d).\n",GetLastError()); + b3Warning("Could not map view of file (%d).\n",GetLastError()); CloseHandle(m_internalData->m_hMapFile); return 0; } diff --git a/examples/SharedMemory/main.cpp b/examples/SharedMemory/main.cpp index 5eabfff57..46c5c7527 100644 --- a/examples/SharedMemory/main.cpp +++ b/examples/SharedMemory/main.cpp @@ -72,6 +72,7 @@ int main(int argc, char* argv[]) CommonExampleOptions options(&noGfx); args.GetCmdLineArgument("shared_memory_key", gSharedMemoryKey); + args.GetCmdLineArgument("sharedMemoryKey", gSharedMemoryKey); // options.m_option |= PHYSICS_SERVER_ENABLE_COMMAND_LOGGING; // options.m_option |= PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG; diff --git a/examples/SharedMemory/udp/main.cpp b/examples/SharedMemory/udp/main.cpp index bc8c6c57d..af0a76e95 100644 --- a/examples/SharedMemory/udp/main.cpp +++ b/examples/SharedMemory/udp/main.cpp @@ -2,13 +2,16 @@ #include #include #include "../../CommonInterfaces/CommonGUIHelperInterface.h" +#include "Bullet3Common/b3CommandLineArgs.h" + #ifdef NO_SHARED_MEMORY #include "PhysicsServerCommandProcessor.h" typedef PhysicsServerCommandProcessor MyCommandProcessor; #else #include "SharedMemoryCommandProcessor.h" - typedef SharedMemoryCommandProcessor MyCommandProcessor ; + typedef SharedMemoryCommandProcessor MyCommandProcessor; #endif //NO_SHARED_MEMORY + #include "SharedMemoryCommands.h" #include "Bullet3Common/b3AlignedObjectArray.h" #include "PhysicsServerCommandProcessor.h" @@ -32,11 +35,28 @@ void MySerializeInt(unsigned int sz, unsigned char* output) int main(int argc, char *argv[]) { + b3CommandLineArgs parseArgs(argc,argv); DummyGUIHelper guiHelper; - PhysicsCommandProcessorInterface* sm = new MyCommandProcessor; + MyCommandProcessor* sm = new MyCommandProcessor; sm->setGuiHelper(&guiHelper); - + + int port = 1234; + if (parseArgs.GetCmdLineArgument("port",port)) + { + printf("Using UDP port %d\n", port); + } + + gVerboseNetworkMessagesServer = parseArgs.CheckCmdLineFlag("verbose"); + +#ifndef NO_SHARED_MEMORY + int key = 0; + if (parseArgs.GetCmdLineArgument("sharedMemoryKey",key)) + { + sm->setSharedMemoryKey(key); + } +#endif//NO_SHARED_MEMORY + // PhysicsDirect* sm = new PhysicsDirect(sdk); //PhysicsClientSharedMemory* sm = new PhysicsClientSharedMemory(); @@ -65,7 +85,7 @@ int main(int argc, char *argv[]) /* enet_address_set_host (& address, "x.x.x.x"); */ address.host = ENET_HOST_ANY; /* Bind the server to port 1234. */ - address.port = 1234; + address.port = port; server = enet_host_create(&address, 32, /* number of clients */ diff --git a/examples/StandaloneMain/hellovr_opengl_main.cpp b/examples/StandaloneMain/hellovr_opengl_main.cpp index 71a1aa7e9..0c3bae495 100644 --- a/examples/StandaloneMain/hellovr_opengl_main.cpp +++ b/examples/StandaloneMain/hellovr_opengl_main.cpp @@ -10,7 +10,7 @@ #include "Bullet3Common/b3Transform.h" #include "Bullet3Common/b3CommandLineArgs.h" - +#include "../Utils/b3Clock.h" #include "../ExampleBrowser/OpenGLGuiHelper.h" #include "../CommonInterfaces/CommonExampleInterface.h" #include "../CommonInterfaces/CommonGUIHelperInterface.h" @@ -156,6 +156,7 @@ private: uint32_t m_nWindowWidth; uint32_t m_nWindowHeight; bool m_hasContext; + b3Clock m_clock; private: // OpenGL bookkeeping int m_iTrackedControllerCount; @@ -1620,7 +1621,10 @@ void CMainApplication::RenderStereoTargets() { B3_PROFILE("CMainApplication::RenderStereoTargets"); - sExample->stepSimulation(1./60.); + btScalar dtSec = btScalar(m_clock.getTimeInSeconds()); + dtSec = b3Min(dtSec,0.1); + sExample->stepSimulation(dtSec); + m_clock.reset(); glClearColor( 0.15f, 0.15f, 0.18f, 1.0f ); // nice background color, but not black glEnable( GL_MULTISAMPLE ); diff --git a/examples/StandaloneMain/main_opengl_single_example.cpp b/examples/StandaloneMain/main_opengl_single_example.cpp index a040882c6..0111460c3 100644 --- a/examples/StandaloneMain/main_opengl_single_example.cpp +++ b/examples/StandaloneMain/main_opengl_single_example.cpp @@ -99,6 +99,7 @@ int main(int argc, char* argv[]) app->m_instancingRenderer->updateCamera(app->getUpAxis()); btScalar dtSec = btScalar(clock.getTimeInSeconds()); + dtSec = b3Min(dtSec,0.1); example->stepSimulation(dtSec); clock.reset(); diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 88cf53b85..35ce25d60 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -30,9 +30,31 @@ enum eCONNECT_METHOD { static PyObject* SpamError; #define MAX_PHYSICS_CLIENTS 1024 -static b3PhysicsClientHandle sPhysicsClients[MAX_PHYSICS_CLIENTS] = {0}; +static b3PhysicsClientHandle sPhysicsClients1[MAX_PHYSICS_CLIENTS] = {0}; static int sNumPhysicsClients=0; +b3PhysicsClientHandle getPhysicsClient(int physicsClientId) +{ + b3PhysicsClientHandle sm; + if ((physicsClientId <0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients1[physicsClientId])) + { + return 0; + } + sm = sPhysicsClients1[physicsClientId]; + if (sm) + { + if (b3CanSubmitCommand(sm)) + { + return sm; + } + //broken connection? + b3DisconnectSharedMemory(sm); + sPhysicsClients1[physicsClientId] = 0; + sNumPhysicsClients--; + } + return 0; + +} static double pybullet_internalGetFloatFromSequence(PyObject* seq, int index) { double v = 0.0; @@ -161,13 +183,12 @@ static PyObject* pybullet_stepSimulation(PyObject* self, PyObject* args, PyObjec { return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; - { b3SharedMemoryStatusHandle statusHandle; @@ -281,22 +302,23 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P }; } - - for (i=0;i=0) + { + sPhysicsClients1[freeIndex] = sm; + sNumPhysicsClients++; } } - - if (freeIndex>=0) - { - sPhysicsClients[freeIndex] = sm; - sNumPhysicsClients++; - } - return PyInt_FromLong(freeIndex); } @@ -311,20 +333,20 @@ static PyObject* pybullet_disconnectPhysicsServer(PyObject* self, { return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; - + { b3DisconnectSharedMemory(sm); sm = 0; } - sPhysicsClients[physicsClientId] = 0; + sPhysicsClients1[physicsClientId] = 0; sNumPhysicsClients--; Py_INCREF(Py_None); @@ -342,17 +364,12 @@ static PyObject* pybullet_saveWorld(PyObject* self, PyObject* args,PyObject *key { return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; - if (0==sm) - { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } { @@ -397,12 +414,12 @@ static PyObject* pybullet_loadBullet(PyObject* self, PyObject* args,PyObject *ke { return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; command = b3LoadBulletCommandInit(sm, bulletFileName); @@ -448,12 +465,12 @@ static PyObject* pybullet_saveBullet(PyObject* self, PyObject* args, PyObject* k { return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; command = b3SaveBulletCommandInit(sm, bulletFileName); @@ -485,12 +502,12 @@ static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args, PyObject* key { return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; command = b3LoadMJCFCommandInit(sm, mjcfFileName); @@ -522,12 +539,12 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; { b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); @@ -669,13 +686,12 @@ b3PhysicsClientHandle sm=0; } } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; - if (strlen(urdfFileName)) { // printf("(%f, %f, %f) (%f, %f, %f, %f)\n", @@ -736,12 +752,12 @@ b3PhysicsClientHandle sm = 0; { return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; commandHandle = b3LoadSdfCommandInit(sm, sdfFileName); @@ -780,12 +796,13 @@ static PyObject* pybullet_resetSimulation(PyObject* self, PyObject* args, PyObje { return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; + { b3SharedMemoryStatusHandle statusHandle; statusHandle = b3SubmitClientCommandAndWaitStatus( @@ -807,12 +824,13 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) { double kd = 1.0; int valid = 0; int physicsClientId = 0; - b3PhysicsClientHandle sm = sPhysicsClients[physicsClientId]; - - if (0 == sm) { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } + b3PhysicsClientHandle sm; + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } size = PySequence_Size(args); if (size == 4) { @@ -985,12 +1003,12 @@ b3PhysicsClientHandle sm = 0; { return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; { int numJoints; @@ -1066,12 +1084,12 @@ static PyObject* pybullet_setRealTimeSimulation(PyObject* self, { return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; { @@ -1106,12 +1124,12 @@ b3PhysicsClientHandle sm = 0; { return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; { @@ -1146,12 +1164,12 @@ static PyObject* pybullet_setGravity(PyObject* self, PyObject* args, PyObject* k { return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); @@ -1172,18 +1190,20 @@ static PyObject* pybullet_setTimeStep(PyObject* self, PyObject* args, PyObject* double timeStep = 0.001; int ret; int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + static char *kwlist[] = { "timeStep", "physicsClientId", NULL }; if (!PyArg_ParseTupleAndKeywords(args, keywds, "d|i", kwlist,&timeStep, &physicsClientId)) { return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } { - b3PhysicsClientHandle sm = sPhysicsClients[physicsClientId]; b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); b3SharedMemoryStatusHandle statusHandle; @@ -1202,18 +1222,20 @@ pybullet_setDefaultContactERP(PyObject* self, PyObject* args,PyObject* keywds) { double defaultContactERP=0.005; int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + static char *kwlist[] = { "defaultContactERP", "physicsClientId", NULL }; if (!PyArg_ParseTupleAndKeywords(args, keywds, "d|i", kwlist,&defaultContactERP, &physicsClientId)) { return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } { - b3PhysicsClientHandle sm = sPhysicsClients[physicsClientId]; int ret; b3SharedMemoryStatusHandle statusHandle; @@ -1366,12 +1388,12 @@ static PyObject* pybullet_getBasePositionAndOrientation(PyObject* self, { return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; if (0 == pybullet_internalGetBasePositionAndOrientation( bodyUniqueId, basePosition, baseOrientation,sm)) { @@ -1427,12 +1449,12 @@ static PyObject* pybullet_getBaseVelocity(PyObject* self, { return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; if (0 == pybullet_internalGetBaseVelocity( bodyUniqueId, baseLinearVelocity, baseAngularVelocity,sm)) { @@ -1481,12 +1503,12 @@ static PyObject* pybullet_getNumBodies(PyObject* self, PyObject* args, PyObject* { return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; { @@ -1511,12 +1533,12 @@ b3PhysicsClientHandle sm = 0; { return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; { @@ -1546,12 +1568,12 @@ static PyObject* pybullet_getBodyInfo(PyObject* self, PyObject* args, PyObject* { return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; { struct b3BodyInfo info; @@ -1587,12 +1609,12 @@ static PyObject* pybullet_getNumJoints(PyObject* self, PyObject* args, PyObject* { return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; numJoints = b3GetNumJoints(sm, bodyUniqueId); @@ -1619,12 +1641,12 @@ static PyObject* pybullet_resetJointState(PyObject* self, PyObject* args, PyObje { return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; { @@ -1674,12 +1696,12 @@ static PyObject* pybullet_resetBaseVelocity(PyObject* self, PyObject* args, PyOb return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; if (linVelObj || angVelObj) @@ -1737,12 +1759,12 @@ static PyObject* pybullet_resetBasePositionAndOrientation(PyObject* self, { return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; { b3SharedMemoryCommandHandle commandHandle; @@ -1828,12 +1850,12 @@ int physicsClientId = 0; { return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; { @@ -1909,12 +1931,12 @@ static PyObject* pybullet_getJointState(PyObject* self, PyObject* args,PyObject* { return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; { @@ -1994,12 +2016,12 @@ b3PhysicsClientHandle sm = 0; { return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; { { @@ -2097,12 +2119,12 @@ static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObj { return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; res = pybullet_internalSetVectord(textPositionObj,posXYZ); @@ -2168,13 +2190,12 @@ static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObj { return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; - res = pybullet_internalSetVectord(lineFromObj,fromXYZ); if (!res) @@ -2228,13 +2249,12 @@ static PyObject* pybullet_removeUserDebugItem(PyObject* self, PyObject* args, Py return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; - commandHandle = b3InitUserDebugDrawRemove(sm,itemUniqueId); @@ -2258,12 +2278,12 @@ static PyObject* pybullet_removeAllUserDebugItems(PyObject* self, PyObject* args return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; commandHandle = b3InitUserDebugDrawRemoveAll(sm); @@ -2294,12 +2314,12 @@ static PyObject* pybullet_rayTest(PyObject* self, PyObject* args, PyObject *keyw &rayFromObj, &rayToObj,&physicsClientId)) return NULL; - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; pybullet_internalSetVectord(rayFromObj,from); pybullet_internalSetVectord(rayToObj,to); @@ -2407,13 +2427,12 @@ static PyObject* pybullet_getVREvents(PyObject* self, PyObject* args, PyObject * return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; - commandHandle = b3RequestVREventsCommandInit(sm); @@ -2488,12 +2507,12 @@ static PyObject* pybullet_setDebugObjectColor(PyObject* self, PyObject* args, Py &objectUniqueId, &linkIndex, &objectColorRGBObj,&physicsClientId)) return NULL; - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; if (objectColorRGBObj) { @@ -2531,12 +2550,12 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyO { return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; { @@ -2634,13 +2653,12 @@ static PyObject* pybullet_resetVisualShapeData(PyObject* self, PyObject* args,Py { return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; - { @@ -2677,12 +2695,12 @@ static PyObject* pybullet_loadTexture(PyObject* self, PyObject* args, PyObject* { return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; { @@ -2823,12 +2841,12 @@ static PyObject* pybullet_getOverlappingObjects(PyObject* self, PyObject* args, &aabbMinOb, &aabbMaxOb,&physicsClientId)) return NULL; - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; pybullet_internalSetVectord(aabbMinOb, aabbMin); @@ -2890,12 +2908,12 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py &bodyUniqueIdA, &bodyUniqueIdB, &distanceThreshold, &linkIndexA, &linkIndexB,&physicsClientId)) return NULL; - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; commandHandle = b3InitClosestDistanceQuery(sm); b3SetClosestDistanceFilterBodyA(commandHandle, bodyUniqueIdA); @@ -2939,13 +2957,12 @@ static PyObject* pybullet_removeUserConstraint(PyObject* self, PyObject* args, P return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; - commandHandle = b3InitRemoveUserConstraintCommand(sm,userConstraintUniqueId); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); @@ -3017,13 +3034,12 @@ static PyObject* pybullet_createUserConstraint(PyObject* self, PyObject* args, P return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; - pybullet_internalSetVectord(jointAxisObj,jointAxis); pybullet_internalSetVectord(parentFramePositionObj,parentFramePosition); @@ -3091,14 +3107,12 @@ static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args, Py &bodyUniqueIdA, &bodyUniqueIdB,&physicsClientId)) return NULL; - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; - - commandHandle = b3InitRequestContactPointInformation(sm); b3SetContactFilterBodyA(commandHandle, bodyUniqueIdA); @@ -3150,12 +3164,12 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec { return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; command = b3InitRequestCameraImage(sm); b3RequestCameraImageSetPixelResolution(command, width, height); @@ -3478,21 +3492,15 @@ static PyObject* pybullet_renderImageObsolete(PyObject* self, PyObject* args) { // inialize cmd b3SharedMemoryCommandHandle command; + b3PhysicsClientHandle sm; int physicsClientId = 0; - b3PhysicsClientHandle sm=0; - if (0 == sPhysicsClients[physicsClientId]) + + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; - - - if (0 == sm) { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; - } - command = b3InitRequestCameraImage(sm); if (size == 2) // only set camera resolution @@ -3725,12 +3733,12 @@ static PyObject* pybullet_applyExternalForce(PyObject* self, PyObject* args,PyOb { return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; { PyObject* seq; int len, i; @@ -3793,12 +3801,12 @@ static PyObject* pybullet_applyExternalTorque(PyObject* self, PyObject* args,PyO { return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; { PyObject* seq; @@ -3982,12 +3990,12 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, { return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; { double pos[3]; double ori[4]={0,1.0,0,0}; @@ -4067,12 +4075,12 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, { return NULL; } - if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) + sm = getPhysicsClient(physicsClientId); + if (sm == 0) { PyErr_SetString(SpamError, "Not connected to physics server."); return NULL; } - sm = sPhysicsClients[physicsClientId]; { int szObPos = PySequence_Size(objPositionsQ); diff --git a/examples/pybullet/quadruped.py b/examples/pybullet/quadruped.py index 8431a59bd..a85ae0b1c 100644 --- a/examples/pybullet/quadruped.py +++ b/examples/pybullet/quadruped.py @@ -2,11 +2,15 @@ import pybullet as p import time import math -p.connect(p.SHARED_MEMORY) + +physId = p.connect(p.SHARED_MEMORY) +if (physId<0): + p.connect(p.GUI) + p.loadURDF("plane.urdf") p.setGravity(0,0,-1) p.setRealTimeSimulation(0) -quadruped = p.loadURDF("quadruped/quadruped.urdf",10,-2,2) +quadruped = p.loadURDF("quadruped/quadruped.urdf",1,-2,2) #p.getNumJoints(1) #right front leg p.resetJointState(quadruped,0,1.57) @@ -82,6 +86,7 @@ jump_amp = 0.5 #jump t_end = time.time() + 10 i=0 +t=0 while time.time() < t_end: t = time.time() p.setJointMotorControl(quadruped,0,p.POSITION_CONTROL,math.sin(t*speed)*jump_amp+1.57,p_gain) From ac02acc2f3cfe5b31aaadcd1fa244e7f5db13bfe Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 28 Dec 2016 22:10:03 -0800 Subject: [PATCH 3/5] fix compile issue (btMin requires same types, no conversion, just replace with < = combo) --- examples/StandaloneMain/main_opengl_single_example.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/examples/StandaloneMain/main_opengl_single_example.cpp b/examples/StandaloneMain/main_opengl_single_example.cpp index 0111460c3..b8ddbc734 100644 --- a/examples/StandaloneMain/main_opengl_single_example.cpp +++ b/examples/StandaloneMain/main_opengl_single_example.cpp @@ -96,12 +96,14 @@ int main(int argc, char* argv[]) do { app->m_instancingRenderer->init(); - app->m_instancingRenderer->updateCamera(app->getUpAxis()); + app->m_instancingRenderer->updateCamera(app->getUpAxis()); btScalar dtSec = btScalar(clock.getTimeInSeconds()); - dtSec = b3Min(dtSec,0.1); + if (dtSec<0.1) + dtSec = 0.1; + example->stepSimulation(dtSec); - clock.reset(); + clock.reset(); example->renderScene(); From d7d4fe5c457f7aa86b2b5f60435d234415dd5d85 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 28 Dec 2016 22:24:03 -0800 Subject: [PATCH 4/5] fix compile issue (use constructor) --- examples/OpenGLWindow/SimpleOpenGL3App.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/examples/OpenGLWindow/SimpleOpenGL3App.cpp b/examples/OpenGLWindow/SimpleOpenGL3App.cpp index aea665019..de192c5c1 100644 --- a/examples/OpenGLWindow/SimpleOpenGL3App.cpp +++ b/examples/OpenGLWindow/SimpleOpenGL3App.cpp @@ -325,10 +325,10 @@ void SimpleOpenGL3App::drawText3D( const char* txt, float worldPosX, float world 0,0,1,0, 0,0,0,1}; PrimVertex vertexData[4] = { - { PrimVec4(-1.f+2.f*x0/float(screenWidth), 1.f-2.f*y0/float(screenHeight), z, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v0)}, - { PrimVec4(-1.f+2.f*x0/float(screenWidth), 1.f-2.f*y1/float(screenHeight), z, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v1)}, - { PrimVec4( -1.f+2.f*x1/float(screenWidth), 1.f-2.f*y1/float(screenHeight), z, 1.f ), PrimVec4(color[0], color[1], color[2], color[3]) ,PrimVec2(u1,v1)}, - { PrimVec4( -1.f+2.f*x1/float(screenWidth), 1.f-2.f*y0/float(screenHeight), z, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u1,v0)} + PrimVertex(PrimVec4(-1.f+2.f*x0/float(screenWidth), 1.f-2.f*y0/float(screenHeight), z, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v0)), + PrimVertex(PrimVec4(-1.f+2.f*x0/float(screenWidth), 1.f-2.f*y1/float(screenHeight), z, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v1)), + PrimVertex(PrimVec4( -1.f+2.f*x1/float(screenWidth), 1.f-2.f*y1/float(screenHeight), z, 1.f ), PrimVec4(color[0], color[1], color[2], color[3]) ,PrimVec2(u1,v1)), + PrimVertex(PrimVec4( -1.f+2.f*x1/float(screenWidth), 1.f-2.f*y0/float(screenHeight), z, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u1,v0)) }; m_primRenderer->drawTexturedRect3D(vertexData[0],vertexData[1],vertexData[2],vertexData[3],identity,identity,false); From 7b9d194bfd38ff6b12e502e7caf3057ecc61c420 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 28 Dec 2016 22:38:48 -0800 Subject: [PATCH 5/5] fix compile issues --- examples/OpenGLWindow/GLPrimitiveRenderer.cpp | 32 +++++++++---------- examples/OpenGLWindow/SimpleOpenGL2App.cpp | 8 ++--- 2 files changed, 20 insertions(+), 20 deletions(-) diff --git a/examples/OpenGLWindow/GLPrimitiveRenderer.cpp b/examples/OpenGLWindow/GLPrimitiveRenderer.cpp index ba7c217d5..53bbca148 100644 --- a/examples/OpenGLWindow/GLPrimitiveRenderer.cpp +++ b/examples/OpenGLWindow/GLPrimitiveRenderer.cpp @@ -106,10 +106,10 @@ void GLPrimitiveRenderer::loadBufferData() { PrimVertex vertexData[4] = { - { PrimVec4(-1, -1, 0.0, 1.0 ), PrimVec4( 1.0, 0.0, 0.0, 1.0 ) ,PrimVec2(0,0)}, - { PrimVec4(-1, 1, 0.0, 1.0 ), PrimVec4( 0.0, 1.0, 0.0, 1.0 ) ,PrimVec2(0,1)}, - { PrimVec4( 1, 1, 0.0, 1.0 ), PrimVec4( 0.0, 0.0, 1.0, 1.0 ) ,PrimVec2(1,1)}, - { PrimVec4( 1, -1, 0.0, 1.0 ), PrimVec4( 1.0, 1.0, 1.0, 1.0 ) ,PrimVec2(1,0)} + PrimVertex( PrimVec4(-1, -1, 0.0, 1.0 ), PrimVec4( 1.0, 0.0, 0.0, 1.0 ) ,PrimVec2(0,0)), + PrimVertex( PrimVec4(-1, 1, 0.0, 1.0 ), PrimVec4( 0.0, 1.0, 0.0, 1.0 ) ,PrimVec2(0,1)), + PrimVertex( PrimVec4( 1, 1, 0.0, 1.0 ), PrimVec4( 0.0, 0.0, 1.0, 1.0 ) ,PrimVec2(1,1)), + PrimVertex( PrimVec4( 1, -1, 0.0, 1.0 ), PrimVec4( 1.0, 1.0, 1.0, 1.0 ) ,PrimVec2(1,0)) }; @@ -450,10 +450,10 @@ void GLPrimitiveRenderer::drawTexturedRect2a(float x0, float y0, float x1, float { PrimVertex vertexData[4] = { - { PrimVec4(-1.f+2.f*x0/float(m_screenWidth), 1.f-2.f*y0/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v0)}, - { PrimVec4(-1.f+2.f*x0/float(m_screenWidth), 1.f-2.f*y1/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v1)}, - { PrimVec4( -1.f+2.f*x1/float(m_screenWidth), 1.f-2.f*y1/float(m_screenHeight), 0.f, 1.f ), PrimVec4(color[0], color[1], color[2], color[3]) ,PrimVec2(u1,v1)}, - { PrimVec4( -1.f+2.f*x1/float(m_screenWidth), 1.f-2.f*y0/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u1,v0)} + PrimVertex( PrimVec4(-1.f+2.f*x0/float(m_screenWidth), 1.f-2.f*y0/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v0)), + PrimVertex( PrimVec4(-1.f+2.f*x0/float(m_screenWidth), 1.f-2.f*y1/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v1)), + PrimVertex(PrimVec4( -1.f+2.f*x1/float(m_screenWidth), 1.f-2.f*y1/float(m_screenHeight), 0.f, 1.f ), PrimVec4(color[0], color[1], color[2], color[3]) ,PrimVec2(u1,v1)), + PrimVertex( PrimVec4( -1.f+2.f*x1/float(m_screenWidth), 1.f-2.f*y0/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u1,v0)) }; int sz = m_data2->m_numVerticesText; @@ -486,10 +486,10 @@ void GLPrimitiveRenderer::drawTexturedRect2(float x0, float y0, float x1, float { PrimVertex vertexData[4] = { - { PrimVec4(-1.f+2.f*x0/float(m_screenWidth), 1.f-2.f*y0/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v0)}, - { PrimVec4(-1.f+2.f*x0/float(m_screenWidth), 1.f-2.f*y1/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v1)}, - { PrimVec4( -1.f+2.f*x1/float(m_screenWidth), 1.f-2.f*y1/float(m_screenHeight), 0.f, 1.f ), PrimVec4(color[0], color[1], color[2], color[3]) ,PrimVec2(u1,v1)}, - { PrimVec4( -1.f+2.f*x1/float(m_screenWidth), 1.f-2.f*y0/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u1,v0)} + PrimVertex( PrimVec4(-1.f+2.f*x0/float(m_screenWidth), 1.f-2.f*y0/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v0)), + PrimVertex( PrimVec4(-1.f+2.f*x0/float(m_screenWidth), 1.f-2.f*y1/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v1)), + PrimVertex( PrimVec4( -1.f+2.f*x1/float(m_screenWidth), 1.f-2.f*y1/float(m_screenHeight), 0.f, 1.f ), PrimVec4(color[0], color[1], color[2], color[3]) ,PrimVec2(u1,v1)), + PrimVertex( PrimVec4( -1.f+2.f*x1/float(m_screenWidth), 1.f-2.f*y0/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u1,v0)) }; int sz = m_data2->m_numVerticesText; @@ -516,10 +516,10 @@ void GLPrimitiveRenderer::drawTexturedRect(float x0, float y0, float x1, float y 0,0,1,0, 0,0,0,1}; PrimVertex vertexData[4] = { - { PrimVec4(-1.f+2.f*x0/float(m_screenWidth), 1.f-2.f*y0/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v0)}, - { PrimVec4(-1.f+2.f*x0/float(m_screenWidth), 1.f-2.f*y1/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v1)}, - { PrimVec4( -1.f+2.f*x1/float(m_screenWidth), 1.f-2.f*y1/float(m_screenHeight), 0.f, 1.f ), PrimVec4(color[0], color[1], color[2], color[3]) ,PrimVec2(u1,v1)}, - { PrimVec4( -1.f+2.f*x1/float(m_screenWidth), 1.f-2.f*y0/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u1,v0)} + PrimVertex( PrimVec4(-1.f+2.f*x0/float(m_screenWidth), 1.f-2.f*y0/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v0)), + PrimVertex( PrimVec4(-1.f+2.f*x0/float(m_screenWidth), 1.f-2.f*y1/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v1)), + PrimVertex( PrimVec4( -1.f+2.f*x1/float(m_screenWidth), 1.f-2.f*y1/float(m_screenHeight), 0.f, 1.f ), PrimVec4(color[0], color[1], color[2], color[3]) ,PrimVec2(u1,v1)), + PrimVertex( PrimVec4( -1.f+2.f*x1/float(m_screenWidth), 1.f-2.f*y0/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u1,v0)) }; drawTexturedRect3D(vertexData[0],vertexData[1],vertexData[2],vertexData[3],identity,identity,useRGBA); diff --git a/examples/OpenGLWindow/SimpleOpenGL2App.cpp b/examples/OpenGLWindow/SimpleOpenGL2App.cpp index 710b9fdab..bd491c782 100644 --- a/examples/OpenGLWindow/SimpleOpenGL2App.cpp +++ b/examples/OpenGLWindow/SimpleOpenGL2App.cpp @@ -420,10 +420,10 @@ void SimpleOpenGL2App::drawText3D( const char* txt, float worldPosX, float world 0,0,0,1}; */ PrimVertex vertexData[4] = { - { PrimVec4(-1.f+2.f*x0/float(screenWidth), 1.f-2.f*y0/float(screenHeight), z, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v0)}, - { PrimVec4(-1.f+2.f*x0/float(screenWidth), 1.f-2.f*y1/float(screenHeight), z, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v1)}, - { PrimVec4( -1.f+2.f*x1/float(screenWidth), 1.f-2.f*y1/float(screenHeight), z, 1.f ), PrimVec4(color[0], color[1], color[2], color[3]) ,PrimVec2(u1,v1)}, - { PrimVec4( -1.f+2.f*x1/float(screenWidth), 1.f-2.f*y0/float(screenHeight), z, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u1,v0)} + PrimVertex( PrimVec4(-1.f+2.f*x0/float(screenWidth), 1.f-2.f*y0/float(screenHeight), z, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v0)), + PrimVertex( PrimVec4(-1.f+2.f*x0/float(screenWidth), 1.f-2.f*y1/float(screenHeight), z, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v1)), + PrimVertex(PrimVec4( -1.f+2.f*x1/float(screenWidth), 1.f-2.f*y1/float(screenHeight), z, 1.f ), PrimVec4(color[0], color[1], color[2], color[3]) ,PrimVec2(u1,v1)), + PrimVertex( PrimVec4( -1.f+2.f*x1/float(screenWidth), 1.f-2.f*y0/float(screenHeight), z, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u1,v0)) }; glBegin(GL_TRIANGLES);