diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 4197544b2..23dc57826 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/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 #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]); diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 2e2f126e2..588f810a8 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/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 ); diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index d2b7a496c..512956098 100644 --- a/examples/pybullet/pybullet.c +++ b/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 { diff --git a/examples/pybullet/testrender.py b/examples/pybullet/testrender.py new file mode 100644 index 000000000..7ba07e4e7 --- /dev/null +++ b/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() diff --git a/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp b/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp index d81647b68..53f88c4b7 100644 --- a/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp +++ b/src/BulletInverseDynamics/details/IDLinearMathInterface.hpp @@ -14,6 +14,7 @@ class vec3; class vecx; class mat33; typedef btMatrixX matxx; +typedef btVectorX 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 { +class vecx : public vecxx { public: - vecx(int size) : btVectorX(size) {} - const vecx& operator=(const btVectorX& rhs) { - *static_cast*>(this) = rhs; + vecx(int size) : vecxx(size) {} + const vecx& operator=(const vecxx& rhs) { + *static_cast(this) = rhs; return *this; }