From bdf366b0458e6018030a8e531cc8deb6029c8c9a Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 10 Apr 2017 11:03:41 -0700 Subject: [PATCH] implement pybullet.getDebugVisualizerCamera, width, height, providing viewmatrix, projection matrix --- .../CommonInterfaces/CommonCameraInterface.h | 2 + .../CommonGUIHelperInterface.h | 12 ++- examples/ExampleBrowser/OpenGLGuiHelper.cpp | 56 +++++++++++++ examples/ExampleBrowser/OpenGLGuiHelper.h | 4 +- .../OpenGLWindow/GLInstancingRenderer.cpp | 8 +- examples/OpenGLWindow/SimpleCamera.cpp | 34 +++++++- examples/OpenGLWindow/SimpleCamera.h | 4 + examples/SharedMemory/PhysicsClientC_API.cpp | 27 +++++++ examples/SharedMemory/PhysicsClientC_API.h | 3 + .../PhysicsClientSharedMemory.cpp | 10 +++ examples/SharedMemory/PhysicsDirect.cpp | 10 +++ .../PhysicsServerCommandProcessor.cpp | 18 +++++ .../SharedMemory/PhysicsServerExample.cpp | 6 ++ examples/SharedMemory/SharedMemoryCommands.h | 2 +- examples/SharedMemory/SharedMemoryPublic.h | 16 ++++ examples/pybullet/pybullet.c | 81 +++++++++++++++++++ 16 files changed, 284 insertions(+), 9 deletions(-) diff --git a/examples/CommonInterfaces/CommonCameraInterface.h b/examples/CommonInterfaces/CommonCameraInterface.h index 88db58685..87dc2f9d8 100644 --- a/examples/CommonInterfaces/CommonCameraInterface.h +++ b/examples/CommonInterfaces/CommonCameraInterface.h @@ -24,6 +24,8 @@ struct CommonCameraInterface virtual void setCameraUpVector(float x,float y, float z) = 0; virtual void getCameraUpVector(float up[3]) const = 0; + virtual void getCameraForwardVector(float fwd[3]) const = 0; + ///the setCameraUpAxis will call the 'setCameraUpVector' and 'setCameraForwardVector' virtual void setCameraUpAxis(int axis) = 0; virtual int getCameraUpAxis() const = 0; diff --git a/examples/CommonInterfaces/CommonGUIHelperInterface.h b/examples/CommonInterfaces/CommonGUIHelperInterface.h index 493b9e123..fd34803d1 100644 --- a/examples/CommonInterfaces/CommonGUIHelperInterface.h +++ b/examples/CommonInterfaces/CommonGUIHelperInterface.h @@ -44,12 +44,22 @@ struct GUIHelperInterface virtual CommonRenderInterface* getRenderInterface()=0; + virtual const CommonRenderInterface* getRenderInterface() const + { + return 0; + } + virtual CommonGraphicsApp* getAppInterface()=0; virtual void setUpAxis(int axis)=0; virtual void resetCamera(float camDist, float pitch, float yaw, float camPosX,float camPosY, float camPosZ)=0; - + + virtual bool getCameraInfo(int* width, int* height, float viewMatrix[16], float projectionMatrix[16], float camUp[3], float camForward[3],float hor[3], float vert[3] ) const + { + return false; + } + virtual void setVisualizerFlag(int flag, int enable){}; virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.cpp b/examples/ExampleBrowser/OpenGLGuiHelper.cpp index ccaa11113..7a66ca067 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.cpp +++ b/examples/ExampleBrowser/OpenGLGuiHelper.cpp @@ -190,6 +190,11 @@ struct CommonRenderInterface* OpenGLGuiHelper::getRenderInterface() return m_data->m_glApp->m_renderer; } +const struct CommonRenderInterface* OpenGLGuiHelper::getRenderInterface() const +{ + return m_data->m_glApp->m_renderer; +} + void OpenGLGuiHelper::createRigidBodyGraphicsObject(btRigidBody* body, const btVector3& color) { createCollisionObjectGraphicsObject(body,color); @@ -386,6 +391,57 @@ void OpenGLGuiHelper::resetCamera(float camDist, float pitch, float yaw, float c } } +bool OpenGLGuiHelper::getCameraInfo(int* width, int* height, float viewMatrix[16], float projectionMatrix[16], float camUp[3], float camForward[3],float hor[3], float vert[3] ) const +{ + if (getRenderInterface() && getRenderInterface()->getActiveCamera()) + { + *width = m_data->m_glApp->m_window->getWidth()*m_data->m_glApp->m_window->getRetinaScale(); + *height = m_data->m_glApp->m_window->getHeight()*m_data->m_glApp->m_window->getRetinaScale(); + getRenderInterface()->getActiveCamera()->getCameraViewMatrix(viewMatrix); + getRenderInterface()->getActiveCamera()->getCameraProjectionMatrix(projectionMatrix); + getRenderInterface()->getActiveCamera()->getCameraUpVector(camUp); + getRenderInterface()->getActiveCamera()->getCameraForwardVector(camForward); + float frustumNearPlane = getRenderInterface()->getActiveCamera()->getCameraFrustumNear(); + float frustumFarPlane = getRenderInterface()->getActiveCamera()->getCameraFrustumFar(); + + float top = 1.f; + float bottom = -1.f; + float tanFov = (top-bottom)*0.5f / frustumNearPlane; + float fov = btScalar(2.0) * btAtan(tanFov); + btVector3 camPos,camTarget; + getRenderInterface()->getActiveCamera()->getCameraPosition(camPos); + getRenderInterface()->getActiveCamera()->getCameraTargetPosition(camTarget); + btVector3 rayFrom = camPos; + btVector3 rayForward = (camTarget-camPos); + rayForward.normalize(); + float farPlane = 10000.f; + rayForward*= farPlane; + + btVector3 rightOffset; + btVector3 cameraUp=btVector3(camUp[0],camUp[1],camUp[2]); + btVector3 vertical = cameraUp; + btVector3 hori; + hori = rayForward.cross(vertical); + hori.normalize(); + vertical = hori.cross(rayForward); + vertical.normalize(); + float tanfov = tanf(0.5f*fov); + hori *= 2.f * farPlane * tanfov; + vertical *= 2.f * farPlane * tanfov; + btScalar aspect = *width / *height; + hori*=aspect; + //compute 'hor' and 'vert' vectors, useful to generate raytracer rays + hor[0] = hori[0]; + hor[1] = hori[1]; + hor[2] = hori[2]; + vert[0] = vertical[0]; + vert[1] = vertical[1]; + vert[2] = vertical[2]; + return true; + } + return false; +} + void OpenGLGuiHelper::copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.h b/examples/ExampleBrowser/OpenGLGuiHelper.h index 0e91f0f3b..22adc39c1 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.h +++ b/examples/ExampleBrowser/OpenGLGuiHelper.h @@ -15,6 +15,7 @@ struct OpenGLGuiHelper : public GUIHelperInterface virtual ~OpenGLGuiHelper(); virtual struct CommonRenderInterface* getRenderInterface(); + virtual const struct CommonRenderInterface* getRenderInterface() const; virtual void createRigidBodyGraphicsObject(btRigidBody* body, const btVector3& color); @@ -44,7 +45,8 @@ struct OpenGLGuiHelper : public GUIHelperInterface virtual void resetCamera(float camDist, float pitch, float yaw, float camPosX,float camPosY, float camPosZ); - + virtual bool getCameraInfo(int* width, int* height, float viewMatrix[16], float projectionMatrix[16], float camUp[3], float camForward[3],float hor[3], float vert[3] ) const; + virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, diff --git a/examples/OpenGLWindow/GLInstancingRenderer.cpp b/examples/OpenGLWindow/GLInstancingRenderer.cpp index d5262db8b..31e30527f 100644 --- a/examples/OpenGLWindow/GLInstancingRenderer.cpp +++ b/examples/OpenGLWindow/GLInstancingRenderer.cpp @@ -1551,9 +1551,11 @@ void GLInstancingRenderer::renderSceneInternal(int renderMode) b3CreateOrtho(-shadowMapWorldSize,shadowMapWorldSize,-shadowMapWorldSize,shadowMapWorldSize,1,300,depthProjectionMatrix);//-14,14,-14,14,1,200, depthProjectionMatrix); float depthViewMatrix[4][4]; b3Vector3 center = b3MakeVector3(0,0,0); - float upf[3]; - m_data->m_activeCamera->getCameraUpVector(upf); - b3Vector3 up = b3MakeVector3(upf[0],upf[1],upf[2]); + //float upf[3]; + //m_data->m_activeCamera->getCameraUpVector(upf); + b3Vector3 up, fwd; + b3PlaneSpace1(gLightPos,up,fwd); +// b3Vector3 up = b3MakeVector3(upf[0],upf[1],upf[2]); b3CreateLookAt(gLightPos,center,up,&depthViewMatrix[0][0]); //b3CreateLookAt(lightPos,m_data->m_cameraTargetPosition,b3Vector3(0,1,0),(float*)depthModelViewMatrix2); diff --git a/examples/OpenGLWindow/SimpleCamera.cpp b/examples/OpenGLWindow/SimpleCamera.cpp index 93838a212..697f7ce65 100644 --- a/examples/OpenGLWindow/SimpleCamera.cpp +++ b/examples/OpenGLWindow/SimpleCamera.cpp @@ -68,6 +68,8 @@ void SimpleCamera::setVRCamera(const float viewMat[16], const float projectionMa { m_data->m_viewMatrixVR[i] = viewMat[i]; m_data->m_projectionMatrixVR[i] = projectionMatrix[i]; + m_data->m_frustumZNear = m_data->m_projectionMatrixVR[14]/(m_data->m_projectionMatrixVR[10]-1); + m_data->m_frustumZFar = m_data->m_projectionMatrixVR[14]/(m_data->m_projectionMatrixVR[10]+1); } } @@ -346,11 +348,37 @@ void SimpleCamera::setCameraUpVector(float x,float y ,float z) void SimpleCamera::getCameraUpVector(float up[3]) const { - up[0] = float(m_data->m_cameraUp[0]); - up[1] = float(m_data->m_cameraUp[1]); - up[2] = float(m_data->m_cameraUp[2]); + if (m_data->m_enableVR) + { + float viewMatTotal[16]; + getCameraViewMatrix(viewMatTotal); + up[0] = viewMatTotal[0]; + up[1] = viewMatTotal[4]; + up[2] = viewMatTotal[8]; + } else + { + up[0] = float(m_data->m_cameraUp[0]); + up[1] = float(m_data->m_cameraUp[1]); + up[2] = float(m_data->m_cameraUp[2]); + } } +void SimpleCamera::getCameraForwardVector(float fwd[3]) const +{ + if (m_data->m_enableVR) + { + float viewMatTotal[16]; + getCameraViewMatrix(viewMatTotal); + fwd[0] = viewMatTotal[2]; + fwd[1] = viewMatTotal[6]; + fwd[2] = viewMatTotal[10]; + } else + { + fwd[0] = float(m_data->m_cameraForward[0]); + fwd[1] = float(m_data->m_cameraForward[1]); + fwd[2] = float(m_data->m_cameraForward[2]); + } +} void SimpleCamera::setCameraYaw(float yaw) { diff --git a/examples/OpenGLWindow/SimpleCamera.h b/examples/OpenGLWindow/SimpleCamera.h index b186759c8..7fbdaae8c 100644 --- a/examples/OpenGLWindow/SimpleCamera.h +++ b/examples/OpenGLWindow/SimpleCamera.h @@ -35,6 +35,10 @@ struct SimpleCamera : public CommonCameraInterface virtual void setCameraUpVector(float x,float y, float z); void getCameraUpVector(float up[3]) const; + + void getCameraForwardVector(float fwd[3]) const; + + ///the setCameraUpAxis will call the 'setCameraUpVector' and 'setCameraForwardVector' virtual void setCameraUpAxis(int axis); virtual int getCameraUpAxis() const; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 154547836..94bbedc36 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -2843,6 +2843,33 @@ void b3ConfigureOpenGLVisualizerSetViewMatrix(b3SharedMemoryCommandHandle comman } } +b3SharedMemoryCommandHandle b3InitRequestOpenGLVisualizerCameraCommand(b3PhysicsClientHandle physClient) +{ + PhysicsClient* cl = (PhysicsClient*)physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + + command->m_type = CMD_REQUEST_OPENGL_VISUALIZER_CAMERA; + command->m_updateFlags = 0; + + return (b3SharedMemoryCommandHandle)command; + +} + +int b3GetStatusOpenGLVisualizerCamera(b3SharedMemoryStatusHandle statusHandle, b3OpenGLVisualizerCameraInfo* camera) +{ + const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; + //b3Assert(status); + if (status && status->m_type == CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_COMPLETED) + { + *camera = status->m_visualizerCameraResultArgs; + return 1; + } + return 0; +} + void b3SetTimeOut(b3PhysicsClientHandle physClient, double timeOutInSeconds) { PhysicsClient* cl = (PhysicsClient*)physClient; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index e313d18e6..dcc210760 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -104,6 +104,9 @@ b3SharedMemoryCommandHandle b3InitConfigureOpenGLVisualizer(b3PhysicsClientHandl void b3ConfigureOpenGLVisualizerSetVisualizationFlags(b3SharedMemoryCommandHandle commandHandle, int flag, int enabled); void b3ConfigureOpenGLVisualizerSetViewMatrix(b3SharedMemoryCommandHandle commandHandle, float cameraDistance, float cameraPitch, float cameraYaw, const float cameraTargetPosition[3]); +b3SharedMemoryCommandHandle b3InitRequestOpenGLVisualizerCameraCommand(b3PhysicsClientHandle physClient); +int b3GetStatusOpenGLVisualizerCamera(b3SharedMemoryStatusHandle statusHandle, struct b3OpenGLVisualizerCameraInfo* camera); + /// Add/remove user-specific debug lines and debug text messages b3SharedMemoryCommandHandle b3InitUserDebugDrawAddLine3D(b3PhysicsClientHandle physClient, double fromXYZ[3], double toXYZ[3], double colorRGB[3], double lineWidth, double lifeTime); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index fba7ab366..fdd48a1b3 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -990,6 +990,16 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { b3Warning("State Logging failed"); break; } + case CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_FAILED: + { + b3Warning("Request visualizer camera failed"); + break; + } + case CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_COMPLETED: + { + break; + } + default: { b3Error("Unknown server status %d\n", serverCmd.m_type); btAssert(0); diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index cb2c62e68..92f53e648 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -834,6 +834,16 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd break; } + case CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_COMPLETED: + { + break; + } + + case CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_FAILED: + { + b3Warning("requestOpenGLVisualizeCamera failed"); + break; + } case CMD_REMOVE_USER_CONSTRAINT_FAILED: { b3Warning("removeConstraint failed"); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 436a2f5b2..5b15506cf 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -4186,6 +4186,24 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm break; } + case CMD_REQUEST_OPENGL_VISUALIZER_CAMERA: + { + BT_PROFILE("CMD_REQUEST_OPENGL_VISUALIZER_CAMERA"); + SharedMemoryStatus& serverCmd = serverStatusOut; + bool result = this->m_data->m_guiHelper->getCameraInfo( + &serverCmd.m_visualizerCameraResultArgs.m_width, + &serverCmd.m_visualizerCameraResultArgs.m_height, + serverCmd.m_visualizerCameraResultArgs.m_viewMatrix, + serverCmd.m_visualizerCameraResultArgs.m_projectionMatrix, + serverCmd.m_visualizerCameraResultArgs.m_camUp, + serverCmd.m_visualizerCameraResultArgs.m_camForward, + serverCmd.m_visualizerCameraResultArgs.m_horizontal, + serverCmd.m_visualizerCameraResultArgs.m_vertical); + serverCmd.m_type = result ? CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_COMPLETED: CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_FAILED; + hasStatus = true; + break; + } + case CMD_CONFIGURE_OPENGL_VISUALIZER: { BT_PROFILE("CMD_CONFIGURE_OPENGL_VISUALIZER"); diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index ea7a8be8b..05a38169a 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -808,6 +808,12 @@ public: m_childGuiHelper->resetCamera(camDist,pitch,yaw,camPosX,camPosY,camPosZ); } + virtual bool getCameraInfo(int* width, int* height, float viewMatrix[16], float projectionMatrix[16], float camUp[3], float camForward[3],float hor[3], float vert[3] ) const + { + return m_childGuiHelper->getCameraInfo(width,height,viewMatrix,projectionMatrix,camUp,camForward,hor,vert); + } + + float m_viewMatrix[16]; float m_projectionMatrix[16]; unsigned char* m_pixelsRGBA; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 2fbda032c..f85a62d80 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -790,7 +790,7 @@ struct SharedMemoryStatus struct SendKeyboardEvents m_sendKeyboardEvents; struct SendRaycastHits m_raycastHits; struct StateLoggingResultArgs m_stateLoggingResultArgs; - + struct b3OpenGLVisualizerCameraInfo m_visualizerCameraResultArgs; }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 81c997d2b..ab906172e 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -54,6 +54,7 @@ enum EnumSharedMemoryClientCommand CMD_STATE_LOGGING, CMD_CONFIGURE_OPENGL_VISUALIZER, CMD_REQUEST_KEYBOARD_EVENTS_DATA, + CMD_REQUEST_OPENGL_VISUALIZER_CAMERA, //don't go beyond this command! CMD_MAX_CLIENT_COMMANDS, @@ -133,6 +134,8 @@ enum EnumSharedMemoryServerStatus CMD_STATE_LOGGING_FAILED, CMD_REQUEST_KEYBOARD_EVENTS_DATA_COMPLETED, CMD_REQUEST_KEYBOARD_EVENTS_DATA_FAILED, + CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_FAILED, + CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_COMPLETED, //don't go beyond 'CMD_MAX_SERVER_COMMANDS! CMD_MAX_SERVER_COMMANDS }; @@ -248,6 +251,19 @@ struct b3CameraImageData const int* m_segmentationMaskValues;//m_pixelWidth*m_pixelHeight ints }; +struct b3OpenGLVisualizerCameraInfo +{ + int m_width; + int m_height; + float m_viewMatrix[16]; + float m_projectionMatrix[16]; + + float m_camUp[3]; + float m_camForward[3]; + + float m_horizontal[3]; + float m_vertical[3]; +}; enum b3VREventType { diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index cba2c39b3..a8d04318b 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -3246,6 +3246,84 @@ static PyObject* pybullet_getVREvents(PyObject* self, PyObject* args, PyObject* return Py_None; } +static PyObject* pybullet_getDebugVisualizerCamera(PyObject* self, PyObject* args, PyObject* keywds) +{ + + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + static char* kwlist[] = {"physicsClientId", NULL}; + b3SharedMemoryCommandHandle commandHandle; + int hasCamInfo; + b3SharedMemoryStatusHandle statusHandle; + struct b3OpenGLVisualizerCameraInfo camera; + PyObject* pyCameraList =0; + + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + commandHandle = b3InitRequestOpenGLVisualizerCameraCommand(sm); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + + hasCamInfo = b3GetStatusOpenGLVisualizerCamera(statusHandle, &camera); + if (hasCamInfo) + { + PyObject* item=0; + pyCameraList = PyTuple_New(8); + item = PyInt_FromLong(camera.m_width); + PyTuple_SetItem(pyCameraList,0,item); + item = PyInt_FromLong(camera.m_height); + PyTuple_SetItem(pyCameraList,1,item); + { + PyObject* viewMat16 = PyTuple_New(16); + PyObject* projMat16 = PyTuple_New(16); + int i; + + for (i=0;i<16;i++) + { + item = PyFloat_FromDouble(camera.m_viewMatrix[i]); + PyTuple_SetItem(viewMat16,i,item); + item = PyFloat_FromDouble(camera.m_projectionMatrix[i]); + PyTuple_SetItem(projMat16,i,item); + } + PyTuple_SetItem(pyCameraList,2,viewMat16); + PyTuple_SetItem(pyCameraList,3,projMat16); + } + + { + PyObject* item=0; + int i; + PyObject* camUp = PyTuple_New(3); + PyObject* camFwd = PyTuple_New(3); + PyObject* hor = PyTuple_New(3); + PyObject* vert= PyTuple_New(3); + for (i=0;i<3;i++) + { + item = PyFloat_FromDouble(camera.m_camUp[i]); + PyTuple_SetItem(camUp,i,item); + item = PyFloat_FromDouble(camera.m_camForward[i]); + PyTuple_SetItem(camFwd,i,item); + item = PyFloat_FromDouble(camera.m_horizontal[i]); + PyTuple_SetItem(hor,i,item); + item = PyFloat_FromDouble(camera.m_vertical[i]); + PyTuple_SetItem(vert,i,item); + } + PyTuple_SetItem(pyCameraList,4,camUp); + PyTuple_SetItem(pyCameraList,5,camFwd); + PyTuple_SetItem(pyCameraList,6,hor); + PyTuple_SetItem(pyCameraList,7,vert); + } + return pyCameraList; + } + + PyErr_SetString(SpamError, "Cannot get OpenGL visualizer camera info."); + return NULL; + +} + static PyObject* pybullet_configureDebugVisualizer(PyObject* self, PyObject* args, PyObject* keywds) { int flag = 1; @@ -5427,6 +5505,9 @@ static PyMethodDef SpamMethods[] = { "Override the wireframe debug drawing color for a particular object unique id / link index." "If you ommit the color, the custom color will be removed."}, + {"getDebugVisualizerCamera", (PyCFunction)pybullet_getDebugVisualizerCamera, METH_VARARGS | METH_KEYWORDS, + "Get information about the 3D visualizer camera, such as width, height, view matrix, projection matrix etc."}, + {"configureDebugVisualizer", (PyCFunction)pybullet_configureDebugVisualizer, METH_VARARGS | METH_KEYWORDS, "For the 3D OpenGL Visualizer, enable/disable GUI, shadows."},