From 8b9e252246bd24e83f6c3e0012fe4d7b85028616 Mon Sep 17 00:00:00 2001 From: ZappoMan Date: Wed, 17 Apr 2013 23:21:12 -0700 Subject: [PATCH] Updated Orientation class to use Philip's technique. - Changed Orientation class to use Philip's technique for determining vectors - updated main.cpp to take command line option to run Orientation tests - tweaked eulerToOrthonormals() to return right vector instead of left vector --- interface/src/Util.cpp | 26 +++--- interface/src/main.cpp | 18 +--- libraries/avatars/src/Orientation.cpp | 115 ++++++++++++++------------ libraries/avatars/src/Orientation.h | 7 ++ 4 files changed, 85 insertions(+), 81 deletions(-) diff --git a/interface/src/Util.cpp b/interface/src/Util.cpp index 591cc5024d..4ca77fcb68 100644 --- a/interface/src/Util.cpp +++ b/interface/src/Util.cpp @@ -19,7 +19,7 @@ -void eulerToOrthonormals(glm::vec3 * angles, glm::vec3 * fwd, glm::vec3 * left, glm::vec3 * up) { +void eulerToOrthonormals(glm::vec3 * angles, glm::vec3 * front, glm::vec3 * right, glm::vec3 * up) { // // Converts from three euler angles to the associated orthonormal vectors // @@ -27,28 +27,28 @@ void eulerToOrthonormals(glm::vec3 * angles, glm::vec3 * fwd, glm::vec3 * left, // // First, create the quaternion associated with these euler angles - glm::quat q(glm::vec3(angles->x, angles->y, angles->z)); - + 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; + glm::vec4 qup(0,1,0,0); + glm::vec4 qright(-1,0,0,0); + glm::vec4 qfront(0,0,1,0); + glm::vec4 upNew = qup*rotation; + glm::vec4 rightNew = qright*rotation; + glm::vec4 frontNew = qfront*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; - + up->x = upNew.x; up->y = upNew.y; up->z = upNew.z; + right->x = rightNew.x; right->y = rightNew.y; right->z = rightNew.z; + front->x = frontNew.x; front->y = frontNew.y; front->z = frontNew.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/main.cpp b/interface/src/main.cpp index 3d66828af6..9cb5e85cb5 100644 --- a/interface/src/main.cpp +++ b/interface/src/main.cpp @@ -1510,20 +1510,10 @@ 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(); // 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); + if (cmdOptionExists(argc, argv, "--testOrientation")) { + testOrientationClass(); + return EXIT_SUCCESS; + } AgentList::createInstance(AGENT_TYPE_INTERFACE); diff --git a/libraries/avatars/src/Orientation.cpp b/libraries/avatars/src/Orientation.cpp index f11f820c89..3d6734d92d 100755 --- a/libraries/avatars/src/Orientation.cpp +++ b/libraries/avatars/src/Orientation.cpp @@ -7,19 +7,26 @@ #include "Orientation.h" #include +#include +#include +//#include "Util.h" -static bool testingForNormalizationAndOrthogonality = true; +// XXXBHG - this test has not yet been reworked to match the correct vector orientation +// of the coordinate system, so don't use it for now. +static bool testingForNormalizationAndOrthogonality = false; Orientation::Orientation() { setToIdentity(); } - void Orientation::setToIdentity() { - right = glm::vec3( 1.0, 0.0, 0.0 ); - up = glm::vec3( 0.0, 1.0, 0.0 ); - front = glm::vec3( 0.0, 0.0, 1.0 ); + _yaw = 0.0; + _pitch = 0.0; + _roll = 0.0; + right = glm::vec3( -1.0f, 0.0f, 0.0f ); + up = glm::vec3( 0.0f, 1.0f, 0.0f ); + front = glm::vec3( 0.0f, 0.0f, 1.0f ); } void Orientation::set( Orientation o ) { @@ -28,72 +35,75 @@ void Orientation::set( Orientation o ) { front = o.front; } +void Orientation::update() { -void Orientation::yaw( float angle ) { - float r = angle * PI_OVER_180; - float s = sin(r); - float c = cos(r); - - glm::vec3 cosineFront = front * c; - glm::vec3 cosineRight = right * c; - glm::vec3 sineFront = front * s; - glm::vec3 sineRight = right * s; - - front = cosineFront + sineRight; - right = cosineRight - sineFront; + float pitchRads = _pitch * PI_OVER_180; + float yawRads = _yaw * PI_OVER_180; + float rollRads = _roll * PI_OVER_180; + + glm::quat q(glm::vec3(pitchRads, -(yawRads), rollRads)); + + // 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 qup(0,1,0,0); + glm::vec4 qright(-1,0,0,0); + glm::vec4 qfront(0,0,1,0); + glm::vec4 upNew = qup*rotation; + glm::vec4 rightNew = qright*rotation; + glm::vec4 frontNew = qfront*rotation; + + // Copy the answers to output vectors + up.x = upNew.x; + up.y = upNew.y; + up.z = upNew.z; + + right.x = rightNew.x; + right.y = rightNew.y; + right.z = rightNew.z; + + front.x = frontNew.x; + front.y = frontNew.y; + front.z = frontNew.z; if ( testingForNormalizationAndOrthogonality ) { testForOrthogonalAndNormalizedVectors( EPSILON ); } } +void Orientation::yaw(float angle) { + // remember the value for any future changes to other angles + _yaw = angle; + update(); +} void Orientation::pitch( float angle ) { - float r = angle * PI_OVER_180; - float s = sin(r); - float c = cos(r); - - glm::vec3 cosineUp = up * c; - glm::vec3 cosineFront = front * c; - glm::vec3 sineUp = up * s; - glm::vec3 sineFront = front * s; - - up = cosineUp + sineFront; - front = cosineFront - sineUp; - - if ( testingForNormalizationAndOrthogonality ) { testForOrthogonalAndNormalizedVectors( EPSILON ); } + // remember the value for any future changes to other angles + _pitch = angle; + update(); } void Orientation::roll( float angle ) { - float r = angle * PI_OVER_180; - float s = sin(r); - float c = cos(r); - - glm::vec3 cosineUp = up * c; - glm::vec3 cosineRight = right * c; - glm::vec3 sineUp = up * s; - glm::vec3 sineRight = right * s; - - up = cosineUp + sineRight; - right = cosineRight - sineUp; - - if ( testingForNormalizationAndOrthogonality ) { testForOrthogonalAndNormalizedVectors( EPSILON ); } + _roll = angle; + update(); } void Orientation::setRightUpFront( const glm::vec3 &r, const glm::vec3 &u, const glm::vec3 &f ) { - right = r; - up = u; - front = f; + right = r; + up = u; + front = f; } - - -//---------------------------------------------------------------------- void Orientation::testForOrthogonalAndNormalizedVectors( float epsilon ) { - //------------------------------------------------------------------ + // XXXBHG - this test has not yet been reworked to match the correct vector orientation + // of the coordinate system + // bail for now, assume all is good + return; + // make sure vectors are normalized (or close enough to length 1.0) - //------------------------------------------------------------------ float rightLength = glm::length( right ); float upLength = glm::length( up ); float frontLength = glm::length( front ); @@ -122,10 +132,7 @@ void Orientation::testForOrthogonalAndNormalizedVectors( float epsilon ) { assert ( frontLength < 1.0f + epsilon ); - - //---------------------------------------------------------------- // make sure vectors are orthogonal (or close enough) - //---------------------------------------------------------------- glm::vec3 rightCross = glm::cross( up, front ); glm::vec3 upCross = glm::cross( front, right ); glm::vec3 frontCross = glm::cross( right, up ); diff --git a/libraries/avatars/src/Orientation.h b/libraries/avatars/src/Orientation.h index e088f40517..becddbd338 100755 --- a/libraries/avatars/src/Orientation.h +++ b/libraries/avatars/src/Orientation.h @@ -20,6 +20,13 @@ enum Axis class Orientation { +private: + float _yaw; + float _pitch; + float _roll; + + void update(); // actually updates the vectors from yaw, pitch, roll + public: Orientation();