TEST(MathLib, GlobalDenseMatrix) { const std::size_t n_rows(5); const std::size_t n_cols(5); MathLib::GlobalDenseMatrix<double, std::size_t> mat0(n_rows,n_cols); MathLib::GlobalDenseMatrix<double, std::size_t> mat1(n_rows,n_cols); MathLib::GlobalDenseMatrix<double, std::size_t> mat2(n_rows,n_cols-1); for (std::size_t i(0); i<n_rows; i++) { for (std::size_t j(0); j<n_cols; j++) { mat0(i,j) = 1.0 / (i+1.0+j+1.0); } } mat1.setZero(); mat1 = mat0; for (std::size_t i(0); i<n_rows; i++) { for (std::size_t j(0); j<n_cols; j++) { ASSERT_NEAR(1.0/(i+j+2.0), mat1(i,j), std::numeric_limits<double>::epsilon()); } } ASSERT_THROW(mat2 = mat1, std::range_error); }
int main () { Matrix mat0 (3, 4); Matrix mat1 (3, 2); mat0 [0] = 1; mat0 [1] = 2; mat0 [2] = 3; mat0 [3] = 4; mat0 [4] = 5; mat0 [5] = 6; mat0 [6] = 7; mat0 [7] = 8; mat0 [8] = 9; mat0 [9] = 4; mat0 [10] = 5; mat0 [11] = 6; mat1 (0,0) = 9.5; mat1 (0,1) = 8.5; mat1 (1,0) = 7.5; mat1 (1,1) = 6.5; mat1 (2,0) = 5.5; mat1 (2,1) = 4.5; MatrixOperation::print (mat0); MatrixOperation::print (mat1); std::vector<double > a (3); a[0] = 9; a[1] = 8; a[2] = 7; std::vector<double > b (2); b[0] = 4.5; b[1] = 5.5; // MatrixOperation::mv (a, 1.5, 2.1, mat0, true, b); // print (a); Matrix C; MatrixOperation::mm (C, 2.2, mat0, true, mat1, false); MatrixOperation::print (C); }
//vectormath makes a difference between point and vector. void VectormathTest() { printf("Vectormath\n"); Vectormath::Aos::Transform3 tr; tr = Vectormath::Aos::Transform3::identity(); tr.setTranslation(Vectormath::Aos::Vector3(10,0,0)); //initialization Vectormath::Aos::Point3 pointA(0,0,0); Vectormath::Aos::Point3 pointB,pointC,pointE; Vectormath::Aos::Vector3 pointD; //assignment pointB = pointA; //in-place initialization pointB = Vectormath::Aos::Point3(1,2,3); //or pointB.setElem(0,1); //or pointB.setX(1); //transform over tr pointB = tr * pointA; printf("pointB = tr * pointA = (%f,%f,%f)\n",(float)pointB.getX(),(float)pointB.getY(),(float)pointB.getZ()); //transform over tr //pointE = tr(pointA); //inverse transform pointC = Vectormath::Aos::inverse(tr) * pointA; printf("Vectormath::Aos::inverse(tr) * pointA = (%f,%f,%f)\n",(float)pointC.getX(),(float)pointC.getY(),(float)pointC.getZ()); btScalar x; //dot product x = Vectormath::Aos::dot(Vectormath::Aos::Vector3(pointD),Vectormath::Aos::Vector3(pointE)); //square length x = Vectormath::Aos::lengthSqr(Vectormath::Aos::Vector3(pointD)); //length x = Vectormath::Aos::length(Vectormath::Aos::Vector3(pointD)); const Vectormath::Aos::Vector3& constPointD = (Vectormath::Aos::Vector3&)pointD; //get a normalized vector from constPointD, without changing constPointD Vectormath::Aos::Vector3 norm = Vectormath::Aos::normalize(constPointD); //in-place normalize pointD pointD = Vectormath::Aos::normalize(Vectormath::Aos::Vector3(pointD)); //quaternions & matrices Vectormath::Aos::Quat quat(0,0,0,1); Vectormath::Aos::Quat quat1; quat1 = Vectormath::Aos::Quat::rotationY(90.f * SIMD_RADS_PER_DEG); Vectormath::Aos::Matrix3 mat0(quat1); Vectormath::Aos::Matrix3 mat1 = Vectormath::Aos::inverse(mat0); Vectormath::Aos::Matrix3 mat2 = Vectormath::Aos::transpose(mat0); Vectormath::Aos::Transform3 tr1(mat2,Vectormath::Aos::Vector3(0,10,0)); Vectormath::Aos::Transform3 tr2 = Vectormath::Aos::inverse(tr1); Vectormath::Aos::Point3 pt0(1,1,1); Vectormath::Aos::Point3 pt1 = tr2 * pt0; printf("Vectormath::Aos::Vector3 pt1 = tr2 * pt0; = (%f,%f,%f)\n",(float)pt1.getX(),(float)pt1.getY(),(float)pt1.getZ()); Vectormath::Aos::Vector3 pt2 = tr2.getUpper3x3() * Vectormath::Aos::Vector3(pt0); //Vectormath::Aos::Vector3 pt3 = pt0 * tr2.getUpper3x3(); Vectormath::Aos::Vector3 pt3 = Vectormath::Aos::inverse(tr2.getUpper3x3()) * Vectormath::Aos::Vector3(pt0); Vectormath::Aos::Vector3 pt4 = Vectormath::Aos::inverse(tr2.getUpper3x3()) * Vectormath::Aos::Vector3(pt0); Vectormath::Aos::Transform3 tr3 = Vectormath::Aos::inverse(tr2) * tr2; }
//Bullet, a btVector can be used for both points and vectors. //it is up to the user/developer to use the right multiplication: btTransform for points, and btQuaternion or btMatrix3x3 for vectors. void BulletTest() { printf("Bullet Linearmath\n"); btTransform tr; tr.setIdentity(); tr.setOrigin(btVector3(10,0,0)); //initialization btVector3 pointA(0,0,0); btVector3 pointB,pointC,pointD,pointE; //assignment pointB = pointA; //in-place initialization pointB.setValue(1,2,3); //transform over tr pointB = tr * pointA; printf("pointB = tr * pointA = (%f,%f,%f)\n",pointB.getX(),pointB.getY(),pointB.getZ()); //transform over tr pointE = tr(pointA); //inverse transform pointC = tr.inverse() * pointA; printf("pointC = tr.inverse() * pointA = (%f,%f,%f)\n",pointC.getX(),pointC.getY(),pointC.getZ()); //inverse transform pointD = tr.invXform( pointA ); btScalar x; //dot product x = pointD.dot(pointE); //square length x = pointD.length2(); //length x = pointD.length(); const btVector3& constPointD = pointD; //get a normalized vector from constPointD, without changing constPointD btVector3 norm = constPointD.normalized(); //in-place normalize pointD pointD.normalize(); //quaternions & matrices btQuaternion quat(0,0,0,1); btQuaternion quat1(btVector3(0,1,0),90.f * SIMD_RADS_PER_DEG); btMatrix3x3 mat0(quat1); btMatrix3x3 mat1 = mat0.inverse(); btMatrix3x3 mat2 = mat0.transpose(); btTransform tr1(mat2,btVector3(0,10,0)); btTransform tr2 =tr1.inverse(); btVector3 pt0(1,1,1); btVector3 pt1 = tr2 * pt0; printf("btVector3 pt1 = tr2 * pt0 = (%f,%f,%f)\n",pt1.getX(),pt1.getY(),pt1.getZ()); btVector3 pt2 = tr2.getBasis() * pt0; btVector3 pt3 = pt0 * tr2.getBasis(); btVector3 pt4 = tr2.getBasis().inverse() * pt0; btTransform tr3 = tr2.inverseTimes(tr2); }
bool TestQuaternion( ) { bool ok = true; cout << "Testing Quaternion" << endl; cout << "Quaternion( ) [default constructor]" << endl; QuaternionF quat0; float w = 3.f; float x = 0.f; float y = 4.f; float z = 0.f; cout << "Set( " << w << ", " << x << ", " << y << ", " << z << " )" << endl; quat0.Set( w, x, y, z ); TESTCHECK( quat0.W(), w, &ok ); TESTCHECK( quat0.X(), x, &ok ); TESTCHECK( quat0.Y(), y, &ok ); TESTCHECK( quat0.Z(), z, &ok ); TESTCHECK( quat0[0], w, &ok ); TESTCHECK( quat0[1], x, &ok ); TESTCHECK( quat0[2], y, &ok ); TESTCHECK( quat0[3], z, &ok ); TESTCHECK( quat0.At( 0 ), w, &ok ); TESTCHECK( quat0.At( 1 ), x, &ok ); TESTCHECK( quat0.At( 2 ), y, &ok ); TESTCHECK( quat0.At( 3 ), z, &ok ); try { TESTCHECK( quat0.At(4), 0.f, &ok ); cout << "At(4) should have thrown an exception." << endl; ok = false; } catch( out_of_range & exceptn ) { cout << "Exception here is OK" << endl; cout << exceptn.what() << endl; } cout << "quat0.Array()" << endl; const float * pF = quat0.Array(); TESTCHECK( pF[0], w, &ok ); TESTCHECK( pF[1], x, &ok ); TESTCHECK( pF[2], y, &ok ); TESTCHECK( pF[3], z, &ok ); TESTCHECK( quat0.Real(), w, &ok ); TESTCHECK( quat0.Imaginary().X(), x, &ok ); TESTCHECK( quat0.Imaginary().Y(), y, &ok ); TESTCHECK( quat0.Imaginary().Z(), z, &ok ); cout << "Conjugate( )" << endl; QuaternionF quat1 = quat0.Conjugate( ); TESTCHECK( quat1.W(), w, &ok ); TESTCHECK( quat1.X(), -x, &ok ); TESTCHECK( quat1.Y(), -y, &ok ); TESTCHECK( quat1.Z(), -z, &ok ); TESTCHECK( quat0.Length(), 5.f, &ok ); TESTCHECK( quat0.Norm(), 25.f, &ok ); TESTCHECK( quat1.Length(), 5.f, &ok ); TESTCHECK( quat1.Norm(), 25.f, &ok ); TESTCHECK( (quat0 + quat1).W(), 6.f, &ok ); TESTCHECK( (quat0 + quat1).X(), 0.f, &ok ); TESTCHECK( (quat0 + quat1).Y(), 0.f, &ok ); TESTCHECK( (quat0 + quat1).Z(), 0.f, &ok ); ostringstream ost; cout << "operator<<" << endl; ost << quat0; TESTCHECK( ost.str(), string( "( 3, 0, 4, 0 )" ), &ok ); TESTCHECK( ToJSON( quat0 ), string( "[ +3.00000e+00, +0.00000e+00, +4.00000e+00," " +0.00000e+00 ]" ), &ok ); FromJSON( "[ 3, 0, -4.0, 0. ]", &quat1 ); TESTCHECK( quat1.W(), w, &ok ); TESTCHECK( quat1.X(), -x, &ok ); TESTCHECK( quat1.Y(), -y, &ok ); TESTCHECK( quat1.Z(), -z, &ok ); cout << "Normalize( )" << endl; quat1.Normalize( ); TESTCHECKF( quat1.W(), 0.6f, &ok ); TESTCHECKF( quat1.X(), 0.f, &ok ); TESTCHECKF( quat1.Y(), -0.8f, &ok ); TESTCHECKF( quat1.Z(), 0.f, &ok ); TESTCHECKF( quat1.Norm(), 1.f, &ok ); cout << "Inverse( )" << endl; quat1 = quat0.Inverse( ); TESTCHECKF( quat1.W(), 0.12f, &ok ); TESTCHECKF( quat1.X(), 0.f, &ok ); TESTCHECKF( quat1.Y(), -0.16f, &ok ); TESTCHECKF( quat1.Z(), 0.f, &ok ); TESTCHECKF( (quat0 * quat1).W(), 1.f, &ok ); TESTCHECKF( (quat0 * quat1).X(), 0.f, &ok ); TESTCHECKF( (quat0 * quat1).Y(), 0.f, &ok ); TESTCHECKF( (quat0 * quat1).Z(), 0.f, &ok ); float coords[4] = { 0.5f, 0.5f, 0.5f, 0.5f }; cout << "QuaternionF( coords ) [array constructor]" << endl; QuaternionF quat2( coords ); TESTCHECKF( quat2.W(), 0.5f, &ok ); TESTCHECKF( quat2.X(), 0.5f, &ok ); TESTCHECKF( quat2.Y(), 0.5f, &ok ); TESTCHECKF( quat2.Z(), 0.5f, &ok ); TESTCHECKF( quat2.Norm(), 1.f, &ok ); cout << "Normalize( )" << endl; quat2.Normalize( ); TESTCHECKF( quat2.W(), 0.5f, &ok ); TESTCHECKF( quat2.X(), 0.5f, &ok ); TESTCHECKF( quat2.Y(), 0.5f, &ok ); TESTCHECKF( quat2.Z(), 0.5f, &ok ); TESTCHECKF( quat2.Norm(), 1.f, &ok ); cout << "Conjugate( )" << endl; quat1 = quat2.Conjugate( ); TESTCHECKF( quat1.W(), 0.5f, &ok ); TESTCHECKF( quat1.X(), -0.5f, &ok ); TESTCHECKF( quat1.Y(), -0.5f, &ok ); TESTCHECKF( quat1.Z(), -0.5f, &ok ); TESTCHECKF( quat1.Norm(), 1.f, &ok ); TESTCHECKF( (quat1 + quat2).W(), 1.f, &ok ); TESTCHECKF( (quat1 + quat2).X(), 0.f, &ok ); TESTCHECKF( (quat1 + quat2).Y(), 0.f, &ok ); TESTCHECKF( (quat1 + quat2).Z(), 0.f, &ok ); TESTCHECKF( (quat1 - quat2).W(), 0.f, &ok ); TESTCHECKF( (quat1 - quat2).X(), -1.f, &ok ); TESTCHECKF( (quat1 - quat2).Y(), -1.f, &ok ); TESTCHECKF( (quat1 - quat2).Z(), -1.f, &ok ); TESTCHECKF( (quat1 * quat2).W(), 1.f, &ok ); TESTCHECKF( (quat1 * quat2).X(), 0.f, &ok ); TESTCHECKF( (quat1 * quat2).Y(), 0.f, &ok ); TESTCHECKF( (quat1 * quat2).Z(), 0.f, &ok ); cout << "Inverse( )" << endl; quat1 = quat2.Inverse( ); TESTCHECKF( quat1.W(), 0.5f, &ok ); TESTCHECKF( quat1.X(), -0.5f, &ok ); TESTCHECKF( quat1.Y(), -0.5f, &ok ); TESTCHECKF( quat1.Z(), -0.5f, &ok ); TESTCHECKF( quat1.Norm(), 1.f, &ok ); double a = M_PI / 2.; cout << "QuaternionF( AxisAngle( Vector3F::UnitZ, Angle( " << a << " ) ) ) [axis-angle constructor]" << endl; QuaternionF quat3( AxisAngleF( Vector3F::UnitZ, Angle( a ) ) ); TESTCHECKF( quat3.W(), sqrt( 0.5f ), &ok ); TESTCHECKF( quat3.X(), 0.f, &ok ); TESTCHECKF( quat3.Y(), 0.f, &ok ); TESTCHECKF( quat3.Z(), sqrt( 0.5f ), &ok ); TESTCHECKF( quat3.Norm(), 1.f, &ok ); cout << "GetAxisAngle( )" << endl; AxisAngleF axisAngle = quat3.GetAxisAngle( ); TESTCHECKF( axisAngle.Axis().X(), 0.f, &ok ); TESTCHECKF( axisAngle.Axis().Y(), 0.f, &ok ); TESTCHECKF( axisAngle.Axis().Z(), 1.f, &ok ); TESTCHECKF( axisAngle.GetAngle().Radians(), M_PI / 2., &ok ); cout << "Set( 2, a )" << endl; quat1.Set( 2, a ); TESTCHECKF( quat1.W(), sqrt( 0.5f ), &ok ); TESTCHECKF( quat1.X(), 0.f, &ok ); TESTCHECKF( quat1.Y(), 0.f, &ok ); TESTCHECKF( quat1.Z(), sqrt( 0.5f ), &ok ); TESTCHECKF( quat1.Norm(), 1.f, &ok ); cout << "Log()" << endl; quat1 = quat3.Log( ); TESTCHECKF( quat1.W(), 0.f, &ok ); TESTCHECKF( quat1.X(), 0.f, &ok ); TESTCHECKF( quat1.Y(), 0.f, &ok ); TESTCHECKF( quat1.Z(), M_PI / 4.f, &ok ); cout << "SetW( 1. )" << endl; quat1.SetW( 1.f ); cout << "Exp()" << endl; quat1 = quat1.Exp( ); TESTCHECKF( quat1.W(), exp( 1.f ) * sqrt( 0.5f ), &ok ); TESTCHECKF( quat1.X(), 0.f, &ok ); TESTCHECKF( quat1.Y(), 0.f, &ok ); TESTCHECKF( quat1.Z(), exp( 1.f ) * sqrt( 0.5f ), &ok ); TESTCHECK( (quat1 == quat3), false, &ok ); cout << "Log()" << endl; quat1 = quat1.Log( ); TESTCHECKF( quat1.W(), 1.f, &ok ); TESTCHECKF( quat1.X(), 0.f, &ok ); TESTCHECKF( quat1.Y(), 0.f, &ok ); TESTCHECKF( quat1.Z(), M_PI / 4.f, &ok ); a = 2. * M_PI / 3.; float f = static_cast<float>( sqrt( 1./3. ) ); cout << "RotationMatrix3F( AxisAngleF( Vector3F( " << f << ", " << f << ", " << f << "), Angle( " << a << " ) ) )" << endl; axisAngle.Set( Vector3F( f, f, f ), Angle( a ) ); RotationMatrix3F mat0( axisAngle ); cout << "QuaternionF( mat0 ) [rotation matrix constructor]" << endl; QuaternionF quat4( mat0 ); TESTCHECKF( quat4.W(), quat2.W(), &ok ); TESTCHECKF( quat4.X(), quat2.X(), &ok ); TESTCHECKF( quat4.Y(), quat2.Y(), &ok ); TESTCHECKF( quat4.Z(), quat2.Z(), &ok ); TESTCHECKF( quat4.Norm(), 1.f, &ok ); cout << "GetAxisAngle( )" << endl; axisAngle = quat4.GetAxisAngle( ); TESTCHECKF( axisAngle.Axis().X(), f, &ok ); TESTCHECKF( axisAngle.Axis().Y(), f, &ok ); TESTCHECKF( axisAngle.Axis().Z(), f, &ok ); TESTCHECKF( axisAngle.GetAngle().Radians(), a, &ok ); cout << "Matrix( )" << endl; RotationMatrix3F mat1 = quat4.Matrix( ); TESTCHECKF( mat1(0,0), mat0(0,0), &ok ); TESTCHECKF( mat1(0,1), mat0(0,1), &ok ); TESTCHECKF( mat1(0,2), mat0(0,2), &ok ); TESTCHECKF( mat1(1,0), mat0(1,0), &ok ); TESTCHECKF( mat1(1,1), mat0(1,1), &ok ); TESTCHECKF( mat1(1,2), mat0(1,2), &ok ); TESTCHECKF( mat1(2,0), mat0(2,0), &ok ); TESTCHECKF( mat1(2,1), mat0(2,1), &ok ); TESTCHECKF( mat1(2,2), mat0(2,2), &ok ); cout << "(quat4 * Quaternion( Vector3F::UnitX )" << "* quat4.Conjugate()).Imaginary() [rotation]" << endl; Vector3F vec1 = (quat4 * QuaternionF( Vector3F::UnitX ) * quat4.Conjugate()).Imaginary(); TESTCHECKF( vec1.X(), 0.f, &ok ); TESTCHECKF( vec1.Y(), 1.f, &ok ); TESTCHECKF( vec1.Z(), 0.f, &ok ); cout << "Rotate( Vector3F::UnitX )" << endl; vec1 = quat4.Rotate( Vector3F::UnitX ); TESTCHECKF( vec1.X(), 0.f, &ok ); TESTCHECKF( vec1.Y(), 1.f, &ok ); TESTCHECKF( vec1.Z(), 0.f, &ok ); float a0 = -2.9f; float a1 = 1.8f; float a2 = -0.7f; cout << "Quaternion qrot0( 0, " << a0 << " )" << endl; QuaternionF qrot0( 0, Angle( a0 ) ); cout << "Quaternion qrot1( 1, " << a1 << " )" << endl; QuaternionF qrot1( 1, Angle( a1 ) ); cout << "Quaternion qrot2( 2, " << a2 << " )" << endl; QuaternionF qrot2( 2, Angle( a2 ) ); cout << "quat1 = qrot1 * qrot2 * qrot0" << endl; quat1 = qrot1 * qrot2 * qrot0; cout << "GetEulerAngles( EulerAngles::YZX )" << endl; EulerAngles euler = quat1.GetEulerAngles( EulerAngles::YZX ); TESTCHECKF( euler[0].Radians(), a1, &ok ); TESTCHECKF( euler[1].Radians(), a2, &ok ); TESTCHECKF( euler[2].Radians(), a0, &ok ); cout << "Quaternion( EulerAngles( " << a1 << ", " << a2 << ", " << a0 << " ), EulerAngles::YZX ) [Euler angles constructor]" << endl; QuaternionF quat5( EulerAngles( a1, a2, a0 ), EulerAngles::YZX ); TESTCHECKF( quat5.W(), quat1.W(), &ok ); TESTCHECKF( quat5.X(), quat1.X(), &ok ); TESTCHECKF( quat5.Y(), quat1.Y(), &ok ); TESTCHECKF( quat5.Z(), quat1.Z(), &ok ); cout << "GetEulerAngles( EulerAngles::YZX )" << endl; euler = quat5.GetEulerAngles( EulerAngles::YZX ); TESTCHECKF( euler[0].Radians(), a1, &ok ); TESTCHECKF( euler[1].Radians(), a2, &ok ); TESTCHECKF( euler[2].Radians(), a0, &ok ); a2 = static_cast<float>( - M_PI / 2. ); cout << "qrot2.Set( 2, " << a2 << " )" << endl; qrot2.Set( 2, Angle( a2 ) ); cout << "quat1 = qrot1 * qrot2 * qrot0" << endl; quat1 = qrot1 * qrot2 * qrot0; cout << "GetEulerAngles( EulerAngles::YZX )" << endl; euler = quat1.GetEulerAngles( EulerAngles::YZX ); Angle a10( a1 - a0 ); a10.Normalize(); float a1_a0 = static_cast<float>( a10.Radians() ); TESTCHECKF( euler[0].Radians(), a1_a0, &ok ); TESTCHECKF( euler[1].Radians(), a2, &ok ); TESTCHECKF( euler[2].Radians(), 0.f, &ok ); cout << "Matrix().GetEulerAngles( EulerAngles::YZX )" << endl; euler = quat1.Matrix().GetEulerAngles( EulerAngles::YZX ); TESTCHECKF( euler[0].Radians(), a1_a0, &ok ); TESTCHECKF( euler[1].Radians(), a2, &ok ); TESTCHECKF( euler[2].Radians(), 0.f, &ok ); cout << "Set( EulerAngles( " << a1 << ", " << a2 << ", " << a0 << " ), EulerAngles::YZX )" << endl; quat5.Set( EulerAngles( a1, a2, a0 ), EulerAngles::YZX ); TESTCHECKF( quat5.W(), quat1.W(), &ok ); TESTCHECKF( quat5.X(), quat1.X(), &ok ); TESTCHECKF( quat5.Y(), quat1.Y(), &ok ); TESTCHECKF( quat5.Z(), quat1.Z(), &ok ); cout << "GetEulerAngles( EulerAngles::YZX )" << endl; euler = quat5.GetEulerAngles( EulerAngles::YZX ); TESTCHECKF( euler[0].Radians(), a1_a0, &ok ); TESTCHECKF( euler[1].Radians(), a2, &ok ); TESTCHECKF( euler[2].Radians(), 0.f, &ok ); a0 = 1.1f; a1 = -1.0f; a2 = 0.5f; cout << "qrot0.Set( 0, " << a0 << " )" << endl; qrot0.Set( 0, Angle( a0 ) ); cout << "qrot1.Set( 1, " << a1 << " )" << endl; qrot1.Set( 1, Angle( a1 ) ); cout << "qrot2.Set( 2, " << a2 << " )" << endl; qrot2.Set( 2, Angle( a2 ) ); cout << "quat1 = qrot2 * qrot1 * qrot0" << endl; quat1 = qrot2 * qrot1 * qrot0; cout << "GetEulerAngles( EulerAngles::ZYX )" << endl; euler = quat1.GetEulerAngles( EulerAngles::ZYX ); TESTCHECKF( euler[0].Radians(), a2, &ok ); TESTCHECKF( euler[1].Radians(), a1, &ok ); TESTCHECKF( euler[2].Radians(), a0, &ok ); cout << "Matrix().GetEulerAngles( EulerAngles::ZYX )" << endl; euler = quat1.Matrix().GetEulerAngles( EulerAngles::ZYX ); TESTCHECKF( euler[0].Radians(), a2, &ok ); TESTCHECKF( euler[1].Radians(), a1, &ok ); TESTCHECKF( euler[2].Radians(), a0, &ok ); cout << "Set( EulerAngles( " << a2 << ", " << a1 << ", " << a0 << " ), EulerAngles::ZYX )" << endl; quat5.Set( EulerAngles( a2, a1, a0 ), EulerAngles::ZYX ); TESTCHECKF( quat5.W(), quat1.W(), &ok ); TESTCHECKF( quat5.X(), quat1.X(), &ok ); TESTCHECKF( quat5.Y(), quat1.Y(), &ok ); TESTCHECKF( quat5.Z(), quat1.Z(), &ok ); cout << "GetEulerAngles( EulerAngles::ZYX )" << endl; euler = quat5.GetEulerAngles( EulerAngles::ZYX ); TESTCHECKF( euler[0].Radians(), a2, &ok ); TESTCHECKF( euler[1].Radians(), a1, &ok ); TESTCHECKF( euler[2].Radians(), a0, &ok ); a1 = static_cast<float>( M_PI / 2. ); cout << "qrot1.Set( 1, " << a1 << " )" << endl; qrot1.Set( 1, Angle( a1 ) ); cout << "quat1 = qrot2 * qrot1 * qrot0" << endl; quat1 = qrot2 * qrot1 * qrot0; cout << "GetEulerAngles( EulerAngles::ZYX )" << endl; euler = quat1.GetEulerAngles( EulerAngles::ZYX ); Angle a20 = Angle( a2 - a0 ); a20.Normalize(); float a2_a0 = static_cast<float>( a20.Radians() ); TESTCHECKF( euler[0].Radians(), a2_a0, &ok ); TESTCHECKF( euler[1].Radians(), a1, &ok ); TESTCHECKF( euler[2].Radians(), 0.f, &ok ); cout << "Set( EulerAngles( " << a2 << ", " << a1 << ", " << a0 << " ), EulerAngles::ZYX )" << endl; quat5.Set( EulerAngles( a2, a1, a0 ), EulerAngles::ZYX ); TESTCHECKF( quat5.W(), quat1.W(), &ok ); TESTCHECKF( quat5.X(), quat1.X(), &ok ); TESTCHECKF( quat5.Y(), quat1.Y(), &ok ); TESTCHECKF( quat5.Z(), quat1.Z(), &ok ); cout << "GetEulerAngles( EulerAngles::ZYX )" << endl; euler = quat5.GetEulerAngles( EulerAngles::ZYX ); TESTCHECKF( euler[0].Radians(), a2_a0, &ok ); TESTCHECKF( euler[1].Radians(), a1, &ok ); TESTCHECKF( euler[2].Radians(), 0.f, &ok ); if ( ok ) cout << "Quaternion PASSED." << endl << endl; else cout << "Quaternion FAILED." << endl << endl; return ok; }