implement pybullet.getDebugVisualizerCamera, width, height, providing viewmatrix, projection matrix

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;

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], 

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, 

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, 

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];
+	//float upf[3];
-	m_data->m_activeCamera->getCameraUpVector(upf);
+	//m_data->m_activeCamera->getCameraUpVector(upf);
-	b3Vector3 up = b3MakeVector3(upf[0],upf[1],upf[2]);
+	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);
 

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]);
+	if (m_data->m_enableVR)
-	up[1] = float(m_data->m_cameraUp[1]);
+	{
-	up[2] = float(m_data->m_cameraUp[2]);
+		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)
 {

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;

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;

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);

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);

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");

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");

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;

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;
 	};
 };
 

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
 {

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."},