diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 771d1b16c..4212ed996 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -948,6 +948,32 @@ static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16]) return 0; } +// internal function to set a float vector[3] +// used to initialize camera position with +// a view and projection matrix in renderImage() +// +// // Args: +// matrix - float[16] which will be set by values from objMat +static int pybullet_internalSetVector(PyObject* objMat, float vector[3]) +{ + int i, len; + PyObject* seq; + + seq = PySequence_Fast(objMat, "expected a sequence"); + len = PySequence_Size(objMat); + if (len==3) + { + for (i = 0; i < len; i++) + { + vector[i] = pybullet_internalGetFloatFromSequence(seq,i); + } + Py_DECREF(seq); + return 1; + } + Py_DECREF(seq); + return 0; +} + // Render an image from the current timestep of the simulation // // Examples: @@ -967,10 +993,21 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) ///request an image from a simulated camera, using a software renderer. struct b3CameraImageData imageData; PyObject* objViewMat,* objProjMat; - int width, height; + PyObject* objCameraPos,*objTargetPos,* objCameraUp; + + int width, height; int size= PySequence_Size(args); float viewMatrix[16]; float projectionMatrix[16]; + + float cameraPos[3]; + float targetPos[3]; + float cameraUp[3]; + + float left, right, bottom, top, aspect; + float nearVal, farVal; + // float nearVal = .001; + // float farVal = 1000; // inialize cmd b3SharedMemoryCommandHandle command; @@ -1007,6 +1044,47 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) } } + if (size==7) // set camera resoluation and view and projection matrix + { + if (PyArg_ParseTuple(args, "iiOOOii", &width, &height, &objCameraPos, &objTargetPos, &objCameraUp, &nearVal, &farVal)) + { + b3RequestCameraImageSetPixelResolution(command,width,height); + if (pybullet_internalSetVector(objCameraPos, cameraPos) && + pybullet_internalSetVector(objTargetPos, targetPos) && + pybullet_internalSetVector(objCameraUp, cameraUp)) + { + // printf("\ncamera pos:\n"); + // for(int i =0;i<3; i++) { + // printf(" %f", cameraPos[i]); + // } + // + // printf("\ntargetPos pos:\n"); + // for(int i =0;i<3; i++) { + // printf(" %f", targetPos[i]); + // } + // + // printf("\ncameraUp pos:\n"); + // for(int i =0;i<3; i++) { + // printf(" %f", cameraUp[i]); + // } + // printf("\n"); + b3RequestCameraImageSetViewMatrix(command, cameraPos, targetPos, cameraUp); + // printf("\n"); + + } + + aspect = width/height; + left = -aspect * nearVal; + right = aspect * nearVal; + bottom = -nearVal; + top = nearVal; + + b3RequestCameraImageSetProjectionMatrix(command, left, right, bottom, top, nearVal, farVal); + // printf("\n"); + + } + } + if (b3CanSubmitCommand(sm)) { b3SharedMemoryStatusHandle statusHandle; @@ -1387,7 +1465,7 @@ static PyMethodDef SpamMethods[] = { "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"}, {"renderImage", pybullet_renderImage, METH_VARARGS, - "Render an image (given the pixel resolution width & height and camera view & projection matrices), and return the 8-8-8bit RGB pixel data and floating point depth values"}, + "Render an image (given the pixel resolution width, height, camera view matrix, projection matrix, near, and far values), and return the 8-8-8bit RGB pixel data and floating point depth values"}, {"getQuaternionFromEuler", pybullet_getQuaternionFromEuler, METH_VARARGS, "Convert Euler [roll, pitch, yaw] as in URDF/SDF convention, to quaternion [x,y,z,w]"}, @@ -1402,6 +1480,10 @@ static PyMethodDef SpamMethods[] = { ////todo(erwincoumans) //collision info //raycast info + + //applyBaseForce + //applyLinkForce + {NULL, NULL, 0, NULL} /* Sentinel */ };