TeslaRel410/CODE/RP/MUNGA/ROTATION.TCP

//===========================================================================//
// File:	rotation.tst                                                        //
// Project: MUNGA           Brick: Math Library                              //
// Contents: Test code for rotation classes                                  //
//---------------------------------------------------------------------------//
//   Date    Who  Modification                                               //
// --------  ---  ---------------------------------------------------------- //
// 11/20/94  JMA  Initial coding.                                            //
// 12/01/94  JMA  Made compatible with SGI CC                                //
//---------------------------------------------------------------------------//
// Copyright (C) 1994-1995, Virtual World Entertainment, Inc.                //
// All Rights reserved worldwide                                             //
// This unpublished sourcecode is PROPRIETARY and CONFIDENTIAL               //
//===========================================================================//

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ EulerAngles ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

//
//#############################################################################
//#############################################################################
//
Logical
	EulerAngles::TestClass()
{
	DEBUG_STREAM << "Starting EulerAngle test...\n";

	const EulerAngles
		a(Identity);
	EulerAngles
		b;
	const EulerAngles
		c(PI_OVER_4,PI_OVER_6,PI_OVER_3);

	Test(!a.pitch && !a.yaw && !a.roll);
	Test(c.pitch == PI_OVER_4 && c.yaw == PI_OVER_6 && c.roll == PI_OVER_3);

	Test(!a);
	b = c;
	Test(b == c);
	Test(b != a);

	Test(b[Y_Axis] == b.yaw);
	Test(c[Z_Axis] == c.roll);

	b.Lerp(a,c,0.5f);
	Test(b == EulerAngles(::Lerp(a.pitch,c.pitch,0.5f),::Lerp(a.yaw,c.yaw,0.5f),::Lerp(a.roll,c.roll,0.5f)));

	LinearMatrix m(True);
	m = c;
	b = m;
	Test(b == c);

	Quaternion q;
   q = Hinge(X_Axis, PI_OVER_4);
	b = q;
	Test(b == EulerAngles(PI_OVER_4,0.0f,0.0f));

	return True;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ Quaternion ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

//
//#############################################################################
//#############################################################################
//
#define NUMBER_OF_MULTIPLICATIONS 30
#include "random.hpp"
#include <fstream.h>
class fstream;
Logical
	Quaternion::TestClass()
{

	Tell("Starting Quaternion Test...\n");

#if 0
//	ofstream testout("gene.tst",ios::app);        //GY

	Vector3D
		r_vec;
	Quaternion
//		q[2],
		q[NUMBER_OF_MULTIPLICATIONS+1],
		multiplied_q(Identity),
		converted_q;
	LinearMatrix
//		lm[2],
		lm[NUMBER_OF_MULTIPLICATIONS+1],
		multiplied_lm(True);
	Scalar  r_float;

//cerr<<"Starting Quaternion::TestClass()\n";
	for (int k=0; k<TEST_SEQUENCE_LENGTH; ++k)
	{

		for(int i=0; i<=NUMBER_OF_MULTIPLICATIONS; ++i)
		{
		//----------------------
		//Generate random vector
		//----------------------
			for ( int j=0; j<3; ++j)
			{
				r_float = 2.0f*Random - 1.0f;
				r_vec[j] = r_float;
			}
		//----------------------
		//Normalize it
		//----------------------
		//	Tell("Normalize vector\n");
			r_vec.Normalize(r_vec);

		//Generate random angle
			Radian radian(TWO_PI*Random - PI);
		//----------------------
		//Make random quaternion
		//----------------------
		//	Tell("Make random quaternion\n");
			SinCosPair p;
			p = radian;
			Quaternion r_q(
				r_vec.x * p.sine,
				r_vec.y * p.sine,
				r_vec.z * p.sine,
				p.cosine
			);

			Check(&r_q);


			q[i] = r_q;
			lm[i] = r_q;
//     }
		//----------------------
		//Multiply quaternions
		//----------------------
		//	Tell("Multiply quaternions...\n");
//			multiplied_q.Multiply(q[1],q[0]);
			Quaternion previous_q(multiplied_q);
			//Tell("Multiply quaternions.\n");
			multiplied_q.Multiply(q[i],previous_q);
			//Tell("Multiplied quaternions\n");
			Check(&multiplied_q);

			if(i)
			{
		//----------------------
		//Normalize quaternion
		//----------------------
		//	Tell("Normalize quaternion\n");
			multiplied_q.Normalize();
			Check(&multiplied_q);
			}

		//----------------------
		//Multiply matrices
		//----------------------
		//	Tell("Multiply matrices...\n");
//			multiplied_lm.Multiply(lm[0],lm[1]);
			LinearMatrix previous_lm(multiplied_lm);
			//Tell("Multiply matrices.\n");
			multiplied_lm.Multiply(previous_lm,lm[i]);
			//Tell("Multiplied matrices\n");

//			if(i)
//cerr<<"Multiplied "<<i<<" times\n";
		}
//cerr<<"Multiplied "<<(i-1)<<" times\n";

		//----------------------
		//Convert matrix to quaternian
		//----------------------
		//Tell("Convert matrix to quaternian \n");
		converted_q = multiplied_lm;

		Check(&converted_q);

		//-----------------------
		// Compare results
		//-----------------------
		//Tell("Compare results\n");
		for (i=0;i<4;i++)
		{
			if (!Close_Enough(converted_q[i],
									multiplied_q[i],
									SMALL)//4.91e-3f)//2.62e-3f) //1.9874e-3f)//1.66e-4f)//1.35e-4f)//8.0e-6f) //2.75e-06f)//SMALL)
			)
			{
//cerr<<"i= "<<i<<" delta= "<<(converted_q[i]-multiplied_q[i])<<endl;
//testout<<"i= "<<i<<" delta= "<<(converted_q[i]-multiplied_q[i])<<endl;

			}
		}
//cerr<<"SUCCESS at "<<(k+1)<<" tests !!!!!\n";
	}
//	testout.close();
//cerr<<"Quaternion::TestClass  done !\n";
	//Tell("  Quaternion::TestClass is stubbed out!\n");
#endif
	return False;
}