From e825764304dbf6fd8c612f842a4f94b0f08d7a82 Mon Sep 17 00:00:00 2001 From: Jeffrey Ventrella Date: Tue, 16 Apr 2013 19:53:48 -0700 Subject: [PATCH] added instrumentation to Orientation class to test for normalized and orthogonal vectors --- libraries/avatars/src/Orientation.cpp | 81 +++++++++++++++++++++++++++ libraries/avatars/src/Orientation.h | 3 + libraries/shared/src/SharedUtil.h | 28 ++++----- 3 files changed, 98 insertions(+), 14 deletions(-) diff --git a/libraries/avatars/src/Orientation.cpp b/libraries/avatars/src/Orientation.cpp index ed1500a506..ed4af46d25 100755 --- a/libraries/avatars/src/Orientation.cpp +++ b/libraries/avatars/src/Orientation.cpp @@ -8,6 +8,9 @@ #include "Orientation.h" #include + +static bool testingForNormalizationAndOrthogonality = false; + Orientation::Orientation() { right = glm::vec3( 1.0, 0.0, 0.0 ); up = glm::vec3( 0.0, 1.0, 0.0 ); @@ -41,6 +44,8 @@ void Orientation::yaw( float angle ) { front = cosineFront + sineRight; right = cosineRight - sineFront; + + if ( testingForNormalizationAndOrthogonality ) { testForOrthogonalAndNormalizedVectors( EPSILON ); } } @@ -56,6 +61,8 @@ void Orientation::pitch( float angle ) { up = cosineUp + sineFront; front = cosineFront - sineUp; + + if ( testingForNormalizationAndOrthogonality ) { testForOrthogonalAndNormalizedVectors( EPSILON ); } } @@ -71,6 +78,8 @@ void Orientation::roll( float angle ) { up = cosineUp + sineRight; right = cosineRight - sineUp; + + if ( testingForNormalizationAndOrthogonality ) { testForOrthogonalAndNormalizedVectors( EPSILON ); } } @@ -79,3 +88,75 @@ void Orientation::setRightUpFront( const glm::vec3 &r, const glm::vec3 &u, const up = u; front = f; } + + + +//---------------------------------------------------------------------- +void Orientation::testForOrthogonalAndNormalizedVectors( float epsilon ) { + + //---------------------------------------------------------------- + // make sure vectors are normalized (or close to length 1) + //---------------------------------------------------------------- + float rightLength = glm::length( right ); + float upLength = glm::length( up ); + float frontLength = glm::length( front ); + + if (( rightLength > 1.0f + epsilon ) + || ( rightLength < 1.0f - epsilon )) { + printf( "Error in Orientation class: right direction length is %f \n", rightLength ); + } + assert ( rightLength > 1.0f - epsilon ); + assert ( rightLength < 1.0f + epsilon ); + + + if (( upLength > 1.0f + epsilon ) + || ( upLength < 1.0f - epsilon )) { + printf( "Error in Orientation class: up direction length is %f \n", upLength ); + } + assert ( upLength > 1.0f - epsilon ); + assert ( upLength < 1.0f + epsilon ); + + + if (( frontLength > 1.0f + epsilon ) + || ( frontLength < 1.0f - epsilon )) { + printf( "Error in Orientation class: front direction length is %f \n", frontLength ); + } + assert ( frontLength > 1.0f - epsilon ); + assert ( frontLength < 1.0f + epsilon ); + + + + //---------------------------------------------------------------- + // make sure vectors are orthoginal (or close enough) + //---------------------------------------------------------------- + glm::vec3 rightCross = glm::cross( up, front ); + glm::vec3 upCross = glm::cross( front, right ); + glm::vec3 frontCross = glm::cross( right, up ); + + float rightDiff = glm::length( rightCross - right ); + float upDiff = glm::length( upCross - up ); + float frontDiff = glm::length( frontCross - front ); + + + if ( rightDiff > epsilon ) { + printf( "Error in Orientation class: right direction not orthogonal to up and/or front. " ); + printf( "The tested cross of up and front is off by %f \n", rightDiff ); + } + assert ( rightDiff < epsilon ); + + + if ( upDiff > epsilon ) { + printf( "Error in Orientation class: up direction not orthogonal to front and/or right. " ); + printf( "The tested cross of front and right is off by %f \n", upDiff ); + } + assert ( upDiff < epsilon ); + + + if ( frontDiff > epsilon ) { + printf( "Error in Orientation class: front direction not orthogonal to right and/or up. " ); + printf( "The tested cross of right and up is off by %f \n", frontDiff ); + } + assert ( frontDiff < epsilon ); +} + + diff --git a/libraries/avatars/src/Orientation.h b/libraries/avatars/src/Orientation.h index ae209a5f47..e088f40517 100755 --- a/libraries/avatars/src/Orientation.h +++ b/libraries/avatars/src/Orientation.h @@ -39,6 +39,9 @@ public: glm::vec3 getFront() { return front; } void setRightUpFront( const glm::vec3 &, const glm::vec3 &, const glm::vec3 & ); + +private: + void testForOrthogonalAndNormalizedVectors( float epsilon ); }; diff --git a/libraries/shared/src/SharedUtil.h b/libraries/shared/src/SharedUtil.h index 6941c74462..f7e639ba43 100644 --- a/libraries/shared/src/SharedUtil.h +++ b/libraries/shared/src/SharedUtil.h @@ -18,20 +18,20 @@ #include #endif -static const float ZERO = 0.0; -static const float ONE = 1.0; -static const float ONE_HALF = 0.5; -static const double ONE_THIRD = 0.3333333; -static const double PIE = 3.14159265359; -static const double PI_TIMES_TWO = 3.14159265359 * 2.0; -static const double PI_OVER_180 = 3.14159265359 / 180.0; -static const double EPSILON = 0.00001; //smallish number - used as margin of error for some computations -static const double SQUARE_ROOT_OF_2 = sqrt(2); -static const double SQUARE_ROOT_OF_3 = sqrt(3); -static const float METER = 1.0; -static const float DECIMETER = 0.1; -static const float CENTIMETER = 0.01; -static const float MILLIIMETER = 0.001; +static const float ZERO = 0.0f; +static const float ONE = 1.0f; +static const float ONE_HALF = 0.5f; +static const float ONE_THIRD = 0.333333f; +static const float PIE = 3.141592f; +static const float PI_TIMES_TWO = 3.141592f * 2.0f; +static const float PI_OVER_180 = 3.141592f / 180.0f; +static const float EPSILON = 0.000001f; //smallish positive number - used as margin of error for some computations +static const float SQUARE_ROOT_OF_2 = (float)sqrt(2); +static const float SQUARE_ROOT_OF_3 = (float)sqrt(3); +static const float METER = 1.0f; +static const float DECIMETER = 0.1f; +static const float CENTIMETER = 0.01f; +static const float MILLIIMETER = 0.001f; double usecTimestamp(timeval *time); double usecTimestampNow();