//-----------------------------------------------------------------------------
//
//  $Logfile:: /Code/DLLs/game/q_mathsys.c                                   $
// $Revision:: 6                                                              $
//   $Author:: Steven                                                         $
//     $Date:: 10/10/02 1:13p                                                 $
//
// Copyright (C) 2001 by Ritual Entertainment, Inc.
// All rights reserved.
//
// This source may not be distributed and/or modified without
// expressly written permission by Ritual Entertainment, Inc.
//
//
// DESCRIPTION:

#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( const float in1[3][3], const 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.0f / 360.0f );
	sy = sin(angle);
	cy = cos(angle);
	angle = angles[PITCH] * ( M_PI * 2.0f / 360.0f );
	sp = sin(angle);
	cp = cos(angle);
	angle = angles[ROLL] * ( M_PI * 2.0f / 360.0f );
	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 AxisToAngles( vec3_t axis[3], vec3_t angles ) {
	MatrixToEulerAngles( axis, angles );
	//vectoangles(axis[0], angles);
}

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.0f / 360.0f );
	sy = sin(angle);
	cy = cos(angle);
	angle = angles[PITCH] * ( M_PI * 2.0f / 360.0f );
	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.0f / 360.0f );
   	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 );
}