void llquat_test_object_t::test<12>()
{
LLVector3d vect(-2.0f, 5.0f, -6.0f);
LLQuaternion quat(-3.5f, 4.5f, 3.5f, 6.5f);
LLVector3d result = vect * quat;
ensure(
"1. LLVector3d operator*(const LLVector3d &a, const LLQuaternion &rot) failed ",
(-633.0f == result.mdV[0]) &&
(-300.0f == result.mdV[1]) &&
(-36.0f == result.mdV[2]));
LLVector3d vect1(5.0f, -4.5f, 8.21f);
LLQuaternion quat1(2.0f, 4.5f, -7.2f, 9.5f);
result = vect1 * quat1;
ensure(
"2. LLVector3d operator*(const LLVector3d &a, const LLQuaternion &rot) failed",
is_approx_equal_fraction(-120.29f, (F32) result.mdV[0], 8) &&
is_approx_equal_fraction(-1683.958f, (F32) result.mdV[1], 8) &&
is_approx_equal_fraction(516.56f, (F32) result.mdV[2], 8));
LLVector3d vect2(2.0f, 3.5f, 1.1f);
LLQuaternion quat2(1.0f, 4.0f, 2.0f, 5.0f);
result = vect2 * quat2;
ensure(
"3. LLVector3d operator*(const LLVector3d &a, const LLQuaternion &rot) failed",
is_approx_equal_fraction(18.400001f, (F32) result.mdV[0], 8) &&
is_approx_equal_fraction(188.6f, (F32) result.mdV[1], 8) &&
is_approx_equal_fraction(32.20f, (F32) result.mdV[2], 8));
}
static void go()
{
gmtl::Quat<T> quat1((T)1.0, (T)2.0, (T)34.0, (T)4.0),
quat2(-(T)1.0, -(T)2.0, -(T)34.0, -(T)4.0),
quat3((T)1.0, (T)2.0, (T)34.0, (T)4.0),
quat4;
// Test for geometric equivelency
CPPUNIT_ASSERT( gmtl::isEquiv( quat1, quat2 ) );
CPPUNIT_ASSERT( gmtl::isEquiv( quat1, quat2, (T)0.0f ) );
CPPUNIT_ASSERT( gmtl::isEquiv( quat2, quat1, (T)0.0f ) );
CPPUNIT_ASSERT( gmtl::isEquiv( quat2, quat1, (T)100000.0f ) );
// Test for geometric equivelency
CPPUNIT_ASSERT( gmtl::isEquiv( quat1, quat3 ) );
CPPUNIT_ASSERT( gmtl::isEquiv( quat1, quat3, (T)0.0f ) );
CPPUNIT_ASSERT( gmtl::isEquiv( quat3, quat1, (T)0.0f ) );
CPPUNIT_ASSERT( gmtl::isEquiv( quat3, quat1, (T)100000.0f ) );
// Test for geometric un-equivelency
CPPUNIT_ASSERT( !gmtl::isEquiv( quat1, quat4 ) );
CPPUNIT_ASSERT( !gmtl::isEquiv( quat1, quat4, (T)0.0f ) );
CPPUNIT_ASSERT( !gmtl::isEquiv( quat4, quat1, (T)0.0f ) );
CPPUNIT_ASSERT( !gmtl::isEquiv( quat4, quat1, (T)30.0f ) );
}
inline double SimpleRotateFrame::calc_angle(
double rx1, double ry1, double rz1,
double rx2, double ry2, double rz2) const
{
Quaternion quat1(rx1, ry1, rz1);
Quaternion quat2(ry2, ry2, rz2);
return quat1.angle_to(quat2);
}
void llquat_test_object_t::test<6>()
{
LLQuaternion quat1(3.0f, 2.0f, 6.0f, 0.0f), quat2(1.0f, 1.0f, 1.0f, 1.0f);
ensure("1. The two values are different", llround(12.000000f, 2) == llround(dot(quat1, quat2), 2));
LLQuaternion quat0(3.0f, 9.334f, 34.5f, 23.0f), quat(34.5f, 23.23f, 2.0f, 45.5f);
ensure("2. The two values are different", llround(1435.828807f, 2) == llround(dot(quat0, quat), 2));
}
int main(int argc, char* argv[]) {
omp_set_nested(1);
stepMode = true;
GPUSystem = new System(1);
GPUSystem->mTimeStep = .001;
GPUSystem->mEndTime = 35;
GPUSystem->mNumObjects = 1;
GPUSystem->mIterations = 100;
GPUSystem->mTolerance = 1e-5;
GPUSystem->mOmegaContact = .9;
GPUSystem->mOmegaBilateral = .2;
GPUSystem->mUseOGL = 1;
float mMu = .5;
float mWallMu = .5;
if (argc == 2) {
numY = atoi(argv[1]);
} else {
cout << "ARGS: number of particle layers in y direction" << endl;
exit(1);
}
float container_R = 10.0, container_T = .1;
ChQuaternion<> quat(1, 0, 0, 0);
ChVector<> lpos(0, 0, 0);
CHBODYSHAREDPTR L = CHBODYSHAREDPTR(new CHBODY);
CHBODYSHAREDPTR R = CHBODYSHAREDPTR(new CHBODY);
CHBODYSHAREDPTR F = CHBODYSHAREDPTR(new CHBODY);
CHBODYSHAREDPTR B = CHBODYSHAREDPTR(new CHBODY);
CHBODYSHAREDPTR BTM = CHBODYSHAREDPTR(new CHBODY);
CHBODYSHAREDPTR FREE = CHBODYSHAREDPTR(new CHBODY);
ChQuaternion<> quat2(1, 0, 0, 0);
quat2.Q_from_AngAxis(PI / 6.0, ChVector<> (1, 0, 0));
//GPUSystem->InitObject(L, 100000, ChVector<> (-container_R, 0, 0), quat, mWallMu, mWallMu, 0, true, true, -20, -20);
//GPUSystem->InitObject(R, 100000, ChVector<> (container_R, 0, 0), quat, mWallMu, mWallMu, 0, true, true, -20, -20);
//GPUSystem->InitObject(F, 100000, ChVector<> (0, 0, -container_R), quat, mWallMu, mWallMu, 0, true, true, -20, -20);
//GPUSystem->InitObject(B, 100000, ChVector<> (0, 0, container_R), quat, mWallMu, mWallMu, 0, true, true, -20, -20);
GPUSystem->InitObject(BTM, 1, ChVector<> (0, -container_R, 0), quat, mWallMu, mWallMu, 0, true, true, -1000, -20000);
//GPUSystem->AddCollisionGeometry(L, BOX, ChVector<> (container_T, container_R, container_R), lpos, quat);
//GPUSystem->AddCollisionGeometry(R, BOX, ChVector<> (container_T, container_R, container_R), lpos, quat);
//GPUSystem->AddCollisionGeometry(F, BOX, ChVector<> (container_R, container_R, container_T), lpos, quat);
//GPUSystem->AddCollisionGeometry(B, BOX, ChVector<> (container_R, container_R, container_T), lpos, quat);
GPUSystem->AddCollisionGeometry(BTM, BOX, ChVector<> (container_R, container_T, container_R), lpos, quat);
//GPUSystem->FinalizeObject(L);
//GPUSystem->FinalizeObject(R);
//GPUSystem->FinalizeObject(F);
//GPUSystem->FinalizeObject(B);
GPUSystem->FinalizeObject(BTM);
((ChLcpSolverGPU*) (GPUSystem->mSystem->GetLcpSolverSpeed()))->SetContactFactor(.6);
GPUSystem->Setup();
SimulationLoop(argc, argv);
return 0;
}
void ArcBall::lookAT(double theta, double phi)
{
theta = -theta;
phi = -phi;
double sinphi = sin(phi / 2.0);
double cosphi = cos(phi / 2.0);
double sintheta = sin(theta / 2.0);
double costheta = cos(theta / 2.0);
Quat quat1(0, 0, sinphi, cosphi);
Quat quat2(-sin(-phi) * sintheta, cos(-phi) * sintheta, 0, costheta);
qNow = quat1 * quat2;
qDown = quat1 * quat2;
}
void llquat_test_object_t::test<9>()
{
//test case for LLQuaternion operator*(const LLQuaternion &a, const LLQuaternion &b) fn
LLQuaternion quat1(1.0f, 2.5f, 3.5f, 5.5f);
LLQuaternion quat2(4.0f, 3.0f, 5.0f, 1.0f);
LLQuaternion result = quat1 * quat2;
ensure("1. LLQuaternion Operator* failed", (21.0f == result.mQ[0]) &&
(10.0f == result.mQ[1]) &&
(38.0f == result.mQ[2]) &&
(-23.5f == result.mQ[3]));
LLQuaternion quat3(2341.340f, 2352.345f, 233.25f, 7645.5f);
LLQuaternion quat4(674.067f, 893.0897f, 578.0f, 231.0f);
result = quat3 * quat4;
ensure("2. LLQuaternion Operator* failed", (4543086.5f == result.mQ[0]) &&
(8567578.0f == result.mQ[1]) &&
(3967591.25f == result.mQ[2]) &&
is_approx_equal(-2047783.25f, result.mQ[3]));
//inline LLQuaternion operator+(const LLQuaternion &a, const LLQuaternion &b)fn.
result = quat1 + quat2;
ensure("3. LLQuaternion operator+ failed", (5.0f == result.mQ[0]) &&
(5.5f == result.mQ[1]) &&
(8.5f == result.mQ[2]) &&
(6.5f == result.mQ[3]));
result = quat3 + quat4;
ensure(
"4. LLQuaternion operator+ failed",
is_approx_equal(3015.407227f, result.mQ[0]) &&
is_approx_equal(3245.434570f, result.mQ[1]) &&
(811.25f == result.mQ[2]) &&
(7876.5f == result.mQ[3]));
//inline LLQuaternion operator-(const LLQuaternion &a, const LLQuaternion &b) fn
result = quat1 - quat2;
ensure(
"5. LLQuaternion operator-(const LLQuaternion &a, const LLQuaternion &b) failed",
(-3.0f == result.mQ[0]) &&
(-0.5f == result.mQ[1]) &&
(-1.5f == result.mQ[2]) &&
(4.5f == result.mQ[3]));
result = quat3 - quat4;
ensure(
"6. LLQuaternion operator-(const LLQuaternion &a, const LLQuaternion &b) failed",
is_approx_equal(1667.273071f, result.mQ[0]) &&
is_approx_equal(1459.255249f, result.mQ[1]) &&
(-344.75f == result.mQ[2]) &&
(7414.50f == result.mQ[3]));
}
void QuatCompareMetricTest::testQuatTimingEquiv()
{
gmtl::Quatf quat1(1.0, 2.0, 34.0, 4.0),
quat2(-1.0, -2.0, -34.0, -4.0);
unsigned true_count(0);
const long iters(200000);
for( long iter=0;iter<iters; ++iter)
{
if (gmtl::isEquiv( quat1, quat2, 0.0001f ) )
++true_count;
}
// Make sure the compiler doesn't optimize out true_count
CPPUNIT_ASSERT( true_count > 0 );
}
void QuatCompareMetricTest::testQuatTimingEquiv()
{
gmtl::Quatf quat1(1.0, 2.0, 34.0, 4.0),
quat2(-1.0, -2.0, -34.0, -4.0);
unsigned true_count(0);
const long iters(200000);
CPPUNIT_METRIC_START_TIMING();
for( long iter=0;iter<iters; ++iter)
{
if (gmtl::isEquiv( quat1, quat2, 0.0001f ) )
++true_count;
}
CPPUNIT_METRIC_STOP_TIMING();
CPPUNIT_ASSERT_METRIC_TIMING_LE("QuatCompareTest/isEquiv(quat,quat,tol)", iters, 0.075f, 0.1f); // warn at 7.5%, error at 10%
// Make sure the compiler doesn't optimize out true_count
CPPUNIT_ASSERT( true_count > 0 );
}
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;
}
void llquat_test_object_t::test<8>()
{
F32 value1 = 15.0f;
LLQuaternion quat1(1.0f, 2.0f, 4.0f, 1.0f);
LLQuaternion quat2(4.0f, 3.0f, 6.5f, 9.7f);
LLQuaternion res_lerp, res_slerp, res_nlerp;
//test case for lerp(F32 t, const LLQuaternion &q) fn.
res_lerp = lerp(value1, quat1);
ensure("1. LLQuaternion lerp(F32 t, const LLQuaternion &q) failed",
is_approx_equal_fraction(0.181355f, res_lerp.mQ[0], 16) &&
is_approx_equal_fraction(0.362711f, res_lerp.mQ[1], 16) &&
is_approx_equal_fraction(0.725423f, res_lerp.mQ[2], 16) &&
is_approx_equal_fraction(0.556158f, res_lerp.mQ[3], 16));
//test case for lerp(F32 t, const LLQuaternion &p, const LLQuaternion &q) fn.
res_lerp = lerp(value1, quat1, quat2);
ensure("2. LLQuaternion lerp(F32 t, const LLQuaternion &p, const LLQuaternion &q) failed",
is_approx_equal_fraction(0.314306f, res_lerp.mQ[0], 16) &&
is_approx_equal_fraction(0.116156f, res_lerp.mQ[1], 16) &&
is_approx_equal_fraction(0.283559f, res_lerp.mQ[2], 16) &&
is_approx_equal_fraction(0.898506f, res_lerp.mQ[3], 16));
//test case for slerp( F32 u, const LLQuaternion &a, const LLQuaternion &b ) fn.
res_slerp = slerp(value1, quat1, quat2);
ensure("3. LLQuaternion slerp( F32 u, const LLQuaternion &a, const LLQuaternion &b) failed",
is_approx_equal_fraction(46.000f, res_slerp.mQ[0], 16) &&
is_approx_equal_fraction(17.00f, res_slerp.mQ[1], 16) &&
is_approx_equal_fraction(41.5f, res_slerp.mQ[2], 16) &&
is_approx_equal_fraction(131.5f, res_slerp.mQ[3], 16));
//test case for nlerp(F32 t, const LLQuaternion &a, const LLQuaternion &b) fn.
res_nlerp = nlerp(value1, quat1, quat2);
ensure("4. LLQuaternion nlerp(F32 t, const LLQuaternion &a, const LLQuaternion &b) failed",
is_approx_equal_fraction(0.314306f, res_nlerp.mQ[0], 16) &&
is_approx_equal_fraction(0.116157f, res_nlerp.mQ[1], 16) &&
is_approx_equal_fraction(0.283559f, res_nlerp.mQ[2], 16) &&
is_approx_equal_fraction(0.898506f, res_nlerp.mQ[3], 16));
//test case for nlerp(F32 t, const LLQuaternion &q) fn.
res_slerp = slerp(value1, quat1);
ensure("5. LLQuaternion slerp(F32 t, const LLQuaternion &q) failed",
is_approx_equal_fraction(1.0f, res_slerp.mQ[0], 16) &&
is_approx_equal_fraction(2.0f, res_slerp.mQ[1], 16) &&
is_approx_equal_fraction(4.0000f, res_slerp.mQ[2], 16) &&
is_approx_equal_fraction(1.000f, res_slerp.mQ[3], 16));
LLQuaternion quat3(2.0f, 1.0f, 5.5f, 10.5f);
LLQuaternion res_nlerp1;
value1 = 100.0f;
res_nlerp1 = nlerp(value1, quat3);
ensure("6. LLQuaternion nlerp(F32 t, const LLQuaternion &q) failed",
is_approx_equal_fraction(0.268245f, res_nlerp1.mQ[0], 16) && is_approx_equal_fraction(0.134122f, res_nlerp1.mQ[1], 2) &&
is_approx_equal_fraction(0.737673f, res_nlerp1.mQ[2], 16) &&
is_approx_equal_fraction(0.604892f, res_nlerp1.mQ[3], 16));
//test case for lerp(F32 t, const LLQuaternion &q) fn.
res_lerp = lerp(value1, quat2);
ensure("7. LLQuaternion lerp(F32 t, const LLQuaternion &q) failed",
is_approx_equal_fraction(0.404867f, res_lerp.mQ[0], 16) &&
is_approx_equal_fraction(0.303650f, res_lerp.mQ[1], 16) &&
is_approx_equal_fraction(0.657909f, res_lerp.mQ[2], 16) &&
is_approx_equal_fraction(0.557704f, res_lerp.mQ[3], 16));
}