From 557bf79e0a7e59572f542e4559596d79df508620 Mon Sep 17 00:00:00 2001 From: Philip Rosedale Date: Wed, 17 Apr 2013 17:49:26 -0700 Subject: [PATCH 1/2] body yaw debugging (commented out) --- libraries/avatars/src/AvatarData.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/libraries/avatars/src/AvatarData.cpp b/libraries/avatars/src/AvatarData.cpp index 904a106f73..726617b9e0 100644 --- a/libraries/avatars/src/AvatarData.cpp +++ b/libraries/avatars/src/AvatarData.cpp @@ -82,7 +82,10 @@ void AvatarData::parseData(unsigned char* sourceBuffer, int numBytes) { memcpy(&_handPosition, sourceBuffer, sizeof(float) * 3); sourceBuffer += sizeof(float) * 3; + + //printf( "_bodyYaw = %f", _bodyYaw ); + //std::cout << _handPosition.x << ", " << _handPosition.y << ", " << _handPosition.z << "\n"; //std::cout << _bodyPosition.x << ", " << _bodyPosition.y << ", " << _bodyPosition.z << "\n"; From d2009366157701dc3157dc7262ce2c40751956ea Mon Sep 17 00:00:00 2001 From: Philip Rosedale Date: Wed, 17 Apr 2013 21:10:31 -0700 Subject: [PATCH 2/2] Added eulerToOrthonormals function for Brad in Util.cpp, sample execution at start of main. --- interface/src/Util.cpp | 32 ++++++++++++++++++++++++++++++++ interface/src/Util.h | 1 + interface/src/main.cpp | 15 ++++++++++++++- 3 files changed, 47 insertions(+), 1 deletion(-) diff --git a/interface/src/Util.cpp b/interface/src/Util.cpp index 5c41318ec0..ccd407e400 100644 --- a/interface/src/Util.cpp +++ b/interface/src/Util.cpp @@ -11,12 +11,44 @@ #include #include +#include #include #include "world.h" #include "Util.h" + +void eulerToOrthonormals(glm::vec3 * angles, glm::vec3 * fwd, glm::vec3 * left, glm::vec3 * up) { + // + // Converts from three euler angles to the associated orthonormal vectors + // + // Angles contains (pitch, yaw, roll) in radians + // + + // First, create the quaternion associated with these euler angles + glm::quat q(glm::vec3(angles->x, angles->y, angles->z)); + + // Next, create a rotation matrix from that quaternion + glm::mat4 rotation; + rotation = glm::mat4_cast(q); + + // Transform the original vectors by the rotation matrix to get the new vectors + glm::vec4 u(0,1,0,0); + glm::vec4 l(1,0,0,0); + glm::vec4 f(0,0,1,0); + glm::vec4 uNew = u*rotation; + glm::vec4 lNew = l*rotation; + glm::vec4 fNew = f*rotation; + + // Copy the answers to output vectors + up->x = uNew.x; up->y = uNew.y; up->z = uNew.z; + left->x = lNew.x; left->y = lNew.y; left->z = lNew.z; + fwd->x = fNew.x; fwd->y = fNew.y; fwd->z = fNew.z; + +} + + // Return the azimuth angle in degrees between two points. float azimuth_to(glm::vec3 head_pos, glm::vec3 source_pos) { return atan2(head_pos.x - source_pos.x, head_pos.z - source_pos.z) * 180.0f / PIf; diff --git a/interface/src/Util.h b/interface/src/Util.h index f2adc918be..5a42884317 100644 --- a/interface/src/Util.h +++ b/interface/src/Util.h @@ -19,6 +19,7 @@ #include +void eulerToOrthonormals(glm::vec3 * angles, glm::vec3 * fwd, glm::vec3 * left, glm::vec3 * up); float azimuth_to(glm::vec3 head_pos, glm::vec3 source_pos); float angle_to(glm::vec3 head_pos, glm::vec3 source_pos, float render_yaw, float head_yaw); diff --git a/interface/src/main.cpp b/interface/src/main.cpp index b2934cab17..3d66828af6 100644 --- a/interface/src/main.cpp +++ b/interface/src/main.cpp @@ -1510,7 +1510,20 @@ void audioMixerUpdate(in_addr_t newMixerAddress, in_port_t newMixerPort) { int main(int argc, const char * argv[]) { // Quick test of the Orientation class on startup! - testOrientationClass(); + //testOrientationClass(); // PER - commented out to test orthonormal code + + // + // For Brad: Demo of function to test conversion of euler angles to orthonormals + // (Note that the euler angles order is Pitch, Yaw, Roll, and in radians) + // + glm::vec3 angles(0, PI/4, 0); + glm::vec3 fwd, left, up; + + eulerToOrthonormals(&angles, &fwd, &left, &up); + + printf("fwd: %4.2f, %4.2f, %4.2f\n", fwd.x, fwd.y, fwd.z); + printf("left: %4.2f, %4.2f, %4.2f\n", left.x, left.y, left.z); + printf("up: %4.2f, %4.2f, %4.2f\n", up.x, up.y, up.z); AgentList::createInstance(AGENT_TYPE_INTERFACE);