Descent3/scripts/lnx/osiris_vector.h

#ifndef OSIRIS_VECTOR_H
#define OSIRIS_VECTOR_H

#include <math.h>
#include <time.h>

#include "../../Main/lib/vecmat_external.h"

const vector Zero_vector = {0.0f, 0.0f, 0.0f};

// Disable the "possible loss of data" warning
#pragma warning (disable:4244)

//Angles are unsigned shorts
typedef unsigned short angle;

//The basic fixed-point type
typedef long fix;

#define PI 3.141592654

//Constants for converted between fix and float
#define FLOAT_SCALER		65536.0
#define FIX_SHIFT			16

//1.0 in fixed-point
#define F1_0				(1 << FIX_SHIFT)

//Generate the data for the trig tables.  Must be called before trig functions
void InitMathTables ();

//Returns the sine of the given angle.  Linearly interpolates between two entries in a 256-entry table
float FixSin(angle a);

//Returns the cosine of the given angle.  Linearly interpolates between two entries in a 256-entry table
float FixCos(angle a);

//Returns the sine of the given angle, but does no interpolation
float FixSinFast(angle a);

//Returns the cosine of the given angle, but does no interpolation
float FixCosFast(angle a);

#define Round(x)   ((int) (x + 0.5))

fix FloatToFixFast(float num);

//Conversion macros
//??#define FloatToFix(num) Round((num) * FLOAT_SCALER)
#define FloatToFix(num) ((fix)((num)*FLOAT_SCALER))
#define IntToFix(num) ((num) << FIX_SHIFT)
#define ShortToFix(num) (((long) (num)) << FIX_SHIFT)
#define FixToFloat(num) (((float) (num)) / FLOAT_SCALER)
#define FixToInt(num) ((num) >> FIX_SHIFT)
#define FixToShort(num) ((short) ((num) >> FIX_SHIFT))

/* --NOT NEEDED -JEFF
//Fixed-point math functions in inline ASM form
#if defined(MACINTOSH)
	#include "fixmac.h"
#elif defined(WIN32)
	#include "..\..\main\win\fixwin32.h"
#endif
*/

//Tables for trig functions
float sincos_table[321];				//256 entries + 64 sin-only + 1 for interpolation
angle asin_table[257];					// 1 quadrants worth, +1 for interpolation
angle acos_table[257];					

//Generate the data for the trig tables
void InitMathTables ()
{
	int i;
	float rad,s,c;

	for (i=0;i<321;i++) {
		rad = (float) ((double) i / 256.0 * 2 * PI);
		sincos_table[i] = (float) sin(rad);
	}

	for (i=0;i<256;i++) {
				
		s=asin((float)i/256.0);
		c=acos((float)i/256.0);

		s=(s/(PI*2));
		c=(c/(PI*2));
		
		asin_table[i] = FloatToFix(s);
		acos_table[i] = FloatToFix(c);
	}
	
	asin_table[256]=asin_table[255];
	acos_table[256]=acos_table[255];

//	Initialize a random seed.
	srand(time(NULL));
}

//Returns the sine of the given angle.  Linearly interpolates between two entries in a 256-entry table
float FixSin(angle a)
{
   int i,f;
   float s0,s1;

   i = (a>>8)&0xff;
   f = a&0xff;

   s0 = sincos_table[i];
   s1 = sincos_table[i+1];
   return (float) (s0 + ((s1 - s0) * (double) f / 256.0));
}

//Returns the cosine of the given angle.  Linearly interpolates between two entries in a 256-entry table
float FixCos(angle a)
{
   int i,f;
   float c0,c1;

   i = (a>>8)&0xff;
   f = a&0xff;

   c0 = sincos_table[i+64];
   c1 = sincos_table[i+64+1];
   return (float) (c0 + ((c1 - c0) * (double) f / 256.0));
}

//Returns the sine of the given angle, but does no interpolation
float FixSinFast(angle a)
{
   int i;

   i = ((a+0x80)>>8)&0xff;

   return sincos_table[i];
}

//Returns the cosine of the given angle, but does no interpolation
float FixCosFast(angle a)
{
   int i;

   i = ((a+0x80)>>8)&0xff;

   return sincos_table[i+64];
}

// use this instead of:
// for:  (int)floor(x+0.5f) use FloatRound(x)
//       (int)ceil(x-0.5f)  use FloatRound(x)
//       (int)floor(x-0.5f) use FloatRound(x-1.0f)
//       (int)floor(x)      use FloatRound(x-0.5f)
// for values in the range -2048 to 2048

// Set a vector to {0,0,0}
int FloatRound( float x )
{
	float nf;
	nf = x + 8390656.0f;
	return ((*((int *)&nf)) & 0x7FFFFF)-2048;
}

// A fast way to convert floats to fix
fix FloatToFixFast( float x )
{

	float nf;
	nf = x*65536.0f + 8390656.0f;
	return ((*((int *)&nf)) & 0x7FFFFF)-2048;
}

//Get rid of the "no return value" warnings in the next three functions
#pragma warning (disable:4035)

//compute inverse sine
angle FixAsin(float v)
{
	fix vv;
	int i,f,aa;

	vv = FloatToFix(fabs(v));

	if (vv >= F1_0)		//check for out of range
		return 0x4000;

	i = (vv>>8)&0xff;
	f = vv&0xff;

	aa = asin_table[i];
	aa = aa + (((asin_table[i+1] - aa) * f)>>8);

	if (v < 0)
		aa = F1_0-aa;

	return aa;
}

//compute inverse cosine
angle FixAcos(float v)
{
	fix vv;
	int i,f,aa;

	vv = FloatToFix(fabs(v));

	if (vv >= F1_0)		//check for out of range
		return 0;

	i = (vv>>8)&0xff;
	f = vv&0xff;

	aa = acos_table[i];
	aa = aa + (((acos_table[i+1] - aa) * f)>>8);

	if (v < 0)
		aa = 0x8000 - aa;

	return aa;
}

//given cos & sin of an angle, return that angle.
//parms need not be normalized, that is, the ratio of the parms cos/sin must
//equal the ratio of the actual cos & sin for the result angle, but the parms 
//need not be the actual cos & sin.  
//NOTE: this is different from the standard C atan2, since it is left-handed.
angle FixAtan2(float cos,float sin)
{
	float q,m;
	angle t;
	
	//find smaller of two


	q=(sin*sin)+(cos*cos);
	
	m = sqrt(q);

	if (m==0)
		return 0;

	if (fabs(sin) < fabs(cos)) 
	{
		//sin is smaller, use arcsin
		t = FixAsin(sin/m);
		if (cos<0)
			t = 0x8000 - t;
	
		return t;
	}
	else 
	{
		t = FixAcos(cos/m);
		if (sin<0)
			t = F1_0-t;
	
		return t;
	}

}


// Does a ceiling operation on a fixed number
fix FixCeil (fix num)
{
	int int_num;
	fix new_num;

	int_num=FixToInt (num);

	if (num & 0xFFFF)
	{
		new_num=IntToFix(int_num+1);
		return new_num;
	}

	new_num=IntToFix(int_num);
	return (new_num);
}

// Floors a fixed number 
fix FixFloor (fix num)
{
	int int_num=FixToInt (num);

	return (IntToFix (int_num));
}

// use this instead of:
// for:  (int)floor(x+0.5f) use FloatRound(x)
//       (int)ceil(x-0.5f)  use FloatRound(x)
//       (int)floor(x-0.5f) use FloatRound(x-1.0f)
//       (int)floor(x)      use FloatRound(x-0.5f)
// for values in the range -2048 to 2048
int FloatRound( float x );

angle FixAtan2(float cos,float sin);
angle FixAsin(float v);
angle FixAcos(float v);

// Does a ceiling operation on a fixed number
fix FixCeil (fix num);

// Floors a fixed number 
fix FixFloor (fix num);

// Used for debugging.  It is used in printf's so we do not have to write out the structure 3 times 
// to print all the coordinates.
#define XYZ(v) (v)->x,(v)->y,(v)->z

extern const matrix Identity_matrix;

// Given a matrix, makes it an identity matrix
extern void vm_MakeIdentity (matrix *);

// Set a vector to {0,0,0}
extern void vm_MakeZero(vector *v);

// Set an angvec to {0,0,0}
extern void vm_MakeZero(angvec *a);

// Rotates a vector thru a matrix
extern void vm_MatrixMulVector (vector *,vector *,matrix *);

//Multiply a vector times the transpose of a matrix
void vm_VectorMulTMatrix(vector *result,vector *v,matrix *m);

// Multiplies 2 3x3 matrixes, returning the result in first argument
extern void vm_MatrixMul (matrix *,matrix *,matrix *);
 
//Multiply a matrix times the transpose of a matrix
void vm_MatrixMulTMatrix(matrix *dest,matrix *src0,matrix *src1);

// Computes all math look up tables, must be called before any vector stuff is used
extern void vm_InitMathTables();

// Given a vector, returns the magnitude.  Uses sqrt so it's slow
extern float vm_GetMagnitude (vector *);

// Given a vector, returns an approximation of the magnitude
extern float vm_GetMagnitudeFast (vector *);

// Returns the dot product of the two given vectors
extern float vm_DotProduct (vector *,vector *);

// Returns a perpendicular vector to the two given vectors
extern void vm_CrossProduct (vector *,vector *,vector *);

// Returns the difference between two vectors
extern void vm_SubVectors (vector *,const vector *,const vector *);

// Returns adds two vectors, returns result in first arg
extern void vm_AddVectors (vector *,vector *,vector *);

// Inits vector to 0,0,0
extern void vm_CenterVector (vector *);

// Given a vector, divides second arg by vector components
extern void vm_AverageVector (vector *,int);

// Normalizes a vector
// Returns the magnitude before normalization
extern float vm_VectorNormalize (vector *);

// Scales second arg vector by 3rd arg, placing result in first arg
extern void vm_ScaleVector (vector *,vector *,float);

// Scales all components of vector v by value s adds the result to p and stores result in vector d
extern void vm_ScaleAddVector (vector *d,vector *p,vector *v,float s);

// Divides second vector components by 3rd arg, placing result in first arg.  Useful for parametric lines
extern void vm_DivVector (vector *,vector *,float);

// Same as VectorNormalize, but uses approximation 
extern float vm_VectorNormalizeFast (vector *);

// Clears a matrix to zero
extern void vm_ClearMatrix (matrix *);

// Transposes a matrix in place
extern void vm_TransposeMatrix (matrix *);

// Given 3 angles (p,h,b), makes a rotation matrix out of them
extern void vm_AnglesToMatrix (matrix *,angle p,angle h,angle b);

//Ensure that a matrix is orthogonal
void vm_Orthogonalize(matrix *m);

//Compute a matrix from one or two vectors.  At least one and at most two vectors must/can be specified.
//Parameters:	m - filled in with the orienation matrix
//					fvec,uvec,rvec - pointers to vectors that determine the matrix.
//						One or two of these must be specified, with the other(s) set to NULL.
void vm_VectorToMatrix(matrix *m,vector *fvec,vector *uvec=NULL,vector *rvec=NULL);

//Computes a matrix from a vector and and angle of rotation around that vector
//Parameters:	m - filled in with the computed matrix
//					v - the forward vector of the new matrix
//					a - the angle of rotation around the forward vector
void vm_VectorAngleToMatrix(matrix *m,vector *v,angle a);

// Given an angle, places sin in 2nd arg, cos in 3rd.  Either can be null
extern void vm_SinCos (angle,float *,float *);

// Given x1,y1,x2,y2, returns the slope 
extern float vm_GetSlope (float,float,float,float);

//Calculates the perpendicular vector given three points
//Parms:	n - the computed perp vector (filled in)
//			v0,v1,v2 - three clockwise vertices
void vm_GetPerp(vector *n,vector *a,vector *b,vector *c);

//Calculates the (normalized) surface normal give three points
//Parms:	n - the computed surface normal (filled in)
//			v0,v1,v2 - three clockwise vertices
//Returns the magnitude of the normal before it was normalized.
//The bigger this value, the better the normal.
float vm_GetNormal(vector *n,vector *v0,vector *v1,vector *v2);

#define vm_GetSurfaceNormal vm_GetNormal

// Gets the distances (magnitude) between two vectors. Slow.
extern float vm_VectorDistance (const vector *a, const vector *b);

// Gets the approx distances (magnitude) between two vectors. Faster.
extern float vm_VectorDistanceQuick (vector *a,vector *b);

//Computes a normalized direction vector between two points
//Parameters:	dest - filled in with the normalized direction vector
//					start,end - the start and end points used to calculate the vector
//Returns:		the distance between the two input points
float vm_GetNormalizedDir (vector *dest,vector *end,vector *start);

// Returns a normalized direction vector between two points
// Uses sloppier magnitude, less precise
float vm_GetNormalizedDirFast (vector *dest,vector *end,vector *start);

//extract angles from a matrix 
angvec *vm_ExtractAnglesFromMatrix(angvec *a,matrix *m);

//	returns the angle between two vectors and a forward vector
angle vm_DeltaAngVec(vector *v0,vector *v1,vector *fvec);

//	returns the angle between two normalized vectors and a forward vector
angle vm_DeltaAngVecNorm(vector *v0,vector *v1,vector *fvec);

// Computes the distance from a point to a plane.
// Parms:	checkp - the point to check
// Parms:	norm - the (normalized) surface normal of the plane
//				planep - a point on the plane
// Returns:	The signed distance from the plane; negative dist is on the back of the plane
float vm_DistToPlane(vector *checkp,vector *norm,vector *planep);

//returns the value of a determinant
float calc_det_value(matrix *det);

void vm_MakeInverseMatrix (matrix *dest);
void vm_SinCosToMatrix(matrix *m,float sinp,float cosp,float sinb,float cosb,float sinh,float cosh);

// Gets the real center of a polygon
float vm_GetCentroid (vector *centroid,vector *src,int nv);

//	retrieves a random vector in values -RAND_MAX/2 to RAND_MAX/2
void vm_MakeRandomVector(vector *vec);

// Given a set of points, computes the minimum bounding sphere of those points
float vm_ComputeBoundingSphere(vector *center,vector *vecs,int num_verts);

// Gets the real center of a polygon, but uses fast magnitude calculation
// Returns the size of the passed in stuff
float vm_GetCentroidFast (vector *centroid,vector *src,int nv);


// Here are the C++ operator overloads -- they do as expected
extern matrix operator *(matrix src0, matrix src1);
extern matrix operator *=(matrix &src0, matrix src1);


void vm_AverageVector (vector *a,int num)
{
	// Averages a vector.  ie divides each component of vector a by num
	//assert (num!=0);

	a->x=a->x/(float)num;
	a->y=a->y/(float)num;
	a->z=a->z/(float)num;
}

void vm_AddVectors (vector *result,vector *a,vector *b)
{
	// Adds two vectors.  Either source can equal dest

	result->x=a->x+b->x;
	result->y=a->y+b->y;
	result->z=a->z+b->z;
}


void vm_SubVectors (vector *result,const vector *a, const vector *b)
{
	// Subtracts second vector from first.  Either source can equal dest

	result->x=a->x-b->x;
	result->y=a->y-b->y;
	result->z=a->z-b->z;
}

float vm_VectorDistance (const vector *a,const vector *b)
{
	// Given two vectors, returns the distance between them

	vector dest;
	float dist;

	vm_SubVectors (&dest,a,b);
	dist=vm_GetMagnitude (&dest);
	return dist;
}
float vm_VectorDistanceQuick (vector *a,vector *b)
{
	// Given two vectors, returns the distance between them

	vector dest;
	float dist;

	vm_SubVectors (&dest,a,b);
	dist=vm_GetMagnitudeFast (&dest);
	return dist;
}

//Calculates the perpendicular vector given three points
//Parms:	n - the computed perp vector (filled in)
//			v0,v1,v2 - three clockwise vertices
void vm_GetPerp(vector *n,vector *a,vector *b,vector *c)
{
	// Given 3 vertices, return the surface normal in n
	// IMPORTANT: B must be the 'corner' vertex

	vector x,y;
	
	vm_SubVectors (&x,b,a);
	vm_SubVectors (&y,c,b);
	
	vm_CrossProduct (n,&x,&y);
}   

//Calculates the (normalized) surface normal give three points
//Parms:	n - the computed surface normal (filled in)
//			v0,v1,v2 - three clockwise vertices
//Returns the magnitude of the normal before it was normalized.
//The bigger this value, the better the normal.
float vm_GetNormal(vector *n,vector *v0,vector *v1,vector *v2)
{
	vm_GetPerp(n,v0,v1,v2);

	return vm_VectorNormalize(n);
}


// Does a simple dot product calculation
float vm_DotProduct (vector *u,vector *v)
 {
    return (u->x * v->x) + (u->y * v->y) + (u->z * v->z);
 }

// Scales all components of vector v by value s and stores result in vector d
// dest can equal source
void vm_ScaleVector (vector *d,vector *v,float s)
 {
	d->x=(v->x * s);
	d->y=(v->y * s);
	d->z=(v->z * s);
 }


void vm_ScaleAddVector (vector *d,vector *p,vector *v,float s)
{
	// Scales all components of vector v by value s
	// adds the result to p and stores result in vector d
	// dest can equal source

	d->x=p->x+(v->x * s);
	d->y=p->y+(v->y * s);
	d->z=p->z+(v->z * s);
}

void vm_DivVector (vector *dest,vector *src,float n)
{
	// Divides a vector into n portions
	// Dest can equal src

	//assert (n!=0);


	dest->x=src->x/n;
	dest->y=src->y/n;
	dest->z=src->z/n;
}

void vm_CrossProduct (vector *dest,vector *u,vector *v)
{
	// Computes a cross product between u and v, returns the result
	//	in Normal.  Dest cannot equal source.
	
	dest->x = (u->y * v->z ) - (u->z * v->y);
	dest->y = (u->z * v->x ) - (u->x * v->z);
	dest->z = (u->x * v->y ) - (u->y * v->x);
}

//Normalize a vector.
//Returns:  the magnitude before normalization
float vm_VectorNormalize (vector *a)
{
	float mag;

	mag=vm_GetMagnitude (a);
	
	if (mag>0)
		*a /= mag;
	else
	{
		*a = Zero_vector;
		a->x = 1.0;
		mag = 0.0f;
	}

	return mag;
}

float vm_GetMagnitude (vector *a)
{
	float f;

	f=(a->x * a->x) + (a->y * a->y) + (a->z * a->z); 

	return (sqrt(f)); 
}
 
void vm_ClearMatrix (matrix *dest)
 {
 	memset (dest,0,sizeof (matrix));
 }

void vm_MakeIdentity (matrix *dest)
{
	memset (dest,0,sizeof (matrix));
	dest->rvec.x=dest->uvec.y=dest->fvec.z=1.0;
}	
void vm_MakeInverseMatrix (matrix *dest)
{
	memset ((void *)dest,0,sizeof (matrix));
	dest->rvec.x=dest->uvec.y=dest->fvec.z=-1.0;
}

void vm_TransposeMatrix (matrix *m)
{
	// Transposes a matrix in place

   float t;

   t = m->uvec.x;  m->uvec.x = m->rvec.y;  m->rvec.y = t;
   t = m->fvec.x;  m->fvec.x = m->rvec.z;  m->rvec.z = t;
   t = m->fvec.y;  m->fvec.y = m->uvec.z;  m->uvec.z = t;

}

void vm_MatrixMulVector (vector *result,vector *v,matrix *m)
{
	// Rotates a vector thru a matrix

	//assert(result != v);

	result->x = *v * m->rvec;
	result->y = *v * m->uvec;
	result->z = *v * m->fvec;
}

//Multiply a vector times the transpose of a matrix
void vm_VectorMulTMatrix(vector *result,vector *v,matrix *m)
{
	//assert(result != v);

	result->x = vm_Dot3Vector(m->rvec.x,m->uvec.x,m->fvec.x,v);
	result->y = vm_Dot3Vector(m->rvec.y,m->uvec.y,m->fvec.y,v);
	result->z = vm_Dot3Vector(m->rvec.z,m->uvec.z,m->fvec.z,v);
}

void vm_MatrixMul (matrix *dest,matrix *src0,matrix *src1)
{
	// For multiplying two 3x3 matrices together

	//assert((dest != src0) && (dest != src1));

	dest->rvec.x = vm_Dot3Vector(src0->rvec.x,src0->uvec.x,src0->fvec.x, &src1->rvec);
	dest->uvec.x = vm_Dot3Vector(src0->rvec.x,src0->uvec.x,src0->fvec.x, &src1->uvec);
	dest->fvec.x = vm_Dot3Vector(src0->rvec.x,src0->uvec.x,src0->fvec.x, &src1->fvec);

	dest->rvec.y = vm_Dot3Vector(src0->rvec.y,src0->uvec.y,src0->fvec.y, &src1->rvec);
	dest->uvec.y = vm_Dot3Vector(src0->rvec.y,src0->uvec.y,src0->fvec.y, &src1->uvec);
	dest->fvec.y = vm_Dot3Vector(src0->rvec.y,src0->uvec.y,src0->fvec.y, &src1->fvec);

	dest->rvec.z = vm_Dot3Vector(src0->rvec.z,src0->uvec.z,src0->fvec.z, &src1->rvec);
	dest->uvec.z = vm_Dot3Vector(src0->rvec.z,src0->uvec.z,src0->fvec.z, &src1->uvec);
	dest->fvec.z = vm_Dot3Vector(src0->rvec.z,src0->uvec.z,src0->fvec.z, &src1->fvec);
}

//Multiply a matrix times the transpose of a matrix
void vm_MatrixMulTMatrix(matrix *dest,matrix *src0,matrix *src1)
{
	// For multiplying two 3x3 matrices together

	//assert((dest != src0) && (dest != src1));

	dest->rvec.x = src0->rvec.x * src1->rvec.x + src0->uvec.x * src1->uvec.x + src0->fvec.x * src1->fvec.x;
	dest->uvec.x = src0->rvec.x * src1->rvec.y + src0->uvec.x * src1->uvec.y + src0->fvec.x * src1->fvec.y;
	dest->fvec.x = src0->rvec.x * src1->rvec.z + src0->uvec.x * src1->uvec.z + src0->fvec.x * src1->fvec.z;

	dest->rvec.y = src0->rvec.y * src1->rvec.x + src0->uvec.y * src1->uvec.x + src0->fvec.y * src1->fvec.x;
	dest->uvec.y = src0->rvec.y * src1->rvec.y + src0->uvec.y * src1->uvec.y + src0->fvec.y * src1->fvec.y;
	dest->fvec.y = src0->rvec.y * src1->rvec.z + src0->uvec.y * src1->uvec.z + src0->fvec.y * src1->fvec.z;

	dest->rvec.z = src0->rvec.z * src1->rvec.x + src0->uvec.z * src1->uvec.x + src0->fvec.z * src1->fvec.x;
	dest->uvec.z = src0->rvec.z * src1->rvec.y + src0->uvec.z * src1->uvec.y + src0->fvec.z * src1->fvec.y;
	dest->fvec.z = src0->rvec.z * src1->rvec.z + src0->uvec.z * src1->uvec.z + src0->fvec.z * src1->fvec.z;
}

matrix operator *(matrix src0, matrix src1) 
{
	// For multiplying two 3x3 matrices together
	matrix dest;
	
	dest.rvec.x = vm_Dot3Vector(src0.rvec.x,src0.uvec.x,src0.fvec.x, &src1.rvec);
	dest.uvec.x = vm_Dot3Vector(src0.rvec.x,src0.uvec.x,src0.fvec.x, &src1.uvec);
	dest.fvec.x = vm_Dot3Vector(src0.rvec.x,src0.uvec.x,src0.fvec.x, &src1.fvec);

	dest.rvec.y = vm_Dot3Vector(src0.rvec.y,src0.uvec.y,src0.fvec.y, &src1.rvec);
	dest.uvec.y = vm_Dot3Vector(src0.rvec.y,src0.uvec.y,src0.fvec.y, &src1.uvec);
	dest.fvec.y = vm_Dot3Vector(src0.rvec.y,src0.uvec.y,src0.fvec.y, &src1.fvec);

	dest.rvec.z = vm_Dot3Vector(src0.rvec.z,src0.uvec.z,src0.fvec.z, &src1.rvec);
	dest.uvec.z = vm_Dot3Vector(src0.rvec.z,src0.uvec.z,src0.fvec.z, &src1.uvec);
	dest.fvec.z = vm_Dot3Vector(src0.rvec.z,src0.uvec.z,src0.fvec.z, &src1.fvec);

	return dest;
}

matrix operator *=(matrix &src0, matrix src1) 
{
	return (src0 = src0 * src1);
}

//Computes a normalized direction vector between two points
//Parameters:	dest - filled in with the normalized direction vector
//					start,end - the start and end points used to calculate the vector
//Returns:		the distance between the two input points
float vm_GetNormalizedDir (vector *dest,vector *end,vector *start)
{
	vm_SubVectors (dest,end,start);
	return vm_VectorNormalize(dest);
}

//Returns a normalized direction vector between two points
//Just like vm_GetNormalizedDir(), but uses sloppier magnitude, less precise
//Parameters:	dest - filled in with the normalized direction vector
//					start,end - the start and end points used to calculate the vector
//Returns:		the distance between the two input points
float vm_GetNormalizedDirFast (vector *dest,vector *end,vector *start)
{
	vm_SubVectors (dest,end,start);
	return vm_VectorNormalizeFast(dest);
}

float vm_GetMagnitudeFast(vector *v)
{
   float a,b,c,bc;

    a = fabs(v->x);
    b = fabs(v->y);
	c = fabs(v->z);

	if (a < b) {
		float t=a; a=b; b=t;
	}

	if (b < c) {
		float t=b; b=c; c=t;

		if (a < b) {
			float t=a; a=b; b=t;
		}
	}

	bc = (b/4) + (c/8);

	return a + bc + (bc/2);
}

						
//Normalize a vector using an approximation of the magnitude
//Returns:  the magnitude before normalization
float vm_VectorNormalizeFast (vector *a)
{
	float mag;

	mag=vm_GetMagnitudeFast (a);
 
	if (mag==0.0)
	{
		a->x=a->y=a->z=0.0;
		return 0;
	}
  
	a->x=(a->x/mag);
	a->y=(a->y/mag);
	a->z=(a->z/mag);

	return mag;
}

// Computes the distance from a point to a plane.
// Parms:	checkp - the point to check
// Parms:	norm - the (normalized) surface normal of the plane
//				planep - a point on the plane
// Returns:	The signed distance from the plane; negative dist is on the back of the plane
float vm_DistToPlane(vector *checkp,vector *norm,vector *planep)
{
   vector t;

   t = *checkp - *planep;

   return t * *norm;

}

float vm_GetSlope(float x1,float y1,float x2,float y2)
{
	// returns the slope of a line
	float r;

	if (y2-y1==0)
		return (0.0);

	r=(x2-x1)/(y2-y1);
	return (r);

}

void vm_SinCosToMatrix(matrix *m,float sinp,float cosp,float sinb,float cosb,float sinh,float cosh)
{
	float sbsh,cbch,cbsh,sbch;

	sbsh = (sinb*sinh);
	cbch = (cosb*cosh);
	cbsh = (cosb*sinh);
	sbch = (sinb*cosh);

	m->rvec.x  = cbch + (sinp*sbsh);		//m1
	m->uvec.z  = sbsh + (sinp*cbch);		//m8

	m->uvec.x = (sinp*cbsh) - sbch;		//m2
	m->rvec.z = (sinp*sbch) - cbsh;		//m7

	m->fvec.x = (sinh*cosp);				//m3
	m->rvec.y = (sinb*cosp);				//m4
	m->uvec.y = (cosb*cosp);				//m5
	m->fvec.z = (cosh*cosp);				//m9

	m->fvec.y = -sinp;						//m6
}

void vm_AnglesToMatrix(matrix *m,angle p,angle h,angle b)
{
	float sinp,cosp,sinb,cosb,sinh,cosh;

   sinp = FixSin(p); cosp = FixCos(p);
   sinb = FixSin(b); cosb = FixCos(b);
   sinh = FixSin(h); cosh = FixCos(h);

	vm_SinCosToMatrix(m,sinp,cosp,sinb,cosb,sinh,cosh);
}

//Computes a matrix from a vector and and angle of rotation around that vector
//Parameters:	m - filled in with the computed matrix
//					v - the forward vector of the new matrix
//					a - the angle of rotation around the forward vector
void vm_VectorAngleToMatrix(matrix *m,vector *v,angle a)
{
	float sinb,cosb,sinp,cosp,sinh,cosh;

	sinb = FixSin(a); cosb = FixCos(a);

	sinp = -v->y;
	cosp = sqrt(1.0 - (sinp * sinp));

	if (cosp != 0.0) {
		sinh = v->x / cosp;
		cosh = v->z / cosp;
	}
	else {
		sinh = 0;
		cosh = 1.0;
	}

	vm_SinCosToMatrix(m,sinp,cosp,sinb,cosb,sinh,cosh);
}


//Ensure that a matrix is orthogonal
void vm_Orthogonalize(matrix *m)
{
	//Normalize forward vector
	if (vm_VectorNormalize(&m->fvec) == 0) {
		return;
	}

	//Generate right vector from forward and up vectors
	m->rvec = m->uvec ^ m->fvec;
	
	//Normaize new right vector
	if (vm_VectorNormalize(&m->rvec) == 0) {
		vm_VectorToMatrix(m,&m->fvec,NULL,NULL);		//error, so generate from forward vector only
		return;
	}

	//Recompute up vector, in case it wasn't entirely perpendiclar
	m->uvec = m->fvec ^ m->rvec;
}

//do the math for vm_VectorToMatrix()
void DoVectorToMatrix(matrix *m,vector *fvec,vector *uvec,vector *rvec)
{
	vector *xvec=&m->rvec,*yvec=&m->uvec,*zvec=&m->fvec;

	//ASSERT(fvec != NULL);

	*zvec = *fvec;
	if (vm_VectorNormalize(zvec) == 0) {
		return;
	}

	if (uvec == NULL) {

		if (rvec == NULL) {		//just forward vec

bad_vector2:
	;

			if (zvec->x==0 && zvec->z==0) {		//forward vec is straight up or down

				m->rvec.x = 1.0;
				m->uvec.z = (zvec->y<0)?1.0:-1.0;

				m->rvec.y = m->rvec.z = m->uvec.x = m->uvec.y = 0;
			}
			else { 		//not straight up or down

				xvec->x = zvec->z;
				xvec->y = 0;
				xvec->z = -zvec->x;

				vm_VectorNormalize(xvec);

				*yvec = *zvec ^ *xvec;

			}

		}
		else {						//use right vec

			*xvec = *rvec;
			if (vm_VectorNormalize(xvec) == 0)
				goto bad_vector2;

			*yvec = *zvec ^ *xvec;

			//normalize new perpendicular vector
			if (vm_VectorNormalize(yvec) == 0)
				goto bad_vector2;

			//now recompute right vector, in case it wasn't entirely perpendiclar
			*xvec = *yvec ^ *zvec;
		}
	}
	else {		//use up vec

		*yvec = *uvec;
		if (vm_VectorNormalize(yvec) == 0)
			goto bad_vector2;

		*xvec = *yvec ^ *zvec;
		
		//normalize new perpendicular vector
		if (vm_VectorNormalize(xvec) == 0)
			goto bad_vector2;

		//now recompute up vector, in case it wasn't entirely perpendiclar
		*yvec = *zvec ^ *xvec;

	}
}

//Compute a matrix from one or two vectors.  At least one and at most two vectors must/can be specified.
//Parameters:	m - filled in with the orienation matrix
//					fvec,uvec,rvec - pointers to vectors that determine the matrix.
//						One or two of these must be specified, with the other(s) set to NULL.
void vm_VectorToMatrix(matrix *m,vector *fvec,vector *uvec,vector *rvec)
{
	if (! fvec) {					//no forward vector.  Use up and/or right vectors.
		matrix tmatrix;

		if (uvec) {					//got up vector. use up and, if specified, right vectors.
			DoVectorToMatrix(&tmatrix,uvec,NULL,rvec);
			m->fvec = -tmatrix.uvec;
			m->uvec =  tmatrix.fvec;
			m->rvec =  tmatrix.rvec;
			return;
		}
		else {						//no up vector.  Use right vector only.
			//ASSERT(rvec);
			DoVectorToMatrix(&tmatrix,rvec,NULL,NULL);
			m->fvec = -tmatrix.rvec;
			m->uvec =  tmatrix.uvec;
			m->rvec =  tmatrix.fvec;
			return;
		}
	}
	else {
		//ASSERT(! (uvec && rvec));		//can only have 1 or 2 vectors specified
		DoVectorToMatrix(m,fvec,uvec,rvec);
	}
}

void vm_SinCos(unsigned short a, float *s, float *c)
{
	if(s)
		*s = FixSin(a);
	if(c)
		*c = FixCos(a);
}

//extract angles from a matrix 
angvec *vm_ExtractAnglesFromMatrix(angvec *a,matrix *m)
{
	float sinh,cosh,cosp;

	if (m->fvec.x==0 && m->fvec.z==0)		//zero head
		a->h = 0;
	else
		a->h = FixAtan2(m->fvec.z,m->fvec.x);

	sinh=FixSin(a->h);
	cosh=FixCos(a->h);

	if (fabs(sinh) > fabs(cosh))				//sine is larger, so use it
		cosp = (m->fvec.x/sinh);
	else										//cosine is larger, so use it
		cosp = (m->fvec.z/cosh);

	if (cosp==0 && m->fvec.y==0)
		a->p = 0;
	else
		a->p = FixAtan2(cosp,-m->fvec.y);


	if (cosp == 0)	//the cosine of pitch is zero.  we're pitched straight up. say no bank

		a->b = 0;

	else {
		float sinb,cosb;

		sinb = (m->rvec.y/cosp);
		cosb = (m->uvec.y/cosp);

		if (sinb==0 && cosb==0)
			a->b = 0;
		else
			a->b = FixAtan2(cosb,sinb);

	}

	return a;
}

//returns the value of a determinant
float calc_det_value(matrix *det)
{
	return 	det->rvec.x*det->uvec.y*det->fvec.z -
			 	det->rvec.x*det->uvec.z*det->fvec.y -
			 	det->rvec.y*det->uvec.x*det->fvec.z +
			 	det->rvec.y*det->uvec.z*det->fvec.x +
			 	det->rvec.z*det->uvec.x*det->fvec.y -
			 	det->rvec.z*det->uvec.y*det->fvec.x;
}



//computes the delta angle between two vectors. 
//vectors need not be normalized. if they are, call vm_vec_delta_ang_norm()
//the forward vector (third parameter) can be NULL, in which case the absolute
//value of the angle in returned.  Otherwise the angle around that vector is
//returned.

angle vm_DeltaAngVec(vector *v0,vector *v1,vector *fvec)
{
	vector t0,t1;

	t0 = *v0;
	t1 = *v1;

	vm_VectorNormalize(&t0);
	vm_VectorNormalize(&t1);

	return vm_DeltaAngVecNorm(&t0,&t1,fvec);
}

//computes the delta angle between two normalized vectors. 
angle vm_DeltaAngVecNorm(vector *v0,vector *v1,vector *fvec)
{
	angle a;

	a = FixAcos(vm_DotProduct(v0,v1));

	if (fvec) {
		vector t;

		vm_CrossProduct(&t,v0,v1);
		if (vm_DotProduct(&t,fvec) < 0) 
			a = -a;
	}

	return a;
}

// Gets the real center of a polygon
// Returns the size of the passed in stuff
float vm_GetCentroid (vector *centroid,vector *src,int nv)
{
	//ASSERT (nv>2);
	vector normal;
	float area,total_area;
	int i;
	vector tmp_center;

	vm_MakeZero (centroid);
		
	// First figure out the total area of this polygon
	vm_GetPerp (&normal,&src[0],&src[1],&src[2]);
	total_area=(vm_GetMagnitude (&normal)/2);
		
	for (i=2;i<nv-1;i++)
	{
		vm_GetPerp (&normal,&src[0],&src[i],&src[i+1]);
		area=(vm_GetMagnitude (&normal)/2);
		total_area+=area;
	}
	
	// Now figure out how much weight each triangle represents to the overall
	// polygon
	vm_GetPerp (&normal,&src[0],&src[1],&src[2]);
	area=(vm_GetMagnitude (&normal)/2);

	// Get the center of the first polygon
	vm_MakeZero (&tmp_center);
	for (i=0;i<3;i++)
	{
		tmp_center+=src[i];
	}
	tmp_center/=3;

	*centroid+=(tmp_center*(area/total_area));

	// Now do the same for the rest	
	for (i=2;i<nv-1;i++)
	{
		vm_GetPerp (&normal,&src[0],&src[i],&src[i+1]);
		area=(vm_GetMagnitude (&normal)/2);

		vm_MakeZero (&tmp_center);
		
		tmp_center+=src[0];
		tmp_center+=src[i];
		tmp_center+=src[i+1];
		
		tmp_center/=3;

		*centroid+=(tmp_center*(area/total_area));
	}

	return total_area;
}

// Gets the real center of a polygon, but uses fast magnitude calculation
// Returns the size of the passed in stuff
float vm_GetCentroidFast (vector *centroid,vector *src,int nv)
{
	//ASSERT (nv>2);
	vector normal;
	float area,total_area;
	int i;
	vector tmp_center;

	vm_MakeZero (centroid);
		
	// First figure out the total area of this polygon
	vm_GetPerp (&normal,&src[0],&src[1],&src[2]);
	total_area=(vm_GetMagnitudeFast (&normal)/2);
		
	for (i=2;i<nv-1;i++)
	{
		vm_GetPerp (&normal,&src[0],&src[i],&src[i+1]);
		area=(vm_GetMagnitudeFast (&normal)/2);
		total_area+=area;
	}
	
	// Now figure out how much weight each triangle represents to the overall
	// polygon
	vm_GetPerp (&normal,&src[0],&src[1],&src[2]);
	area=(vm_GetMagnitudeFast (&normal)/2);

	// Get the center of the first polygon
	vm_MakeZero (&tmp_center);
	for (i=0;i<3;i++)
	{
		tmp_center+=src[i];
	}
	tmp_center/=3;

	*centroid+=(tmp_center*(area/total_area));

	// Now do the same for the rest	
	for (i=2;i<nv-1;i++)
	{
		vm_GetPerp (&normal,&src[0],&src[i],&src[i+1]);
		area=(vm_GetMagnitudeFast (&normal)/2);

		vm_MakeZero (&tmp_center);
		
		tmp_center+=src[0];
		tmp_center+=src[i];
		tmp_center+=src[i+1];
		
		tmp_center/=3;

		*centroid+=(tmp_center*(area/total_area));
	}

	return total_area;
}


//	creates a completely random, non-normalized vector with a range of values from -1023 to +1024 values)
void vm_MakeRandomVector(vector *vec)
{
	vec->x = rand() - RAND_MAX/2;
	vec->y = rand() - RAND_MAX/2;
	vec->z = rand() - RAND_MAX/2;
}


// Given a set of points, computes the minimum bounding sphere of those points
float vm_ComputeBoundingSphere(vector *center,vector *vecs,int num_verts)
{
	//This algorithm is from Graphics Gems I.  There's a better algorithm in Graphics Gems III that
	//we should probably implement sometime.

	vector *min_x,*max_x,*min_y,*max_y,*min_z,*max_z,*vp;
	float dx,dy,dz;
	float rad,rad2;
	int i;

	//Initialize min, max vars
	min_x = max_x = min_y = max_y = min_z = max_z = &vecs[0];

	//First, find the points with the min & max x,y, & z coordinates
	for (i=0,vp=vecs;i<num_verts;i++,vp++) {

		if (vp->x < min_x->x)
			min_x = vp;

		if (vp->x > max_x->x)
			max_x = vp;

		if (vp->y < min_y->y)
			min_y = vp;

		if (vp->y > max_y->y)
			max_y = vp;

		if (vp->z < min_z->z)
			min_z = vp;

		if (vp->z > max_z->z)
			max_z = vp;
	}

	//Calculate initial sphere

	dx = vm_VectorDistance(min_x,max_x);
	dy = vm_VectorDistance(min_y,max_y);
	dz = vm_VectorDistance(min_z,max_z);

	if (dx > dy)
		if (dx > dz) {
			*center = (*min_x + *max_x) / 2;  rad = dx / 2;
		}
		else {
			*center = (*min_z + *max_z) / 2;  rad = dz / 2;
		}
	else
		if (dy > dz) {
			*center = (*min_y + *max_y) / 2;  rad = dy / 2;
		}
		else {
			*center = (*min_z + *max_z) / 2;  rad = dz / 2;
		}


	//Go through all points and look for ones that don't fit
	rad2 = rad * rad;
	for (i=0,vp=vecs;i<num_verts;i++,vp++) {
		vector delta;
		float t2;
		
		delta = *vp - *center;
		t2 = delta.x * delta.x + delta.y * delta.y + delta.z * delta.z;

		//If point outside, make the sphere bigger
		if (t2 > rad2) {
			float t;

			t = sqrt(t2);
			rad = (rad + t) / 2;
			rad2 = rad * rad;
			*center += delta * (t - rad) / t;
		}
	}

	//We're done
	return rad;
}

#endif