diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 014997f21..5a728b0cb 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -892,6 +892,43 @@ void b3RequestCameraImageSetProjectionMatrix(b3SharedMemoryCommandHandle command command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES; } +void b3RequestCameraImageSetFOVProjectionMatrix(b3SharedMemoryCommandHandle commandHandle, float fov, float aspect, float nearVal, float farVal) +{ + float yScale = 1.0 / tan((3.141592538 / 180.0) * fov / 2); + float xScale = yScale / aspect; + + float frustum[16]; + + frustum[0*4+0] = xScale; + frustum[0*4+1] = float(0); + frustum[0*4+2] = float(0); + frustum[0*4+3] = float(0); + + frustum[1*4+0] = float(0); + frustum[1*4+1] = yScale; + frustum[1*4+2] = float(0); + frustum[1*4+3] = float(0); + + frustum[2*4+0] = 0; + frustum[2*4+1] = 0; + frustum[2*4+2] = (nearVal + farVal) / (nearVal - farVal); + frustum[2*4+3] = float(-1); + + frustum[3*4+0] = float(0); + frustum[3*4+1] = float(0); + frustum[3*4+2] = (float(2) * farVal * nearVal) / (nearVal - farVal); + frustum[3*4+3] = float(0); + + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA); + for (int i=0;i<16;i++) + { + command->m_requestPixelDataArguments.m_projectionMatrix[i] = frustum[i]; + } + command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES; +} + void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle commandHandle, int width, int height ) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index c90015276..dcbe9c30c 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -72,6 +72,7 @@ b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physC void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle command, float viewMatrix[16], float projectionMatrix[16]); void b3RequestCameraImageSetViewMatrix(b3SharedMemoryCommandHandle command, const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3]); void b3RequestCameraImageSetProjectionMatrix(b3SharedMemoryCommandHandle command, float left, float right, float bottom, float top, float nearVal, float farVal); +void b3RequestCameraImageSetFOVProjectionMatrix(b3SharedMemoryCommandHandle command, float fov, float aspect, float nearVal, float farVal); void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle command, int width, int height ); void b3RequestCameraImageSelectRenderer(b3SharedMemoryCommandHandle commandHandle, int renderer); void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData); diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 58e4df4c2..12a7c6bba 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -981,6 +981,12 @@ static int pybullet_internalSetVector(PyObject* objMat, float vector[3]) // renderImage(w, h) - image resolution of (w,h), default camera // renderImage(w, h, view[16], projection[16]) - set both resolution // and initialize camera to the view and projection values +// renderImage(w, h, cameraPos, targetPos, cameraUp, nearVal, farVal) - set +// resolution and initialize camera based on camera position, target +// position, camera up and fulstrum near/far values. +// renderImage(w, h, cameraPos, targetPos, cameraUp, nearVal, farVal, fov) - +// set resolution and initialize camera based on camera position, target +// position, camera up, fulstrum near/far values and camera field of view. // // Note if the (w,h) is too small, the objects may not appear based on // where the camera has been set @@ -1006,8 +1012,7 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) float left, right, bottom, top, aspect; float nearVal, farVal; - // float nearVal = .001; - // float farVal = 1000; + float fov; // inialize cmd b3SharedMemoryCommandHandle command; @@ -1015,76 +1020,90 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args) if (0==sm) { - PyErr_SetString(SpamError, "Not connected to physics server."); - return NULL; + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; } command = b3InitRequestCameraImage(sm); if (size==2) // only set camera resolution { - if (PyArg_ParseTuple(args, "ii", &width, &height)) - { - b3RequestCameraImageSetPixelResolution(command,width,height); - } + if (PyArg_ParseTuple(args, "ii", &width, &height)) + { + b3RequestCameraImageSetPixelResolution(command,width,height); + } } - - if (size==4) // set camera resoluation and view and projection matrix + else if (size==4) // set camera resolution and view and projection matrix { if (PyArg_ParseTuple(args, "iiOO", &width, &height, &objViewMat, &objProjMat)) { b3RequestCameraImageSetPixelResolution(command,width,height); // set camera matrices only if set matrix function succeeds - if (pybullet_internalSetMatrix(objViewMat, viewMatrix) && - (pybullet_internalSetMatrix(objProjMat, projectionMatrix))) - { - b3RequestCameraImageSetCameraMatrices(command, viewMatrix, projectionMatrix); - } - } - } - - 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 (pybullet_internalSetMatrix(objViewMat, viewMatrix) && + (pybullet_internalSetMatrix(objProjMat, projectionMatrix))) + { + b3RequestCameraImageSetCameraMatrices(command, viewMatrix, projectionMatrix); + } + else + { + PyErr_SetString(SpamError, "Error parsing view or projection matrix."); + return NULL; } } - + } + else if (size==7) // set camera resolution, camera positions and calculate projection using near/far values. + { + if (PyArg_ParseTuple(args, "iiOOOff", &width, &height, &objCameraPos, &objTargetPos, &objCameraUp, &nearVal, &farVal)) + { + b3RequestCameraImageSetPixelResolution(command,width,height); + if (pybullet_internalSetVector(objCameraPos, cameraPos) && + pybullet_internalSetVector(objTargetPos, targetPos) && + pybullet_internalSetVector(objCameraUp, cameraUp)) + { + b3RequestCameraImageSetViewMatrix(command, cameraPos, targetPos, cameraUp); + } + else + { + PyErr_SetString(SpamError, "Error parsing camera position, target or up."); + return NULL; + } + + aspect = width/height; + left = -aspect * nearVal; + right = aspect * nearVal; + bottom = -nearVal; + top = nearVal; + b3RequestCameraImageSetProjectionMatrix(command, left, right, bottom, top, nearVal, farVal); + } + } + else if (size==8) // set camera resolution, camera positions and calculate projection using near/far values & field of view + { + if (PyArg_ParseTuple(args, "iiOOOfff", &width, &height, &objCameraPos, &objTargetPos, &objCameraUp, &nearVal, &farVal, &fov)) + { + b3RequestCameraImageSetPixelResolution(command,width,height); + if (pybullet_internalSetVector(objCameraPos, cameraPos) && + pybullet_internalSetVector(objTargetPos, targetPos) && + pybullet_internalSetVector(objCameraUp, cameraUp)) + { + b3RequestCameraImageSetViewMatrix(command, cameraPos, targetPos, cameraUp); + } + else + { + PyErr_SetString(SpamError, "Error parsing camera position, target or up."); + return NULL; + } + + aspect = width/height; + b3RequestCameraImageSetFOVProjectionMatrix(command, fov, aspect, nearVal, farVal); + } + } + else + { + PyErr_SetString(SpamError, "Invalid number of args passed to renderImage."); + return NULL; + } + if (b3CanSubmitCommand(sm)) { b3SharedMemoryStatusHandle statusHandle;