#include <windows.h>

#include <math.h>



#include "Math.h"





/*****************************************************************************/



float

	SinTable[MAX_ANGLES];





/*****************************************************************************/



void Math_Setup()

{

	double

		angle = 0;

	long

		i;



	for (i=0; i<MAX_ANGLES; i++)

	{

		SinTable[i] = (float)sin( angle );

		angle += (2*PI/MAX_ANGLES);

	}

}





/*****************************************************************************/



Vector3 Vector3::operator +( Vector3 &v )

{

	Vector3

		result;



	result.x = x + v.x;

	result.y = y + v.y;

	result.z = z + v.z;



	return( result );

}





Vector3 Vector3::operator -( Vector3 &v )

{

	Vector3

		result;



	result.x = x - v.x;

	result.y = y - v.y;

	result.z = z - v.z;



	return( result );

}





float Vector3::operator *( Vector3 &v )

{

	float

		result;



	result  = x * v.x;

	result += y * v.y;

	result += z * v.z;



	return( result );

}





Vector3& Vector3::operator -=( Vector3 &v )

{

	x -= v.x;

	y -= v.y;

	z -= v.z;



	return( *this );

}





int Vector3::operator ==( Vector3 &v )

{

	if (x == v.x &&

			y == v.y &&

			z == v.z)

		return( TRUE );



	return( FALSE );

}





void Vector3::Normalize()

{

	float

		length;

	

	length = 0;



	length += (x * x);

	length += (y * y);

	length += (z * z);



	length = (float)sqrt( length );



	if (length == 0)

		return;



	length = 1.0F / length;



	x *= length;

	y *= length;

	z *= length;

}





void Vector3::Negate()

{

	x = -x;

	y = -y;

	z = -z;

}





float DotProduct( Vector3 *a, Vector3 *b )

{

	float

		result;



	result  = (a->x * b->x);

	result += (a->y * b->y);

	result += (a->z * b->z);



	return( result );

}





float DotProduct( Vector3 &a, Vector3 &b )

{

	float

		result;



	result  = (a.x * b.x);

	result += (a.y * b.y);

	result += (a.z * b.z);



	return( result );

}





Vector3 CrossProduct( Vector3 *a, Vector3 *b )

{

	Vector3

		result;



	result.x = (a->y * b->z) - (a->z * b->y);

	result.y = (a->z * b->x) - (a->x * b->z);

	result.z = (a->x * b->y) - (a->y * b->x);



	return( result );

}





Vector3 CrossProduct( Vector3 &a, Vector3 &b )

{

	Vector3

		result;



	result.x = (a.y * b.z) - (a.z * b.y);

	result.y = (a.z * b.x) - (a.x * b.z);

	result.z = (a.x * b.y) - (a.y * b.x);



	return( result );

}





/*****************************************************************************/



void Matrix3::SetIdentity()

{

	m[0][0] = 1;

	m[0][1] = 0;

	m[0][2] = 0;



	m[1][0] = 0;

	m[1][1] = 1;

	m[1][2] = 0;



	m[2][0] = 0;

	m[2][1] = 0;

	m[2][2] = 1;



	t[0] = 0;

	t[1] = 0;

	t[2] = 0;

}





void Matrix3::SetScale( float scale )

{

	m[0][0] = scale;

	m[0][1] = 0;

	m[0][2] = 0;



	m[1][0] = 0;

	m[1][1] = scale;

	m[1][2] = 0;



	m[2][0] = 0;

	m[2][1] = 0;

	m[2][2] = scale;



	t[0] = 0;

	t[1] = 0;

	t[2] = 0;

}





void Matrix3::SetScale( float *scale )

{

	m[0][0] = scale[0];

	m[0][1] = 0;

	m[0][2] = 0;



	m[1][0] = 0;

	m[1][1] = scale[1];

	m[1][2] = 0;



	m[2][0] = 0;

	m[2][1] = 0;

	m[2][2] = scale[2];



	t[0] = 0;

	t[1] = 0;

	t[2] = 0;

}





void Matrix3::SetTrans( Vector3 *trans )

{

	t[0] = trans->x;

	t[1] = trans->y;

	t[2] = trans->z;

}





void Matrix3::SetTrans( float *trans )

{

	t[0] = trans[0];

	t[1] = trans[1];

	t[2] = trans[2];

}





void Matrix3::SetTrans( float x, float y, float z )

{

	t[0] = x;

	t[1] = y;

	t[2] = z;

}





void Matrix3::GetTrans( Vector3 *trans )

{

	trans->x = t[0];

	trans->y = t[1];

	trans->z = t[2];

}





void Matrix3::GetTrans( float *trans )

{

	trans[0] = t[0];

	trans[1] = t[1];

	trans[2] = t[2];

}





void Matrix3::ScaleBy( float scale )

{

	m[0][0] *= scale;

	m[0][1] *= scale;

	m[0][2] *= scale;



	m[1][0] *= scale;

	m[1][1] *= scale;

	m[1][2] *= scale;



	m[2][0] *= scale;

	m[2][1] *= scale;

	m[2][2] *= scale;

}





void Matrix3::RotateByX( s32 angle )

{

	Matrix3

		work;

	s32

		cos_ang;



	angle &= ANGLE_MASK;

	cos_ang = (angle + (MAX_ANGLES/4)) & ANGLE_MASK;



	work.SetIdentity();

	work.m[1][1] = SinTable[cos_ang];

	work.m[1][2] = SinTable[angle];

	work.m[2][1] = -SinTable[angle];

	work.m[2][2] = SinTable[cos_ang];

	MulByMatrix( &work );

}





void Matrix3::RotateByY( s32 angle )

{

	Matrix3

		work;

	s32

		cos_ang;



	angle &= ANGLE_MASK;

	cos_ang = (angle + (MAX_ANGLES/4)) & ANGLE_MASK;



	work.SetIdentity();

	work.m[0][0] = SinTable[cos_ang];

	work.m[0][2] = -SinTable[angle];

	work.m[2][0] = SinTable[angle];

	work.m[2][2] = SinTable[cos_ang];

	MulByMatrix( &work );

}





void Matrix3::RotateByZ( s32 angle )

{

	Matrix3

		work;

	s32

		cos_ang;



	angle &= ANGLE_MASK;

	cos_ang = (angle + (MAX_ANGLES/4)) & ANGLE_MASK;



	work.SetIdentity();

	work.m[0][0] = SinTable[cos_ang];

	work.m[0][1] = SinTable[angle];

	work.m[1][0] = -SinTable[angle];

	work.m[1][1] = SinTable[cos_ang];

	MulByMatrix( &work );

}





void Matrix3::SetRotateX( s32 angle )

{

	s32

		cos_ang;



	angle &= ANGLE_MASK;

	cos_ang = (angle + (MAX_ANGLES/4)) & ANGLE_MASK;



	SetIdentity();

	m[1][1] = SinTable[cos_ang];

	m[1][2] = SinTable[angle];

	m[2][1] = -SinTable[angle];

	m[2][2] = SinTable[cos_ang];

}





void Matrix3::SetRotateY( s32 angle )

{

	s32

		cos_ang;



	angle &= ANGLE_MASK;

	cos_ang = (angle + (MAX_ANGLES/4)) & ANGLE_MASK;



	SetIdentity();

	m[0][0] = SinTable[cos_ang];

	m[0][2] = -SinTable[angle];

	m[2][0] = SinTable[angle];

	m[2][2] = SinTable[cos_ang];

}





void Matrix3::SetRotateZ( s32 angle )

{

	s32

		cos_ang;



	angle &= ANGLE_MASK;

	cos_ang = (angle + (MAX_ANGLES/4)) & ANGLE_MASK;



	SetIdentity();

	m[0][0] = SinTable[cos_ang];

	m[0][1] = SinTable[angle];

	m[1][0] = -SinTable[angle];

	m[1][1] = SinTable[cos_ang];

}





void Matrix3::MulByMatrix( Matrix3 *matrix )

{

#if 0



	float

		work,

		mat[3][3];

	s32

		i,

		j,

		k;



	for (j=0; j<3; j++)

	{

		for (i=0; i<3; i++)

		{

			work = 0;



			for (k=0; k<3; k++)

				work += m[k][i] * matrix->m[j][k];

//				work += m[j][k] * matrix->m[k][i];



			mat[j][i] = work;

		}

	}



	for (j=0; j<3; j++)

	{

		for (i=0; i<3; i++)

			m[i][j] = mat[j][i];

	}



#else



	Vector3

		v[3];

	int

		i;



	for (i=0; i<3; i++)

	{

		v[i].x = m[i][0];

		v[i].y = m[i][1];

		v[i].z = m[i][2];

		matrix->Multiply( &v[i] );

	}



	for (i=0; i<3; i++)

	{

		m[i][0] = v[i].x;

		m[i][1] = v[i].y;

		m[i][2] = v[i].z;

	}



#endif

}





void Matrix3::Multiply( Vector3 *vector )

{

	float

		x,

		y,

		z;



	x = (vector->x * m[0][0]) +

			(vector->y * m[1][0]) +

			(vector->z * m[2][0]);



	y = (vector->x * m[0][1]) +

			(vector->y * m[1][1]) +

			(vector->z * m[2][1]);



	z = (vector->x * m[0][2]) +

			(vector->y * m[1][2]) +

			(vector->z * m[2][2]);



	vector->x = x;

	vector->y = y;

	vector->z = z;

}





void Matrix3::Transform( Vector3 *vector )

{

	float

		x,

		y,

		z;



	x = (vector->x * m[0][0]) +

			(vector->y * m[1][0]) +

			(vector->z * m[2][0]) + t[0];



	y = (vector->x * m[0][1]) +

			(vector->y * m[1][1]) +

			(vector->z * m[2][1]) + t[1];



	z = (vector->x * m[0][2]) +

			(vector->y * m[1][2]) +

			(vector->z * m[2][2]) + t[2];



	vector->x = x;

	vector->y = y;

	vector->z = z;

}





void Matrix3::Transform( Matrix3 *matrix )

{

	float

		work,

		mat[3][3];

	s32

		i,

		j,

		k;



matrix->MulByMatrix( this );

Multiply( (Vector3 *)matrix->t );



#if 0

	for (j=0; j<3; j++)

	{

		for (i=0; i<3; i++)

		{

			work = 0;



			for (k=0; k<3; k++)

				work += matrix->m[k][i] * m[j][k];



			mat[j][i] = work;

		}

	}



	Multiply( (Vector3 *)matrix->t );



	memcpy( matrix->m, mat, 4*3*sizeof( float ) );

#endif



	matrix->t[0] += t[0];

	matrix->t[1] += t[1];

	matrix->t[2] += t[2];

}





void Matrix3::CopyToMem( float *mem )

{

	memcpy( mem, m, 3*4*sizeof( float ) );

}





void Matrix3::CopyFromMem( float *mem )

{

	memcpy( m, mem, 3*4*sizeof( float ) );

}