/*
Copyright (C) 1997-2001 Id Software, Inc.

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  

See the GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.

*/
#include "q_shared.h"

vec3_t		vec3_origin = { 0, 0, 0 };
vec3_t		axis_identity[3] = { { 1, 0, 0 }, { 0, 1, 0 }, { 0, 0, 1 } };
quat_t		quat_identity = { 0, 0, 0, 1 };

//============================================================================

vec3_t	bytedirs[NUMVERTEXNORMALS] =
{
#include "anorms.h"
};

int DirToByte( vec3_t dir )
{
	int		i, best;
	float	d, bestd;
	qboolean normalized;

	if (!dir || VectorCompare(dir, vec3_origin))
		return NUMVERTEXNORMALS;

	if( DotProduct( dir, dir ) == 1 )
		normalized = qtrue;
	else
		normalized = qfalse;

	bestd = 0;
	best = 0;
	for( i = 0; i < NUMVERTEXNORMALS; i++ ) {
		d = DotProduct( dir, bytedirs[i] );
		if( (d == 1) && normalized )
			return i;
		if( d > bestd ) {
			bestd = d;
			best = i;
		}
	}

	return best;
}

void ByteToDir (int b, vec3_t dir)
{
	if (b < 0 || b >= NUMVERTEXNORMALS)
		VectorSet (dir, 0, 0, 0);
	else
		VectorCopy (bytedirs[b], dir);
}

//============================================================================

vec4_t		colorBlack	= {0, 0, 0, 1};
vec4_t		colorRed	= {1, 0, 0, 1};
vec4_t		colorGreen	= {0, 1, 0, 1};
vec4_t		colorBlue	= {0, 0, 1, 1};
vec4_t		colorYellow	= {1, 1, 0, 1};
vec4_t		colorOrange	= {1, 0.5, 0, 1};
vec4_t		colorMagenta= {1, 0, 1, 1};
vec4_t		colorCyan	= {0, 1, 1, 1};
vec4_t		colorWhite	= {1, 1, 1, 1};
vec4_t		colorLtGrey	= {0.75, 0.75, 0.75, 1};
vec4_t		colorMdGrey	= {0.5, 0.5, 0.5, 1};
vec4_t		colorDkGrey	= {0.25, 0.25, 0.25, 1};

vec4_t	color_table[MAX_S_COLORS] =
{
	{0.0, 0.0, 0.0, 1.0},
	{1.0, 0.0, 0.0, 1.0},
	{0.0, 1.0, 0.0, 1.0},
	{1.0, 1.0, 0.0, 1.0},
	{0.0, 0.0, 1.0, 1.0},
	{0.0, 1.0, 1.0, 1.0},
	{1.0, 0.0, 1.0, 1.0}, // magenta
	{1.0, 1.0, 1.0, 1.0},
	{1.0, 0.5, 0.0, 1.0}, // orange
	{0.5, 0.5, 0.5, 1.0}, // grey
};

/*
===============
ColorNormalize
===============
*/
vec_t ColorNormalize( const vec_t *in, vec_t *out )
{
	vec_t f = max( max( in[0], in[1] ), in[2] );

	if( f > 1.0f ) {
		f = 1.0f / f;
		out[0] = in[0] * f;
		out[1] = in[1] * f;
		out[2] = in[2] * f;
	} else {
		out[0] = in[0];
		out[1] = in[1];
		out[2] = in[2];
	}

	return f;
}

//============================================================================

void NormToLatLong ( const vec3_t normal, qbyte latlong[2] )
{
	// can't do atan2 (normal[1], normal[0])
	if ( normal[0] == 0 && normal[1] == 0 ) {
		if ( normal[2] > 0 ) {
			latlong[0] = 0;		// acos ( 1 )
			latlong[1] = 0;
		} else {
			latlong[0] = 128;	// acos ( -1 )
			latlong[1] = 0;
		}
	} else {
		int angle;

		angle = (int)( acos (normal[2]) * 255.0 / M_TWOPI ) & 255;
		latlong[0] = angle;
		angle = (int)( atan2 (normal[1], normal[0]) * 255.0 / M_TWOPI ) & 255;
		latlong[1] = angle;
	}
}

void MakeNormalVectors (const vec3_t forward, vec3_t right, vec3_t up)
{
	float		d;

	// this rotate and negate guarantees a vector not colinear with the original
	VectorSet (right, forward[2], -forward[0], forward[1] );
	d = DotProduct (right, forward);
	VectorMA (right, -d, forward, right);
	VectorNormalize (right);
	CrossProduct (right, forward, up);
}

void RotatePointAroundVector( vec3_t dst, const vec3_t dir, const vec3_t point, float degrees )
{
	float		t0, t1;
	float		c, s;
	vec3_t		vr, vu, vf;

	s = DEG2RAD (degrees);
	c = cos (s);
	s = sin (s);

	VectorCopy (dir, vf);
	MakeNormalVectors (vf, vr, vu);

	t0 = vr[0] * c + vu[0] * -s;
	t1 = vr[0] * s + vu[0] *  c;
	dst[0] = (t0 * vr[0] + t1 * vu[0] + vf[0] * vf[0]) * point[0]
		+ (t0 * vr[1] + t1 * vu[1] + vf[0] * vf[1]) * point[1]
		+ (t0 * vr[2] + t1 * vu[2] + vf[0] * vf[2]) * point[2];

	t0 = vr[1] * c + vu[1] * -s;
	t1 = vr[1] * s + vu[1] *  c;
	dst[1] = (t0 * vr[0] + t1 * vu[0] + vf[1] * vf[0]) * point[0]
		+ (t0 * vr[1] + t1 * vu[1] + vf[1] * vf[1]) * point[1]
		+ (t0 * vr[2] + t1 * vu[2] + vf[1] * vf[2]) * point[2];

	t0 = vr[2] * c + vu[2] * -s;
	t1 = vr[2] * s + vu[2] *  c;
	dst[2] = (t0 * vr[0] + t1 * vu[0] + vf[2] * vf[0]) * point[0]
		+ (t0 * vr[1] + t1 * vu[1] + vf[2] * vf[1]) * point[1]
		+ (t0 * vr[2] + t1 * vu[2] + vf[2] * vf[2]) * point[2];
}

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

	angle = DEG2RAD ( angles[YAW] );
	sy = sin(angle);
	cy = cos(angle);
	angle = DEG2RAD ( angles[PITCH] );
	sp = sin(angle);
	cp = cos(angle);
	angle = DEG2RAD ( angles[ROLL] );
	sr = sin(angle);
	cr = cos(angle);

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

void VecToAngles (const vec3_t vec, vec3_t angles)
{
	float	forward;
	float	yaw, pitch;
	
	if (vec[1] == 0 && vec[0] == 0)
	{
		yaw = 0;
		if (vec[2] > 0)
			pitch = 90;
		else
			pitch = 270;
	}
	else
	{
		if (vec[0])
			yaw = RAD2DEG ( atan2(vec[1], vec[0]) );
		else if (vec[1] > 0)
			yaw = 90;
		else
			yaw = -90;
		if (yaw < 0)
			yaw += 360;

		forward = sqrt (vec[0]*vec[0] + vec[1]*vec[1]);
		pitch = RAD2DEG ( atan2(vec[2], forward) );
		if (pitch < 0)
			pitch += 360;
	}

	angles[PITCH] = -pitch;
	angles[YAW] = yaw;
	angles[ROLL] = 0;
}

void AnglesToAxis( const vec3_t angles, vec3_t axis[3] )
{
	AngleVectors( angles, axis[0], axis[1], axis[2] );
	VectorInverse( axis[1] );
}

void BuildBoxPoints( vec3_t p[8], vec3_t org, vec3_t mins, vec3_t maxs )
{
	VectorAdd(org, mins, p[0]);
	VectorAdd(org, maxs, p[1]);
	VectorSet(p[2], p[0][0], p[0][1], p[1][2]);
	VectorSet(p[3], p[0][0], p[1][1], p[0][2]);
	VectorSet(p[4], p[0][0], p[1][1], p[1][2]);
	VectorSet(p[5], p[1][0], p[1][1], p[0][2]);
	VectorSet(p[6], p[1][0], p[0][1], p[1][2]);
	VectorSet(p[7], p[1][0], p[0][1], p[0][2]);
}

void ProjectPointOnPlane( vec3_t dst, const vec3_t p, const vec3_t normal )
{
	float d;
	vec3_t n;
	float inv_denom;

	inv_denom = 1.0F / DotProduct( normal, normal );

	d = DotProduct( normal, p ) * inv_denom;

	n[0] = normal[0] * inv_denom;
	n[1] = normal[1] * inv_denom;
	n[2] = normal[2] * inv_denom;

	dst[0] = p[0] - d * n[0];
	dst[1] = p[1] - d * n[1];
	dst[2] = p[2] - d * n[2];
}

/*
** 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 );
}

//============================================================================

float Q_RSqrt (float number)
{
	int i;
	float x2, y;

	if (number == 0.0)
		return 0.0;

	x2 = number * 0.5f;
	y = number;
	i = * (int *) &y;		// evil floating point bit level hacking
	i = 0x5f3759df - (i >> 1);		// what the fuck?
	y = * (float *) &i;
	y = y * (1.5f - (x2 * y * y));	// this can be done a second time

	return y;
}

int Q_rand ( int *seed )
{
	*seed = *seed * 1103515245 + 12345;
	return ((unsigned int)(*seed / 65536) % 32768);
}

/*
===============
LerpAngle

===============
*/
float LerpAngle (float a2, float a1, const float frac)
{
	if (a1 - a2 > 180)
		a1 -= 360;
	if (a1 - a2 < -180)
		a1 += 360;
	return a2 + frac * (a1 - a2);
}

/*
====================
CalcFov
====================
*/
float CalcFov (float fov_x, float width, float height)
{
	float	x;

	if (fov_x < 1 || fov_x > 179)
		Sys_Error ("Bad fov: %f", fov_x);

	x = width/tan(fov_x/360*M_PI);

	return atan (height/x)*360/M_PI;
}

float	anglemod(float a)
{
	a = (360.0/65536) * ((int)(a*(65536/360.0)) & 65535);
	return a;
}

/*
==================
BoxOnPlaneSide

Returns 1, 2, or 1 + 2
==================
*/
int BoxOnPlaneSide (const vec3_t emins, const vec3_t emaxs, const cplane_t *p)
{
	float	dist1, dist2;
	int		sides;

// fast axial cases
	if (p->type < 3)
	{
		if (p->dist <= emins[p->type])
			return 1;
		if (p->dist >= emaxs[p->type])
			return 2;
		return 3;
	}

// general case
	switch (p->signbits)
	{
	case 0:
dist1 = p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2];
dist2 = p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2];
		break;
	case 1:
dist1 = p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2];
dist2 = p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2];
		break;
	case 2:
dist1 = p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2];
dist2 = p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2];
		break;
	case 3:
dist1 = p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2];
dist2 = p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2];
		break;
	case 4:
dist1 = p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2];
dist2 = p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2];
		break;
	case 5:
dist1 = p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emins[2];
dist2 = p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emaxs[2];
		break;
	case 6:
dist1 = p->normal[0]*emaxs[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2];
dist2 = p->normal[0]*emins[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2];
		break;
	case 7:
dist1 = p->normal[0]*emins[0] + p->normal[1]*emins[1] + p->normal[2]*emins[2];
dist2 = p->normal[0]*emaxs[0] + p->normal[1]*emaxs[1] + p->normal[2]*emaxs[2];
		break;
	default:
		dist1 = dist2 = 0;		// shut up compiler
		assert( 0 );
		break;
	}

	sides = 0;
	if (dist1 >= p->dist)
		sides = 1;
	if (dist2 < p->dist)
		sides |= 2;

#if 0
	assert( sides != 0 );
#endif

	return sides;
}

/*
=================
SignbitsForPlane
=================
*/
int SignbitsForPlane ( const cplane_t *out )
{
	int	bits, j;

	// for fast box on planeside test

	bits = 0;
	for (j=0 ; j<3 ; j++)
	{
		if (out->normal[j] < 0)
			bits |= 1<<j;
	}
	return bits;
}

/*
=================
PlaneTypeForNormal
=================
*/
int	PlaneTypeForNormal ( const vec3_t normal )
{
// NOTE: should these have an epsilon around 1.0?		
	if ( normal[0] >= 1.0)
		return PLANE_X;
	if ( normal[1] >= 1.0 )
		return PLANE_Y;
	if ( normal[2] >= 1.0 )
		return PLANE_Z;
		
	return PLANE_NONAXIAL;
}

/*
=================
CategorizePlane

A slightly simplier version of SignbitsForPlane and PlaneTypeForNormal
=================
*/
void CategorizePlane ( cplane_t *plane )
{
	int i;

	plane->signbits = 0;
	plane->type = PLANE_NONAXIAL;
	for (i = 0; i < 3; i++)
	{
		if (plane->normal[i] < 0)
			plane->signbits |= 1<<i;
		if (plane->normal[i] == 1.0f)
			plane->type = i;
	}
}

/*
=================
PlaneFromPoints
=================
*/
void PlaneFromPoints (  vec3_t verts[3], cplane_t *plane )
{
	vec3_t	v1, v2;

	VectorSubtract( verts[1], verts[0], v1 );
	VectorSubtract( verts[2], verts[0], v2 );
	CrossProduct( v2, v1, plane->normal );
	VectorNormalize( plane->normal );
	plane->dist = DotProduct( verts[0], plane->normal );
}

#define	PLANE_NORMAL_EPSILON	0.00001
#define	PLANE_DIST_EPSILON		0.01

/*
=================
ComparePlanes
=================
*/
qboolean ComparePlanes ( const vec3_t p1normal, vec_t p1dist, const vec3_t p2normal, vec_t p2dist )
{
	if (fabs (p1normal[0] - p2normal[0]) < PLANE_NORMAL_EPSILON
		&& fabs (p1normal[1] - p2normal[1]) < PLANE_NORMAL_EPSILON
		&& fabs (p1normal[2] - p2normal[2]) < PLANE_NORMAL_EPSILON
		&& fabs (p1dist - p2dist) < PLANE_DIST_EPSILON )
		return qtrue;

	return qfalse;
}

/*
==============
SnapVector
==============
*/
void SnapVector ( vec3_t normal )
{
	int		i;

	for ( i = 0; i < 3; i++ ) {
		if ( fabs (normal[i] - 1) < PLANE_NORMAL_EPSILON ) {
			VectorClear ( normal );
			normal[i] = 1;
			break;
		}
		if ( fabs (normal[i] - -1) < PLANE_NORMAL_EPSILON ) {
			VectorClear ( normal );
			normal[i] = -1;
			break;
		}
	}
}

/*
==============
SnapPlane
==============
*/
void SnapPlane ( vec3_t normal, vec_t *dist )
{
	SnapVector ( normal );

	if ( fabs (*dist - Q_rint (*dist)) < PLANE_DIST_EPSILON ) {
		*dist = Q_rint ( *dist );
	}
}

void ClearBounds (vec3_t mins, vec3_t maxs)
{
	mins[0] = mins[1] = mins[2] = 99999;
	maxs[0] = maxs[1] = maxs[2] = -99999;
}

qboolean BoundsIntersect (const vec3_t mins1, const vec3_t maxs1, const vec3_t mins2, const vec3_t maxs2)
{
	return (mins1[0] <= maxs2[0] && mins1[1] <= maxs2[1] && mins1[2] <= maxs2[2] &&
		 maxs1[0] >= mins2[0] && maxs1[1] >= mins2[1] && maxs1[2] >= mins2[2]);
}

/************************************************************************/
/* From GraphicsGem                                                     */
/* http://www.acm.org/tog/GraphicsGems/gems/BoxSphere.c                 */
/* Warning use a solid-solid algorithm otherwise the rj doesnt work     */
/************************************************************************/
#define SQR(a)          ((a)*(a))       
#define MIN(a,b)        (((a)<(b))?(a):(b))     
#define MAX(a,b)        (((a)>(b))?(a):(b)) 
qboolean BoundsAndSphereIntersectSplash(const vec3_t Bmin, const vec3_t Bmax, const vec3_t C, float r)
{
	float  dmin;
	float  r2 = SQR(r);
	int    i;

	/* Solid box - Solid Sphere */
	dmin = 0;
	for( i = 0; i < 3; i++ ) 
	{
		if( C[i] < Bmin[i] ) 
			dmin += SQR(C[i] - Bmin[i] ); 
		else
			if( C[i] > Bmax[i] ) 
				dmin += SQR( C[i] - Bmax[i] );     
	}

	if( dmin <= r2 ) 
		return qtrue;

	return qfalse;
}

qboolean BoundsAndSphereIntersect(const vec3_t mins, const vec3_t maxs, const vec3_t centre, float radius)
{
	return (mins[0] <= centre[0]+radius && mins[1] <= centre[1]+radius && mins[2] <= centre[2]+radius &&
	maxs[0] >= centre[0]-radius && maxs[1] >= centre[1]-radius && maxs[2] >= centre[2]-radius);
}

void AddPointToBounds (const vec3_t v, vec3_t mins, vec3_t maxs)
{
	int		i;
	vec_t	val;

	for (i=0 ; i<3 ; i++)
	{
		val = v[i];
		if (val < mins[i])
			mins[i] = val;
		if (val > maxs[i])
			maxs[i] = val;
	}
}

/*
=================
RadiusFromBounds
=================
*/
float RadiusFromBounds( const vec3_t mins, const vec3_t maxs )
{
	int		i;
	vec3_t	corner;

	for (i=0 ; i<3 ; i++)
	{
		corner[i] = fabs(mins[i]) > fabs(maxs[i]) ? fabs(mins[i]) : fabs(maxs[i]);
	}

	return VectorLength (corner);
}

vec_t VectorNormalize (vec3_t v)
{
	float	length, ilength;

	length = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];

	if (length)
	{
		length = sqrt (length);		// FIXME
		ilength = 1/length;
		v[0] *= ilength;
		v[1] *= ilength;
		v[2] *= ilength;
	}
		
	return length;
}

vec_t VectorNormalize2 (vec3_t v, vec3_t out)
{
	float	length, ilength;

	length = v[0]*v[0] + v[1]*v[1] + v[2]*v[2];

	if (length)
	{
		length = sqrt (length);		// FIXME
		ilength = 1/length;
		out[0] = v[0]*ilength;
		out[1] = v[1]*ilength;
		out[2] = v[2]*ilength;
	}
	else
	{
		VectorClear (out);
	}
		
	return length;
}

// fast vector normalize routine that does not check to make sure
// that length != 0, nor does it return length, uses rsqrt approximation
void VectorNormalizeFast( vec3_t v )
{
	float ilength = Q_RSqrt( DotProduct(v,v) );

	v[0] *= ilength;
	v[1] *= ilength;
	v[2] *= ilength;
}

void _VectorMA (vec3_t veca, float scale, vec3_t vecb, vec3_t vecc)
{
	vecc[0] = veca[0] + scale*vecb[0];
	vecc[1] = veca[1] + scale*vecb[1];
	vecc[2] = veca[2] + scale*vecb[2];
}


vec_t _DotProduct (vec3_t v1, vec3_t v2)
{
	return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];
}

void _VectorSubtract (vec3_t veca, vec3_t vecb, vec3_t out)
{
	out[0] = veca[0]-vecb[0];
	out[1] = veca[1]-vecb[1];
	out[2] = veca[2]-vecb[2];
}

void _VectorAdd (vec3_t veca, vec3_t vecb, vec3_t out)
{
	out[0] = veca[0]+vecb[0];
	out[1] = veca[1]+vecb[1];
	out[2] = veca[2]+vecb[2];
}

void _VectorCopy (vec3_t in, vec3_t out)
{
	out[0] = in[0];
	out[1] = in[1];
	out[2] = in[2];
}

int Q_log2(int val)
{
	int answer=0;
	while (val>>=1)
		answer++;
	return answer;
}

//============================================================================

void Matrix_Identity( vec3_t m[3] )
{
	int i, j;

	for( i = 0; i < 3; i++ )
		for( j = 0; j < 3; j++ )
			if( i == j )
				m[i][j] = 1.0;
			else
				m[i][j] = 0.0;
}

void Matrix_Copy( vec3_t m1[3], vec3_t m2[3] )
{
	int i, j;

	for( i = 0; i < 3; i++ )
		for( j = 0; j < 3; j++ )
			m2[i][j] = m1[i][j];
}

qboolean Matrix_Compare( vec3_t m1[3], vec3_t m2[3] )
{
	int i, j;

	for( i = 0; i < 3; i++ )
		for( j = 0; j < 3; j++ )
			if( m1[i][j] != m2[i][j] )
				return qfalse;
	return qtrue;
}

void Matrix_Multiply( vec3_t m1[3], vec3_t m2[3], vec3_t out[3] )
{
	out[0][0] = m1[0][0]*m2[0][0] + m1[0][1]*m2[1][0] + m1[0][2]*m2[2][0];
	out[0][1] = m1[0][0]*m2[0][1] + m1[0][1]*m2[1][1] + m1[0][2]*m2[2][1];
	out[0][2] = m1[0][0]*m2[0][2] + m1[0][1]*m2[1][2] + m1[0][2]*m2[2][2];
	out[1][0] = m1[1][0]*m2[0][0] + m1[1][1]*m2[1][0] +	m1[1][2]*m2[2][0];
	out[1][1] = m1[1][0]*m2[0][1] + m1[1][1]*m2[1][1] + m1[1][2]*m2[2][1];
	out[1][2] = m1[1][0]*m2[0][2] + m1[1][1]*m2[1][2] +	m1[1][2]*m2[2][2];
	out[2][0] = m1[2][0]*m2[0][0] + m1[2][1]*m2[1][0] +	m1[2][2]*m2[2][0];
	out[2][1] = m1[2][0]*m2[0][1] + m1[2][1]*m2[1][1] +	m1[2][2]*m2[2][1];
	out[2][2] = m1[2][0]*m2[0][2] + m1[2][1]*m2[1][2] +	m1[2][2]*m2[2][2];
}

void Matrix_TransformVector( vec3_t m[3], vec3_t v, vec3_t out )
{
	out[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];
	out[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];
	out[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];
}

void Matrix_Transpose( vec3_t in[3], vec3_t out[3] )
{
	out[0][0] = in[0][0];
	out[1][1] = in[1][1];
	out[2][2] = in[2][2];

	out[0][1] = in[1][0];
	out[0][2] = in[2][0];
	out[1][0] = in[0][1];
	out[1][2] = in[2][1];
	out[2][0] = in[0][2];
	out[2][1] = in[1][2];
}

void Matrix_EulerAngles( vec3_t m[3], vec3_t angles )
{
	vec_t	c;
	vec_t	pitch, yaw, roll;

	pitch = -asin( m[0][2] );
	c = cos ( pitch );
	pitch = RAD2DEG( pitch );

	if ( fabs( c ) > 0.005 )             // Gimball lock?
	{
		c = 1.0f / c;
		yaw = RAD2DEG( atan2 ( m[0][1] * c, m[0][0] * c ) );
		roll = RAD2DEG( atan2 ( -m[1][2] * c, m[2][2] * c ) );
	}
	else
	{
		if (m[0][2] > 0)
			pitch = -90;
		else
			pitch = 90;
		yaw = RAD2DEG( atan2 ( m[1][0], -m[1][1] ) );
		roll = 0;
	}

	angles[PITCH] = anglemod( pitch );
	angles[YAW] = anglemod( yaw );
	angles[ROLL] = anglemod( roll );
}

void Matrix_Rotate( vec3_t m[3], vec_t angle, vec_t x, vec_t y, vec_t z )
{
	vec3_t t[3], b[3];
	vec_t c = cos( DEG2RAD(angle) );
	vec_t s = sin( DEG2RAD(angle) );
	vec_t mc = 1 - c, t1, t2;
	
	t[0][0] = (x * x * mc) + c;
	t[1][1] = (y * y * mc) + c;
	t[2][2] = (z * z * mc) + c;

	t1 = y * x * mc;
	t2 = z * s;
	t[0][1] = t1 + t2;
	t[1][0] = t1 - t2;

	t1 = x * z * mc;
	t2 = y * s;
	t[0][2] = t1 - t2;
	t[2][0] = t1 + t2;

	t1 = y * z * mc;
	t2 = x * s;
	t[1][2] = t1 + t2;
	t[2][1] = t1 - t2;

	Matrix_Copy( m, b );
	Matrix_Multiply( b, t, m );
}

void Matrix_FromPoints( const vec3_t v1, const vec3_t v2, const vec3_t v3, vec3_t m[3] )
{
	float		d;

	m[2][0] = (v1[1] - v2[1]) * (v3[2] - v2[2]) - (v1[2] - v2[2]) * (v3[1] - v2[1]);
	m[2][1] = (v1[2] - v2[2]) * (v3[0] - v2[0]) - (v1[0] - v2[0]) * (v3[2] - v2[2]);
	m[2][2] = (v1[0] - v2[0]) * (v3[1] - v2[1]) - (v1[1] - v2[1]) * (v3[0] - v2[0]);
	VectorNormalizeFast( m[2] );

	// this rotate and negate guarantees a vector not colinear with the original
	VectorSet( m[1], m[2][2], -m[2][0], m[2][1] );
	d = -DotProduct( m[1], m[2] );
	VectorMA( m[1], d, m[2], m[1] );
	VectorNormalizeFast( m[1] );
	CrossProduct( m[1], m[2], m[0] );
}

//============================================================================

void Quat_Identity( quat_t q )
{
	q[0] = 0;
	q[1] = 0;
	q[2] = 0;
	q[3] = 1;
}

void Quat_Copy( const quat_t q1, quat_t q2 )
{
	q2[0] = q1[0];
	q2[1] = q1[1];
	q2[2] = q1[2];
	q2[3] = q1[3];
}

qboolean Quat_Compare( const quat_t q1, const quat_t q2 ) {
	if( q1[0] != q2[0] || q1[1] != q2[1] || q1[2] != q2[2] || q1[3] != q2[3] )
		return qfalse;
	return qtrue;
}

void Quat_Conjugate( const quat_t q1, quat_t q2 )
{
	q2[0] = -q1[0];
	q2[1] = -q1[1];
	q2[2] = -q1[2];
	q2[3] = q1[3];
}

vec_t Quat_Normalize( quat_t q )
{
	vec_t length;

	length = q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3];
	if( length != 0 ) {
		vec_t ilength = 1.0 / sqrt( length );
		q[0] *= ilength;
		q[1] *= ilength;
		q[2] *= ilength;
		q[3] *= ilength;
	}

	return length;
}

vec_t Quat_Inverse( const quat_t q1, quat_t q2 )
{
	Quat_Conjugate( q1, q2 );

	return Quat_Normalize( q2 );
}

void Matrix_Quat( vec3_t m[3], quat_t q )
{
	vec_t tr, s;

	tr = m[0][0] + m[1][1] + m[2][2];
	if( tr > 0.00001 ) {
		s = sqrt( tr + 1.0 );
		q[3] = s * 0.5; s = 0.5 / s;
		q[0] = (m[2][1] - m[1][2]) * s;
		q[1] = (m[0][2] - m[2][0]) * s;
		q[2] = (m[1][0] - m[0][1]) * s;
	} else {
		int i, j, k;

		i = 0;
		if (m[1][1] > m[0][0]) i = 1;
		if (m[2][2] > m[i][i]) i = 2;
		j = (i + 1) % 3;
		k = (i + 2) % 3;

		s = sqrt( m[i][i] - (m[j][j] + m[k][k]) + 1.0 );

		q[i] = s * 0.5; if( s != 0.0 ) s = 0.5 / s;
		q[j] = (m[j][i] + m[i][j]) * s;
		q[k] = (m[k][i] + m[i][k]) * s;
		q[3] = (m[k][j] - m[j][k]) * s;
	}

	Quat_Normalize( q );
}

void Quat_Multiply( const quat_t q1, const quat_t q2, quat_t out )
{
	out[0] = q1[3] * q2[0] + q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1];
	out[1] = q1[3] * q2[1] + q1[1] * q2[3] + q1[2] * q2[0] - q1[0] * q2[2];
	out[2] = q1[3] * q2[2] + q1[2] * q2[3] + q1[0] * q2[1] - q1[1] * q2[0];
	out[3] = q1[3] * q2[3] - q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2];
}

void Quat_Lerp( const quat_t q1, const quat_t q2, vec_t t, quat_t out )
{
	quat_t p1;
	vec_t omega, cosom, sinom, scale0, scale1;

	if( Quat_Compare( q1, q2 ) ) {
		Quat_Copy( q1, out );
		return;
	}

	cosom = q1[0] * q2[0] + q1[1] * q2[1] + q1[2] * q2[2] + q1[3] * q2[3];
	if( cosom < 0.0 ) { 
		cosom = -cosom;
		p1[0] = -q1[0]; p1[1] = -q1[1];
		p1[2] = -q1[2]; p1[3] = -q1[3];
	} else {
		p1[0] = q1[0]; p1[1] = q1[1];
		p1[2] = q1[2]; p1[3] = q1[3];
	}

	if( cosom < 1.0 - 0.001 ) {
		omega = acos( cosom );
		sinom = 1.0 / sin( omega );
		scale0 = sin( (1.0 - t) * omega ) * sinom;
		scale1 = sin( t * omega ) * sinom;
	} else { 
		scale0 = 1.0 - t;
		scale1 = t;
	}

	out[0] = scale0 * p1[0] + scale1 * q2[0];
	out[1] = scale0 * p1[1] + scale1 * q2[1];
	out[2] = scale0 * p1[2] + scale1 * q2[2];
	out[3] = scale0 * p1[3] + scale1 * q2[3];
}

void Quat_Vectors( const quat_t q, vec3_t f, vec3_t r, vec3_t u )
{
	vec_t wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;

	x2 = q[0] + q[0]; y2 = q[1] + q[1]; z2 = q[2] + q[2];
	xx = q[0] * x2; xy = q[0] * y2; xz = q[0] * z2;
	yy = q[1] * y2; yz = q[1] * z2; zz = q[2] * z2;
	wx = q[3] * x2; wy = q[3] * y2; wz = q[3] * z2;

	if( f ) {
		f[0] = 1.0f - yy - zz; f[1] = xy - wz; f[2] = xz + wy;
	}
	if( r ) {
		r[0] = xy + wz; r[1] = 1.0f - xx - zz; r[2] = yz - wx;
	}
	if( u ) {
		u[0] = xz - wy; u[1] = yz + wx; u[2] = 1.0f - xx - yy;
	}
}

void Quat_Matrix( const quat_t q, vec3_t m[3] ) {
	Quat_Vectors( q, m[0], m[1], m[2] );
}

void Quat_TransformVector( const quat_t q, const vec3_t v, vec3_t out )
{
	vec_t wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;

	x2 = q[0] + q[0]; y2 = q[1] + q[1]; z2 = q[2] + q[2];
	xx = q[0] * x2; xy = q[0] * y2; xz = q[0] * z2;
	yy = q[1] * y2; yz = q[1] * z2; zz = q[2] * z2;
	wx = q[3] * x2; wy = q[3] * y2; wz = q[3] * z2;

	out[0] = (1.0f - yy - zz) * v[0] + (xy - wz) * v[1] + (xz + wy) * v[2];
	out[1] = (xy + wz) * v[0] + (1.0f - xx - zz) * v[1] + (yz - wx) * v[2];
	out[2] = (xz - wy) * v[0] + (yz + wx) * v[1] + (1.0f - xx - yy) * v[2];
}

void Quat_ConcatTransforms( const quat_t q1, const vec3_t v1, const quat_t q2, const vec3_t v2, quat_t q, vec3_t v )
{
	Quat_Multiply( q1, q2, q );
	Quat_TransformVector( q1, v2, v );
	v[0] += v1[0]; v[1] += v1[1]; v[2] += v1[2];
}