add pybullet render API with yaw/pitch/roll option

add testrender.py file
allow option to enable OpenGL hardware renderer in multithreaded sim

b3RequestCameraImageSelectRenderer(commandHandle,ER_BULLET_HARDWARE_OPENGL);
This commit is contained in:
Erwin Coumans
2016-08-08 14:23:44 -07:00
parent f5fca86d49
commit b880ddf76b
11 changed files with 219 additions and 81 deletions

View File

@@ -825,7 +825,7 @@ 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)
void b3RequestCameraImageSetViewMatrix2(b3SharedMemoryCommandHandle commandHandle, const float cameraTargetPosition[3], float distance, float yaw, float pitch, float roll, int upAxis)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
@@ -834,47 +834,78 @@ void b3RequestCameraImageSetViewMatrix2(b3SharedMemoryCommandHandle commandHandl
b3Vector3 camForward;
b3Vector3 camPos;
b3Vector3 camTargetPos = b3MakeVector3(cameraTargetPosition[0],cameraTargetPosition[1],cameraTargetPosition[2]);
b3Vector3 eyePos = b3MakeVector3(0,0,0);
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();
case 1:
{
forwardAxis = 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 rollRad = roll * b3Scalar(0.01745329251994329547);
b3Quaternion rollRot(camForward,rollRad);
camUpVector = b3QuatRotate(rollRot,b3MakeVector3(0,1,0));
//gLightPos = b3MakeVector3(-50.f,100,30);
break;
}
case 2:
{
forwardAxis = 1;
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 rollRad = roll * b3Scalar(0.01745329251994329547);
b3Quaternion rollRot(camForward,rollRad);
camUpVector = b3QuatRotate(rollRot,b3MakeVector3(0,0,1));
//gLightPos = b3MakeVector3(-50.f,30,100);
break;
}
default:
{
//b3Assert(0);
return;
}
};
}
b3Scalar rele = yaw * b3Scalar(0.01745329251994329547);// rads per deg
b3Scalar razi = pitch * b3Scalar(0.01745329251994329547);// rads per deg
b3Quaternion rot(camUpVector,razi);
b3Scalar yawRad = yaw * b3Scalar(0.01745329251994329547);// rads per deg
b3Scalar pitchRad = pitch * b3Scalar(0.01745329251994329547);// rads per deg
b3Quaternion pitchRot(camUpVector,pitchRad);
b3Vector3 right = camUpVector.cross(camForward);
b3Quaternion roll(right,-rele);
b3Quaternion yawRot(right,-yawRad);
eyePos = b3Matrix3x3(rot) * b3Matrix3x3(roll) * eyePos;
eyePos = b3Matrix3x3(pitchRot) * b3Matrix3x3(yawRot) * eyePos;
camPos = eyePos;
camPos += camTargetPos;