diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index b4f5c0a9d..c9ee26cda 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1930,14 +1930,14 @@ void b3RequestCameraImageSetShadow(b3SharedMemoryCommandHandle commandHandle, in command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_SHADOW; } -void b3ComputeYawPitchRollFromPosition(const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3], int upAxis, float* cameraDistance, float* cameraYaw, float* cameraPitch) +void b3ComputeYawPitchRollFromPosition(const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3], int upAxis, float& cameraDistance, float& cameraYaw, float& cameraPitch) { b3Vector3 camPos = b3MakeVector3(cameraPosition[0], cameraPosition[1], cameraPosition[2]); b3Vector3 camTargetPos = b3MakeVector3(cameraTargetPosition[0], cameraTargetPosition[1], cameraTargetPosition[2]); b3Vector3 camUpVector = b3MakeVector3(cameraUp[0], cameraUp[1], cameraUp[2]); camUpVector.normalize(); - *cameraDistance = (camPos - camTargetPos).length(); + cameraDistance = (camPos - camTargetPos).length(); b3Vector3 eyePos = camPos - camTargetPos; b3Vector3 eyeInitPos = b3MakeVector3(0, 0, 0); @@ -1956,7 +1956,7 @@ void b3ComputeYawPitchRollFromPosition(const float cameraPosition[3], const floa return; }; - eyeInitPos[forwardAxis] = -(*cameraDistance); + eyeInitPos[forwardAxis] = -cameraDistance; eyeInitPos.normalize(); eyePos.normalize(); @@ -1978,10 +1978,8 @@ void b3ComputeYawPitchRollFromPosition(const float cameraPosition[3], const floa return; }; - *cameraYaw = yawRad/b3Scalar(0.01745329251994329547); - *cameraPitch = pitchRad/b3Scalar(0.01745329251994329547); - float cameraRoll = rollRad/b3Scalar(0.01745329251994329547); - printf("camera roll: %f\n", cameraRoll); + cameraYaw = yawRad/b3Scalar(0.01745329251994329547); + cameraPitch = pitchRad/b3Scalar(0.01745329251994329547); } void b3ComputePositionFromViewMatrix(const float viewMatrix[16], float cameraPosition[3], float cameraTargetPosition[3], float cameraUp[3]) @@ -1992,8 +1990,6 @@ void b3ComputePositionFromViewMatrix(const float viewMatrix[16], float cameraPos b3Transform tinv = t.inverse(); b3Matrix3x3 basis = tinv.getBasis(); b3Vector3 origin = tinv.getOrigin(); - printf("basis: %f, %f, %f, %f, %f, %f, %f, %f, %f\n", basis[0][0], basis[0][1], basis[0][2], basis[1][0], basis[1][1], basis[1][2], basis[2][0], basis[2][1], basis[2][2]); - printf("origin: %f, %f, %f\n", origin[0], origin[1], origin[2]); b3Vector3 s = b3MakeVector3(basis[0][0], basis[1][0], basis[2][0]); b3Vector3 u = b3MakeVector3(basis[0][1], basis[1][1], basis[2][1]); b3Vector3 f = b3MakeVector3(-basis[0][2], -basis[1][2], -basis[2][2]); @@ -2010,6 +2006,14 @@ void b3ComputePositionFromViewMatrix(const float viewMatrix[16], float cameraPos cameraUp[2] = u[2]; } +void b3ComputeYawPitchRollFromViewMatrix(const float viewMatrix[16], int upAxis, float& cameraDistance, float& cameraYaw, float& cameraPitch, float cameraTargetPosition[3]) +{ + float cameraPosition[3]; + float cameraUp[3]; + b3ComputePositionFromViewMatrix(viewMatrix, cameraPosition, cameraTargetPosition, cameraUp); + b3ComputeYawPitchRollFromPosition(cameraPosition, cameraTargetPosition, cameraUp, upAxis, cameraDistance, cameraYaw, cameraPitch); +} + void b3ComputeViewMatrixFromPositions(const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3], float viewMatrix[16]) { b3Vector3 eye = b3MakeVector3(cameraPosition[0], cameraPosition[1], cameraPosition[2]); diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index bcb8d06f0..50f43145b 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -162,7 +162,8 @@ void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImage void b3ComputeViewMatrixFromPositions(const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3], float viewMatrix[16]); void b3ComputeViewMatrixFromYawPitchRoll(const float cameraTargetPosition[3], float distance, float yaw, float pitch, float roll, int upAxis, float viewMatrix[16]); void b3ComputePositionFromViewMatrix(const float viewMatrix[16], float cameraPosition[3], float cameraTargetPosition[3], float cameraUp[3]); -void b3ComputeYawPitchRollFromPosition(const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3], int upAxis, float* cameraDistance, float* cameraYaw, float* cameraPitch); +void b3ComputeYawPitchRollFromPosition(const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3], int upAxis, float& cameraDistance, float& cameraYaw, float& cameraPitch); +void b3ComputeYawPitchRollFromViewMatrix(const float viewMatrix[16], int upAxis, float& cameraDistance, float& cameraYaw, float& cameraPitch, float cameraTargetPosition[3]); ///compute a projection matrix, helper function for b3RequestCameraImageSetCameraMatrices void b3ComputeProjectionMatrix(float left, float right, float bottom, float top, float nearVal, float farVal, float projectionMatrix[16]); diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 7aa501b58..0a9cfa805 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -273,33 +273,25 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) ///request an image from a simulated camera, using a software renderer. b3SharedMemoryCommandHandle commandHandle = b3InitRequestCameraImage(m_physicsClientHandle); - - float cameraPos[3] = {1.0, 1.0, 1.0}; - float cameraTarget[3] = {2.0, 1.0, 1.0}; - float cameraUp[3] = {0.0, 0.0, 1.0}; - float viewMat[16]; - b3ComputeViewMatrixFromPositions(cameraPos, cameraTarget, cameraUp, viewMat); - printf("viewMat: %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f\n", viewMat[0], viewMat[1], viewMat[2], viewMat[3], viewMat[4], viewMat[5], viewMat[6], viewMat[7], viewMat[8], viewMat[9], viewMat[10], viewMat[11], viewMat[12], viewMat[13], viewMat[14], viewMat[15]); - float cameraPos2[3]; - float cameraTarget2[3]; - float cameraUp2[3]; - b3ComputePositionFromViewMatrix(viewMat, cameraPos2, cameraTarget2, cameraUp2); - printf("cameraPos2: %f, %f, %f\n", cameraPos2[0], cameraPos2[1], cameraPos2[2]); - printf("cameraTarget2: %f, %f, %f\n", cameraTarget2[0], cameraTarget2[1], cameraTarget2[2]); - printf("cameraUp2: %f, %f, %f\n", cameraUp2[0], cameraUp2[1], cameraUp2[2]); - - float cameraDistance; - float cameraYaw; - float cameraPitch; - b3ComputeYawPitchRollFromPosition(cameraPos2, cameraTarget2, cameraUp2, 2, &cameraDistance, &cameraYaw, &cameraPitch); - printf("camera distance: %f\n", cameraDistance); - printf("camera yaw: %f\n", cameraYaw); - printf("camera pitch: %f\n", cameraPitch); - - b3ComputeViewMatrixFromYawPitchRoll(cameraTarget2, 1.0, -90.0, 0.0, 0.0, 2, viewMat); //b3RequestCameraImageSelectRenderer(commandHandle,ER_BULLET_HARDWARE_OPENGL); float viewMatrix[16]; + + float cameraTargetPosition[3] = {1.0, 2.0, 3.0}; + float distance = 1.0; + float yaw = 10.0; + float pitch = 70.0; + float roll = 0.0; + int upAxis = 2; + float cameraDistance; + float cameraYaw; + float cameraPitch; + float cameraTargetPositionNew[3]; + b3ComputeViewMatrixFromYawPitchRoll(cameraTargetPosition, distance, yaw, pitch, roll, upAxis, viewMatrix); + b3ComputeYawPitchRollFromViewMatrix(viewMatrix, upAxis, cameraDistance, cameraYaw, cameraPitch, cameraTargetPositionNew); + b3ComputeViewMatrixFromYawPitchRoll(cameraTargetPositionNew, cameraDistance, cameraYaw, cameraPitch, 0.0, upAxis, viewMatrix); + + float projectionMatrix[16]; m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraProjectionMatrix(projectionMatrix); m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraViewMatrix(viewMatrix);