add one more pybullet renderImage API and testrender.py example

tweak Bullet Inverse Dynamics, work-around compiler issue

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -2,6 +2,8 @@
 #include "PhysicsClientSharedMemory.h"
 #include "Bullet3Common/b3Scalar.h"
 #include "Bullet3Common/b3Vector3.h"
+#include "Bullet3Common/b3Matrix3x3.h"
+
 #include <string.h>
 #include "SharedMemoryCommands.h"
 
@@ -823,6 +825,66 @@ void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle commandHa
 	command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES;
 }
 
+void b3RequestCameraImageSetViewMatrix2(b3SharedMemoryCommandHandle commandHandle, const float cameraTargetPosition[3], float distance, float yaw, float pitch, int upAxis)
+{
+    struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+    b3Assert(command);
+    b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
+    b3Vector3 camUpVector;
+    b3Vector3 camForward;
+    b3Vector3 camPos;
+    b3Vector3 camTargetPos = b3MakeVector3(cameraTargetPosition[0],cameraTargetPosition[1],cameraTargetPosition[2]);
+    
+	int forwardAxis(-1);
+	switch (upAxis)
+	{
+    case 1:
+    	forwardAxis = 2;
+    	camUpVector = b3MakeVector3(0,1,0);
+    	//gLightPos = b3MakeVector3(-50.f,100,30);
+    	break;
+    case 2:
+		forwardAxis = 1;
+		camUpVector = b3MakeVector3(0,0,1);
+		//gLightPos = b3MakeVector3(-50.f,30,100);
+		break;
+    default:
+		{
+			//b3Assert(0);
+			return;
+		}
+	};
+
+	b3Vector3 eyePos = b3MakeVector3(0,0,0);
+	eyePos[forwardAxis] = -distance;
+
+	camForward = b3MakeVector3(eyePos[0],eyePos[1],eyePos[2]);
+	if (camForward.length2() < B3_EPSILON)
+	{
+		camForward.setValue(1.f,0.f,0.f);
+	} else
+	{
+		camForward.normalize();
+	}
+
+	b3Scalar rele = yaw * b3Scalar(0.01745329251994329547);// rads per deg
+	b3Scalar razi = pitch * b3Scalar(0.01745329251994329547);// rads per deg
+	b3Quaternion rot(camUpVector,razi);
+	
+	b3Vector3 right = camUpVector.cross(camForward);
+	b3Quaternion roll(right,-rele);
+
+	eyePos = b3Matrix3x3(rot) * b3Matrix3x3(roll) * eyePos;
+	camPos = eyePos;
+	camPos += camTargetPos;
+	
+    float camPosf[4] = {camPos[0],camPos[1],camPos[2],0};
+    float camPosTargetf[4] = {camTargetPos[0],camTargetPos[1],camTargetPos[2],0};
+    float camUpf[4] = {camUpVector[0],camUpVector[1],camUpVector[2],0};
+    
+    b3RequestCameraImageSetViewMatrix(commandHandle,camPosf,camPosTargetf,camUpf);
+    
+}
 void b3RequestCameraImageSetViewMatrix(b3SharedMemoryCommandHandle commandHandle, const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3])
 {
     b3Vector3 eye = b3MakeVector3(cameraPosition[0], cameraPosition[1], cameraPosition[2]);

examples/SharedMemory/PhysicsClientC_API.h

@@ -71,6 +71,7 @@ void    b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* l
 b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient);
 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 b3RequestCameraImageSetViewMatrix2(b3SharedMemoryCommandHandle commandHandle, const float cameraTargetPosition[3], float distance, float yaw, float pitch, int upAxis);
 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 );

examples/pybullet/pybullet.c

@@ -1041,6 +1041,8 @@ static int pybullet_internalSetVector(PyObject* objMat, float vector[3])
 //  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.
+//  renderImage(w, h, targetPos, distance, yaw, pitch, upAxisIndex, nearVal, farVal, fov)
+
 //
 // Note if the (w,h) is too small, the objects may not appear based on
 // where the camera has been set
@@ -1151,6 +1153,35 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args)
       aspect = width/height;
       b3RequestCameraImageSetFOVProjectionMatrix(command, fov, aspect, nearVal, farVal);
     }
+  }
+  else if (size==10)
+  {
+      int upAxisIndex=1;
+      float camDistance,yaw,pitch;
+      
+      //sometimes more arguments are better :-)
+       if (PyArg_ParseTuple(args, "iiOfffifff", &width, &height, &objTargetPos, &camDistance, &yaw, &pitch, &upAxisIndex, &nearVal, &farVal, &fov))
+       {
+       		
+            if (pybullet_internalSetVector(objTargetPos, targetPos))
+            {
+            		printf("width = %d, height = %d, targetPos = %f,%f,%f, distance = %f, yaw = %f, pitch = %f,	upAxisIndex = %d, near=%f, far=%f, fov=%f\n",width,height,targetPos[0],targetPos[1],targetPos[2],camDistance,yaw,pitch,upAxisIndex,nearVal,farVal,fov);
+            	
+                b3RequestCameraImageSetViewMatrix2(command,targetPos,camDistance,yaw,pitch,upAxisIndex);
+                aspect = width/height;
+                b3RequestCameraImageSetFOVProjectionMatrix(command, fov, aspect, nearVal, farVal);
+            } else
+            {
+                PyErr_SetString(SpamError, "Error parsing camera target pos");
+            }
+       } else
+       {
+            PyErr_SetString(SpamError, "Error parsing arguments");
+       }
+      
+      
+      
+      
   }
   else
   {

examples/pybullet/testrender.py

@@ -0,0 +1,40 @@
+import numpy as np
+import matplotlib.pyplot as plt
+import pybullet
+
+pybullet.connect(pybullet.GUI)
+pybullet.loadURDF("r2d2.urdf")
+
+camTargetPos = [0,0,0]
+#cameraUp = [0,0,1]
+cameraPos = [3,3,3]
+yaw = 40.0
+pitch = 0.0
+upAxisIndex = 1
+camDistance = 3
+pixelWidth = 640
+pixelHeight = 480
+nearPlane = 0.01
+farPlane = 1000
+
+fov = 60
+
+#img_arr = pybullet.renderImage(pixelWidth, pixelHeight)
+#renderImage(w, h, view[16], projection[16])
+#img_arr = pybullet.renderImage(pixelWidth, pixelHeight, cameraPos, camTargetPos, cameraUp, nearPlane, farPlane)
+img_arr = pybullet.renderImage(pixelWidth, pixelHeight, camTargetPos, camDistance, yaw, pitch, upAxisIndex, nearPlane, farPlane, fov)
+
+w=img_arr[0] #width of the image, in pixels
+h=img_arr[1] #height of the image, in pixels
+rgb=img_arr[2] #color data RGB
+dep=img_arr[3] #depth data
+
+
+# reshape creates np array
+np_img_arr = np.reshape(rgb, (pixelHeight, pixelWidth, 4))
+np_img_arr = np_img_arr*(1./255.)
+
+#show
+plt.imshow(np_img_arr,interpolation='none')
+plt.show()
+p.resetSimulation()

src/BulletInverseDynamics/details/IDLinearMathInterface.hpp

@@ -14,6 +14,7 @@ class vec3;
 class vecx;
 class mat33;
 typedef btMatrixX<idScalar> matxx;
+typedef btVectorX<idScalar> vecxx;
 
 class vec3 : public btVector3 {
 public:
@@ -46,11 +47,11 @@ inline mat33 operator/(const mat33& a, const idScalar& s) { return a * (1.0 / s)
 
 inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; }
 
-class vecx : public btVectorX<idScalar> {
+class vecx : public vecxx {
 public:
-	vecx(int size) : btVectorX<idScalar>(size) {}
-	const vecx& operator=(const btVectorX<idScalar>& rhs) {
-		*static_cast<btVectorX<idScalar>*>(this) = rhs;
+	vecx(int size) : vecxx(size) {}
+	const vecx& operator=(const vecxx& rhs) {
+		*static_cast<vecxx*>(this) = rhs;
 		return *this;
 	}