/source/gameshared/q_math.c

/*

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_arch.h"

#include "q_math.h"

#include "q_shared.h"

#include "q_collision.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] );

}



// similar to MakeNormalVectors but for rotational matrices

// (FIXME: weird, what's the diff between this and MakeNormalVectors?)

void NormalVectorToAxis( const vec3_t forward, vec3_t axis[3] )

{

	VectorCopy( forward, axis[0] );

	if( forward[0] || forward[1] )

	{

		VectorSet( axis[1], forward[1], -forward[0], 0 );

		VectorNormalize( axis[1] );

		CrossProduct( axis[0], axis[1], axis[2] );

	}

	else

	{

		VectorSet( axis[1], 1, 0, 0 );

		VectorSet( axis[2], 0, 1, 0 );

	}

}



void BuildBoxPoints( vec3_t p[8], const vec3_t org, const vec3_t mins, const 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 );

}



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

//AngleSubtract

//

//Always returns a value from -180 to 180

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

float AngleSubtract( float a1, float a2 )

{

	float a;



	a = a1 - a2;

	while( a > 180 )

	{

		a -= 360;

	}

	while( a < -180 )

	{

		a += 360;

	}

	return a;

}



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

//AnglesSubtract

//

//Always returns a value from -180 to 180

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

void AnglesSubtract( vec3_t v1, vec3_t v2, vec3_t v3 )

{

	v3[0] = AngleSubtract( v1[0], v2[0] );

	v3[1] = AngleSubtract( v1[1], v2[1] );

	v3[2] = AngleSubtract( v1[2], v2[2] );

}



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

//AngleNormalize360

//

//returns angle normalized to the range [0 <= angle < 360]

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

float AngleNormalize360( float angle )

{

	return ( 360.0 / 65536 ) * ( (int)( angle * ( 65536 / 360.0 ) ) & 65535 );

}



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

//AngleNormalize180

//

//returns angle normalized to the range [-180 < angle <= 180]

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

float AngleNormalize180( float angle )

{

	angle = AngleNormalize360( angle );

	if( angle > 180.0 )

	{

		angle -= 360.0;

	}

	return angle;

}



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

//AngleDelta

//

//returns the normalized delta from angle1 to angle2

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

float AngleDelta( float angle1, float angle2 )

{

	return AngleNormalize180( angle1 - angle2 );

}



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

//anglemod

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

float anglemod( float a )

{

	a = ( 360.0/65536 ) * ( (int)( a*( 65536/360.0 ) ) & 65535 );

	return a;

}



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

//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;

}



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

//AdjustFov

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

void AdjustFov( float *fov_x, float *fov_y, float width, float height, qboolean lock_x )

{

	float x, y;



	if( width*3 == 4*height || width*4 == height*5 )

	{                                                   // 4:3 or 5:4 ratio

		return;

	}



	if( lock_x )

	{

		*fov_y = 2 *atan( ( width *3 ) / ( height *4 ) *tan ( *fov_y *M_PI / 360.0 *0.5 ) )*360/M_PI;

		return;

	}



	y = CalcFov( *fov_x, 640, 480 );

	x = *fov_x;



	*fov_x = CalcFov( y, height, width );

	if( *fov_x < x )

		*fov_x = x;

	else

		*fov_y = y;

}



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

//BoxOnPlaneSide

//

//Returns 1, 2, or 1 + 2

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

int BoxOnPlaneSide( const vec3_t emins, const vec3_t emaxs, const struct cplane_s *p )

{

	float dist1, dist2;

	int sides;



	// 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 more complex version of SignbitsForPlane and PlaneTypeForNormal,

//which also tries to fix possible floating point glitches (like -0.00000 cases)

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

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->signbits = ( 1<<i );

				VectorClear( plane->normal );

				plane->normal[i] = -1.0f;

				break;

			}

		}

		else if( plane->normal[i] == 1.0f )

		{

			plane->type = i;

			plane->signbits = 0;

			VectorClear( plane->normal );

			plane->normal[i] = 1.0f;

			break;

		}

	}

}



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

//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 (qboolean)( mins1[0] <= maxs2[0] && mins1[1] <= maxs2[1] && mins1[2] <= maxs2[2] &&

	                   maxs1[0] >= mins2[0] && maxs1[1] >= mins2[1] && maxs1[2] >= mins2[2] );

}



qboolean BoundsAndSphereIntersect( const vec3_t mins, const vec3_t maxs, const vec3_t centre, float radius )

{

	int i;

	float dmin = 0;

	float radius2 = radius * radius;



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

	{

		if( centre[i] < mins[i] )

			dmin += ( centre[i] - mins[i] ) * ( centre[i] - mins[i] );

		else if( centre[i] > maxs[i] )

			dmin += ( centre[i] - maxs[i] ) * ( centre[i] - maxs[i] );

	}



	if( dmin <= radius2 )

		return qtrue;

	return qfalse;

}



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.0/length;

		v[0] *= ilength;

		v[1] *= ilength;

		v[2] *= ilength;

	}



	return length;

}



vec_t VectorNormalize2( const 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.0/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( const vec3_t veca, float scale, const 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( const vec3_t v1, const vec3_t v2 )

{

	return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];

}



void _VectorSubtract( const vec3_t veca, const 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( const vec3_t veca, const 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( const 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 );

	if( fabs( c ) > 5*10e-6 )       // Gimball lock?

	{ // no

		c = 1.0f / c;

		pitch = RAD2DEG( pitch );

		yaw = RAD2DEG( atan2( m[0][1] * c, m[0][0] * c ) );

		roll = RAD2DEG( atan2( -m[1][2] * c, m[2][2] * c ) );

	}

	else

	{ // yes

		pitch = m[0][2] > 0 ? -90 : 90;

		yaw = RAD2DEG( atan2( m[1][0], -m[1][1] ) );

		roll = 180;

	}



	angles[PITCH] = pitch;

	angles[YAW] = yaw;

	angles[ROLL] = 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( vec3_t v1, vec3_t v2, 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, sinsqr;



	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.0001 )

	{

		sinsqr = 1.0 - cosom * cosom;

		sinom = Q_RSqrt( sinsqr );

		omega = atan2( sinsqr * sinom, cosom );

		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; yy = q[1] * y2; zz = q[2] * z2;

	f[0] = 1.0f - yy - zz; r[1] = 1.0f - xx - zz; u[2] = 1.0f - xx - yy;



	yz = q[1] * z2; wx = q[3] * x2;

	r[2] = yz - wx; u[1] = yz + wx;



	xy = q[0] * y2; wz = q[3] * z2;

	f[1] = xy - wz; r[0] = xy + wz;



	xz = q[0] * z2; wy = q[3] * y2;

	f[2] = xz + wy; u[0] = xz - wy;

}



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];

}