#include "q_shared.h"
#include "float.h"

// these routines will be handled as system calls
// for interpreted code to speed things up

#define X            0
#define Y            1
#define Z            2
#define W            3
#define QUAT_EPSILON 0.00001

/*
================
MatrixMultiply
================
*/
void MatrixMultiply( float in1[3][3], float in2[3][3], float out[3][3] )
{
	out[0][0] = in1[0][0] * in2[0][0] + in1[0][1] * in2[1][0]
		    + in1[0][2] * in2[2][0];
	out[0][1] = in1[0][0] * in2[0][1] + in1[0][1] * in2[1][1]
		    + in1[0][2] * in2[2][1];
	out[0][2] = in1[0][0] * in2[0][2] + in1[0][1] * in2[1][2]
		    + in1[0][2] * in2[2][2];
	out[1][0] = in1[1][0] * in2[0][0] + in1[1][1] * in2[1][0]
		    + in1[1][2] * in2[2][0];
	out[1][1] = in1[1][0] * in2[0][1] + in1[1][1] * in2[1][1]
		    + in1[1][2] * in2[2][1];
	out[1][2] = in1[1][0] * in2[0][2] + in1[1][1] * in2[1][2]
		    + in1[1][2] * in2[2][2];
	out[2][0] = in1[2][0] * in2[0][0] + in1[2][1] * in2[1][0]
		    + in1[2][2] * in2[2][0];
	out[2][1] = in1[2][0] * in2[0][1] + in1[2][1] * in2[1][1]
		    + in1[2][2] * in2[2][1];
	out[2][2] = in1[2][0] * in2[0][2] + in1[2][1] * in2[1][2]
		    + in1[2][2] * in2[2][2];
}

void AnglesToAxis( const vec3_t angles, vec3_t axis[3] )
{
	float        angle;
	static float sr, sp, sy, cr, cp, cy;
	// static to help MS compiler fp bugs

	angle = angles[YAW] * ( M_PI * 2 / 360 );
	sy = sin( angle );
	cy = cos( angle );
	angle = angles[PITCH] * ( M_PI * 2 / 360 );
	sp = sin( angle );
	cp = cos( angle );
	angle = angles[ROLL] * ( M_PI * 2 / 360 );
	sr = sin( angle );
	cr = cos( angle );

	axis[0][0] = cp * cy;
	axis[0][1] = cp * sy;
	axis[0][2] = -sp;

	axis[1][0] = ( sr * sp * cy + cr * -sy );
	axis[1][1] = ( sr * sp * sy + cr * cy );
	axis[1][2] = sr * cp;

	axis[2][0] = ( cr * sp * cy + -sr * -sy );
	axis[2][1] = ( cr * sp * sy + -sr * cy );
	axis[2][2] = cr * cp;
}

void AngleVectors( const vec3_t angles, vec3_t forward, vec3_t left, vec3_t up )
{
	float        angle;
	static float sr, sp, sy, cr, cp, cy;
	// static to help MS compiler fp bugs

	angle = angles[YAW] * ( M_PI * 2 / 360 );
	sy = sin( angle );
	cy = cos( angle );
	angle = angles[PITCH] * ( M_PI * 2 / 360 );
	sp = sin( angle );
	cp = cos( angle );

	if( forward )
	{
		forward[0] = cp * cy;
		forward[1] = cp * sy;
		forward[2] = -sp;
	}

	if( left || up )
	{
		angle = angles[ROLL] * ( M_PI * 2 / 360 );
		sr = sin( angle );
		cr = cos( angle );

		if( left )
		{
			left[0] = ( sr * sp * cy + cr * -sy );
			left[1] = ( sr * sp * sy + cr * cy );
			left[2] = sr * cp;
		}
		if( up )
		{
			up[0] = ( cr * sp * cy + -sr * -sy );
			up[1] = ( cr * sp * sy + -sr * cy );
			up[2] = cr * cp;
		}
	}
}

/*
** assumes "src" is normalized
*/
void PerpendicularVector( vec3_t dst, const vec3_t src )
{
	int    pos;
	int    i;
	float  minelem = 1.0F;
	vec3_t tempvec;

	/*
	** find the smallest magnitude axially aligned vector
	*/
	for( pos = 0, i = 0; i < 3; i++ )
	{
		if( fabs( src[i] ) < minelem )
		{
			pos = i;
			minelem = fabs( src[i] );
		}
	}
	tempvec[0] = tempvec[1] = tempvec[2] = 0.0F;
	tempvec[pos] = 1.0F;

	/*
	** project the point onto the plane defined by src
	*/
	ProjectPointOnPlane( dst, tempvec, src );

	/*
	** normalize the result
	*/
	VectorNormalize( dst );
}
