//#define DEBUG

#include "global.h"
#include "maths.h"

/*************************************************************************/
void testrou(int a,int b,int c,int d,int e,int f){

}

/*
void testMagnitude(void)
{
	VECTOR src; //={-532,2366,-5655};
	VECTOR dest; //={8192,42343,-5345};
//	VECTOR copy,copy1;
	int i,time;
//	int error=0;

	testrou(1,2,3,4,5,6);

	while(1)
	{
		time=vblank;
		VSync(0);
		for(i=0; i<50000; i++)
		{
			//DotProduct(&u, &v);
			
//			copy.vx=random(0xffff);
//			copy.vy=random(0xffff);
//			copy.vz=random(0xffff);
//			COPYVECTOR(&src,&copy);
//			MakeUnit(&src);
//			COPYVECTOR(&copy1,&src);
//			COPYVECTOR(&src,&copy);
//			VectorNormal(&src,&dest);
//			CompareVectors(&copy1,&dest);
//			printf("x,y,z,(%d,%d,%d)=%d,%d,%d : %d,%d,%d \n",copy.vx,copy.vy,copy.vz,copy1.vx,copy1.vy,copy1.vz,dest.vx,dest.vy,dest.vz);


		}
//		printf("1st took %d\n", vblank-time);

		time=vblank;
		VSync(0);
		for(i=0; i<50000; i++)
		{
			VectorNormal(&src,&dest);
		}
//		printf("2nd took %d\n", vblank-time);
	}
	CRASH;
}

short pDivTable[MAX_DIVS];

// a multiply is 11 cycles, a divide is 35 !!!
void divtableInit(void)
{
	int i;
	pDivTable[0]=0;
	for(i=1; i<MAX_DIVS; i++)
	{
		pDivTable[i]=4096/i;
	}
	//testMagnitude();
}
*/

/***********************************************************************************/
UBYTE randomtable[]={
	0xD6,0x89,0xFA,0x41,0xEF,0xDA,0x1E,0x1C,0xC0,0xF8,0x6D,0xF3,0x74,0x9A,0x09,0xCC,
	0x73,0xD8,0xEC,0x8B,0x94,0x2D,0x36,0x77,0xE6,0x06,0xC5,0xDB,0xEA,0x3F,0xEB,0xED,
	0x19,0x13,0x05,0xC9,0xE7,0xF2,0xA0,0x0A,0x7E,0xE1,0x95,0x11,0x47,0xE4,0x80,0xF7,
	0x08,0xF0,0x9C,0x0B,0x3D,0x0C,0x63,0x57,0x76,0xB8,0x85,0x01,0xCD,0xB4,0x59,0xD4,
	0x39,0x31,0xBC,0x43,0x02,0xB5,0x9E,0xFF,0xA3,0x1B,0x64,0xFE,0x75,0x97,0x83,0x5A,
	0x4B,0x53,0x14,0x3E,0xB2,0xA4,0xF9,0xF4,0x98,0x20,0x7D,0xB9,0xAC,0x1D,0xBE,0xD2,
	0x29,0x33,0x42,0xC8,0x92,0xDC,0xA1,0x72,0x91,0xD3,0xCA,0xDE,0x55,0x7F,0x6E,0x62,
	0x38,0x81,0x65,0x28,0xC2,0xBD,0x16,0xAF,0x3B,0xF6,0x82,0x96,0x8C,0x3A,0x99,0x9D,
	0x56,0x49,0xE2,0x7B,0x2F,0xFC,0x10,0x1F,0x5B,0xE3,0x8A,0x18,0xFD,0x0D,0xCE,0x1A,
	0xE0,0xAB,0x32,0x60,0x27,0xDF,0x23,0x44,0x61,0x26,0x8F,0xBB,0xBF,0x5D,0xD9,0x52,
	0xC6,0xA6,0x8D,0x30,0x5C,0x12,0x21,0xAA,0x69,0x90,0xC7,0x68,0x2C,0x6A,0x2E,0x45,
	0xC1,0x71,0x15,0x58,0x7C,0xCF,0x03,0xC4,0xAE,0xEE,0x3C,0xF1,0x84,0x17,0x46,0x5F,
	0x88,0xB0,0x0F,0x40,0xB7,0x04,0x66,0xA5,0x70,0xB6,0x25,0xCB,0x6F,0x35,0x9B,0x22,
	0x50,0xE8,0x87,0xA9,0x9F,0xE5,0x4E,0x4D,0xD0,0x5E,0xA2,0xFB,0xDD,0x24,0xD1,0xD5,
	0xA8,0x6B,0x4C,0xB1,0x4A,0x07,0x51,0xA7,0x2B,0x86,0x67,0xB3,0x2A,0xAD,0x78,0xBA,
	0x00,0xC3,0x6C,0x93,0x4F,0xF5,0x79,0x37,0x8E,0x48,0xD7,0x0E,0x7A,0x34,0xE9,0x54};
UBYTE rndcnt = 0;



// Okay, this routine now works properly.
// The reason it was commented out was that it wasn't working.

LONG random(ULONG size)
{
	if(size==0)
		return 0;

	if(size<256)
		return (  ((ULONG)RANDOM256())  MOD size);

	if(size<65536)
		return ((
		  ( ((ULONG)RANDOM256())  <<8)
		+   ((ULONG)RANDOM256())
		)
		MOD size);

	return (
		  ( ((ULONG)RANDOM256())  <<24)
		+ ( ((ULONG)RANDOM256())  <<16)
		+ ( ((ULONG)RANDOM256())   <<8)
		+   ((ULONG)RANDOM256())
		)
		MOD size;
}
/**************************************************************************/
UBYTE GetRandomSeed(void)
{
	return rndcnt;
}

void SetRandomSeed(UBYTE seed)
{
	rndcnt=seed;
}

/***********************************************************************************/
// calc the angle given opposite and adjacent where 0-4096 = 0-360'
// lots faster than atan functions !
// returns a long: msw =hypoteneuse
// 				   lsw =angle 0-4906

//#ifndef DREAMCAST
long calc_angle(LONG adj,LONG opp)
{
	LONG hyp,xsgn,ysgn,sine,quadrant,calc;
	long quadrants[]={ 0,1024,3072,2048};	//
	long quads[]={ 0,1,1,0 };				//

	xsgn = (adj < 0);									// +/-
	ysgn = (opp < 0);									// +/-

	quadrant = (xsgn * 2) + ysgn;							//

// twice sum of squares limit is 32767
// (three times sum of squares limit is 26754, used in the 3d magnitude test)
	while(adj > 32767 || opp > 32767 || adj < -32767 || opp < -32767)
	{
		adj = adj >> 2;
		opp = opp >> 2;
	}
													  
	calc=(adj * adj + opp * opp);
	FASTSQRT(hyp,calc);
	ysgn=((hyp & 0xffff) > 0x7fff);

	sine = ABS(opp) << 10;
	xsgn = ((hyp >> 16) + ysgn) & 0xffff;

	if(xsgn)
		sine = ABS(sine / xsgn);

	sine = acostable[sine];
	
	if(quads[quadrant])
		sine = 1024 - sine;

	sine = ((sine + quadrants[quadrant]) + 4096) & 4095;
	
	sine |= (hyp & 0xffff0000);
	return sine;
}   
//#endif

/*************************************************************************/

LONG arcsin(LONG sin)
{
	sin = (1024 - sin) >> 2;
	
	if(sin < 0)
		return 2048 - acostable[sin + 1024];
	else
		return acostable[sin];
}


/*************************************************************************/

LONG arccos(LONG cos)
{
	cos >>= 2;
	if (cos < 0)
		return 2048 - acostable[cos + 1024];
	else
		return acostable[cos];
}


/*************************************************************************************/
int CompareVectors(VECTOR *v1, VECTOR *v2)
{
	if((v1->vx)!=(v2->vx)) return(FALSE);
	if((v1->vy)!=(v2->vy)) return(FALSE);
	if((v1->vz)!=(v2->vz)) return(FALSE);
	return TRUE;
}

/*	--------------------------------------------------------------------------------
	Function 	: Magnitude()
	Purpose 	: find the magnitude of a vector
	Parameters 	: pointer to vector
	Returns 	: magnitude
	Info 		: INTEGER
*/

long Magnitude(VECTOR *v)
{
	ULONG x,y,z,calc;
	ULONG l;
	ULONG num;
	ULONG up;
	ULONG s=0;

	num=0;

	x=ABS(v->vx);
	num|=x;

	y=ABS(v->vy);
	num|=y;

	z=ABS(v->vz);
	num|=z;

	while(num > 26754)
	{
		num = num>>2;
		s+=2;
	}
	x=(x)>>s;
	y=(y)>>s;
	z=(z)>>s;

	calc=x*x+y*y+z*z;
	FASTSQRT(l,calc);
	up=16-s;

	l=l>>up;

	return(l);
}
/**************************************************************************************/
long Magnitude2D(long x,long y)
{
	ULONG calc;
	ULONG l;
	ULONG num;
	ULONG up;
	ULONG s=0;

	num=0;

	x=ABS(x);
	num|=x;

	y=ABS(y);
	num|=y;

	while(num > 26754)
	{
//		printf("magnitude overflow!!!\n");
		num = num>>2;
		s+=2;
	}
	x=(x)>>s;
	y=(y)>>s;

	calc=x*x+y*y;
	FASTSQRT(l,calc);
	up=16-s;

	l=l>>up;

	return(l);
}
/**************************************************************************************/
int findShortestAngle(int ang1, int ang2)
{
	int temp = ang1 - ang2;

	if ((temp > -2048) && (temp <= 2048))
		return temp;
	else if (temp > 2048)
		return ang1 - ang2 - 4096;
	else
		return ang1 + 4096 - ang2;
}

/**************************************************************************************/
long Magnitude2D_S(short x,short y)  //short version of above.
{	//much simplier, because 2 shorts wont overflow!
	ULONG calc;
	ULONG l;
	calc=x*x+y*y;
	FASTSQRT(l,calc);
	return(l);
}

/*	--------------------------------------------------------------------------------
	Function 	: MagnitudeS()
	Purpose 	: find the magnitude of a short vector
	Parameters 	: pointer to vector
	Returns 	: magnitude
	Info 		: INTEGER
*/

long MagnitudeS(SVECTOR *v)
{
	ULONG x,y,z;
	ULONG l,calc;
	ULONG num;
	ULONG up;
	ULONG s=0;

	num=0;

	x=ABS(v->vx);
	num|=x;
	
	y=ABS(v->vy);
	num|=y;
	
	z=ABS(v->vz);
	num|=z;
	
	while(num > 26754)
	{
		num = num>>2;
		s+=2;
	}

	x=(x)>>s;
	y=(y)>>s;
	z=(z)>>s;

	calc=x*x+y*y+z*z;
	FASTSQRT(l,calc);
	up=16-s;
	l=l>>up;

	return(l);
}



/*	--------------------------------------------------------------------------------
	Function 	: FindHeight()
	Purpose 	: Finds the height from center
	Parameters 	: pointer to vector
	Returns 	: Height
	Info 		: INTEGER
*/


// Note to Andrew: This is *not* the same function as "magnitude2d"
long FindHeight(long a,long b)
{
	ULONG a1,b1;
	ULONG l;
	ULONG num;
	ULONG up;
	ULONG s=0;
	ULONG calc;
	num=0;

	l=ABS(a);
	num|=l;
	a=l;

	l=ABS(b);
	num|=l;
	b=l;

	while(num > 26754)
	{
		num = num>>2;
		s+=2;
	}

	a1=(a)>>s;
	b1=(b)>>s;

	calc=(a1*a1)-(b1*b1);
	FASTSQRT(l,calc);

	up=16-s;
  	l=l>>up;
	return(l);
}


/************************************************************************************/
void CrossProduct(VECTOR *result, VECTOR *operand1, VECTOR *operand2)
{
	result->vx=(operand1->vy * operand2->vz - operand1->vz * operand2->vy)/FRACTIONAL_DIV;
	result->vy=(operand1->vz * operand2->vx - operand1->vx * operand2->vz)/FRACTIONAL_DIV;
	result->vz=(operand1->vx * operand2->vy - operand1->vy * operand2->vx)/FRACTIONAL_DIV;
}

/************************************************************************************/

long DotProductS(SVECTOR *v1, SVECTOR *v2)
{
	VECTOR u1,u2;
	long num=0;
	long result;
	int mf;
	int s=0;

	num|=(ABS(v1->vx));
	num|=(ABS(v2->vx));
	num|=(ABS(v1->vy));
	num|=(ABS(v2->vy));
	num|=(ABS(v1->vz));
	num|=(ABS(v2->vz));

	while(num > 26754) // anything squared *3 bigger than this will overflow a long
	{
		num = num>>2;
		s+=2;
	}

	if(s)
	{
		mf=1<<s;
	
		u1.vx=(v1->vx)/mf;
		u2.vx=(v2->vx)/mf;
		u1.vy=(v1->vy)/mf;
		u2.vy=(v2->vy)/mf;
		u1.vz=(v1->vz)/mf;
		u2.vz=(v2->vz)/mf;

		if(s<6)
			result=((u1.vx)*(u2.vx) + (u1.vy)*(u2.vy) + (u1.vz)*(u2.vz))/(1<<(12-(2*s)));
		else
			if(s==6)
				result=(u1.vx)*(u2.vx) + (u1.vy)*(u2.vy) + (u1.vz)*(u2.vz);
			else
				result=((u1.vx)*(u2.vx) + (u1.vy)*(u2.vy) + (u1.vz)*(u2.vz))<<((2*s)-12);
	}
	else
	{
		result=((v1->vx)*(v2->vx) + (v1->vy)*(v2->vy) + (v1->vz)*(v2->vz))/4096;
	}

	return(result);
}
/****************************************************************************************/
long DotProduct(VECTOR *v1, VECTOR *v2)
{
	VECTOR u1,u2;
	long num;
	long result;
	int mf;
	int s=0;

#ifdef USE_DIVTABLE
	int dt;
#endif

	num=0;

	num|=(ABS(v1->vx));
	num|=(ABS(v2->vx));
	num|=(ABS(v1->vy));
	num|=(ABS(v2->vy));
	num|=(ABS(v1->vz));
	num|=(ABS(v2->vz));

	while(num > 26754) // anything squared *3 bigger than this will overflow a long
	{
		num = num>>2;
		s+=2;
	}

	if(s)
	{
		mf=1<<s;

#ifdef USE_DIVTABLE
		if(mf>=MAX_DIVS) mf=MAX_DIVS;
		
		dt=(int)pDivTable[mf];

		u1.vx=(v1->vx)*dt;
		u2.vx=(v2->vx)*dt;
		u1.vy=(v1->vy)*dt;
		u2.vy=(v2->vy)*dt;
		u1.vz=(v1->vz)*dt;
		u2.vz=(v2->vz)*dt;
#else
		u1.vx=(v1->vx)/mf;
		u2.vx=(v2->vx)/mf;
		u1.vy=(v1->vy)/mf;
		u2.vy=(v2->vy)/mf;
		u1.vz=(v1->vz)/mf;
		u2.vz=(v2->vz)/mf;
#endif

		if(s<6)
			result=((u1.vx)*(u2.vx) + (u1.vy)*(u2.vy) + (u1.vz)*(u2.vz))/(1<<(12-(2*s)));
		else
			if(s==6)
				result=(u1.vx)*(u2.vx) + (u1.vy)*(u2.vy) + (u1.vz)*(u2.vz);
			else
				result=((u1.vx)*(u2.vx) + (u1.vy)*(u2.vy) + (u1.vz)*(u2.vz))<<((2*s)-12);
	}
	else
	{
		result=((v1->vx)*(v2->vx) + (v1->vy)*(v2->vy) + (v1->vz)*(v2->vz))/4096;
	}

	return(result);
}

/*	--------------------------------------------------------------------------------
	Function 	: MakeUnit()
	Purpose 	: makes a vector unit magnitude
	Parameters 	: pointer to vector
	Returns 	: none
	Info 		: INTEGER (this needs looking at to stop overflows)
*/
int MakeUnit(VECTOR *vect)
{
	int tb,s,d;
	long n;
	long m = Magnitude(vect);

	if(m != 0)
	{
		// find top bit
		n=abs(vect->vx)|abs(vect->vy)|abs(vect->vz);

		for(tb=31; !(n&0x80000000); tb--)  	n=n<<1;

		if(tb>18)
		{
			s=30-tb;
			d=tb-18;
			vect->vx= (vect->vx<<s)/(m>>d);
			vect->vy= (vect->vy<<s)/(m>>d);
			vect->vz= (vect->vz<<s)/(m>>d);
		}
		else
		{
			vect->vx= (vect->vx<<FRACTIONAL_BITS)/m;
			vect->vy= (vect->vy<<FRACTIONAL_BITS)/m;
			vect->vz= (vect->vz<<FRACTIONAL_BITS)/m;
		}
	}

	return m;
}
/****************************************************************************************/
void CalcBounce(VECTOR *vel,VECTOR *normal)
{
	long		NdotL;
	MakeUnit(vel);
	NdotL = (-DotProduct(vel,normal))*2;
	vel->vx+= (normal->vx*NdotL)/FRACTIONAL_DIV;
	vel->vy+= (normal->vy*NdotL)/FRACTIONAL_DIV;
	vel->vz+= (normal->vz*NdotL)/FRACTIONAL_DIV;
	MakeUnit(vel);
}
/*	--------------------------------------------------------------------------------
	Function 	: RotateVector2D
	Purpose 	: rotates the X and Z points around the Y axis, by given angle
	Parameters 	: result vector, source vector, angle
	Returns 	: 
	Info 		: destination CAN be same as source
*/

void RotateVector2D(VECTOR *dest, VECTOR *source, LONG angle)
{
	VECTOR temp;
	LONG cosangle;
	LONG sinangle;

	angle = -angle;

	cosangle = rcos(angle);
	sinangle = rsin(angle);

	temp.vy = source->vy;
	temp.vx = ((cosangle * source->vx) + ((-sinangle) * source->vz))/4096;
	temp.vz = ((cosangle * source->vz) + (sinangle * source->vx))/4096;

	COPYVECTOR(dest,&temp);
}


/*	--------------------------------------------------------------------------------
	Function 	: RotateVector2DXYZ
	Purpose 	: Rotate about a given axis 0=x 1=y 2=z
	Parameters 	: result vector, source vector, angle
	Returns 	: 
	Info 		: destination CAN be same as source
*/
void RotateVector2DXYZ(VECTOR *dest, VECTOR *source, LONG angle, BYTE axis)
{
	VECTOR	temp;
	LONG cosangle;
	LONG sinangle;

	angle = -angle;

	cosangle = rcos(angle);
	sinangle = rsin(angle);

	switch(axis)
	{
		case 0://X:
			temp.vx=source->vx;
			temp.vy = ((cosangle * source->vy) + ((-sinangle) * source->vz))/4096;
			temp.vz = ((cosangle * source->vz) + (sinangle * source->vy))/4096;
			break;
		case 1://Y:
			temp.vy=source->vy;
			temp.vz = ((cosangle * source->vz) + ((-sinangle) * source->vx))/4096;
			temp.vx = ((cosangle * source->vx) + (sinangle * source->vz))/4096;
			break;
		case 2://Z:
			temp.vz=source->vz;
			temp.vx = ((cosangle * source->vx) + ((-sinangle) * source->vy))/4096;
			temp.vy = ((cosangle * source->vy) + (sinangle * source->vx))/4096;
			break;
	}
	COPYVECTOR(dest,&temp);
}

/**********************************************************************************************************************/

// speed < 0 means "go there NOW"
// returns YES/NO for arrival
int VectorMoveTo(VECTOR *src, VECTOR *dest, int speed)
{
	VECTOR d;
	int m;

	if(speed < 0)
	{
//		*src = *dest;
		src->vx = dest->vx;	// just copy over the three ints. Not the full four of the vector.
		src->vy = dest->vy;	// (Important, coz we're sometimes casting 3 ints to be a vector)
		src->vz = dest->vz;
		return YES;
	}
	d.vx = dest->vx - src->vx;
	d.vy = dest->vy - src->vy;
	d.vz = dest->vz - src->vz;
	m = Magnitude(&d);
	if(m <= speed)
	{
//		*src = *dest;
		src->vx = dest->vx;	// just copy over the three ints. Not the full four of the vector.
		src->vy = dest->vy;	// (Important, coz we're sometimes casting 3 ints to be a vector)
		src->vz = dest->vz;
		return YES;
	}
	else
	{

// Get rid of overflow considerations...
		while(d.vx > 26754 || d.vx < -26754
			|| d.vy > 26754 || d.vy < -26754
			|| d.vz > 26754 || d.vz < -26754)
		{
			d.vx = d.vx >> 2;
			d.vy = d.vy >> 2;
			d.vz = d.vz >> 2;
			m = m >> 2;
		}
		while(speed > 26754)
		{
			speed = speed >> 2;
			m = m >> 2;
		}

		src->vx += d.vx * speed / m;
		src->vy += d.vy * speed / m;
		src->vz += d.vz * speed / m;
		return NO;
	}
}
/*******************************************************************/
int VectorMoveTo2D(VECTOR *src, VECTOR *dest, int speed)
{
	VECTOR d;
	int m;

	if(speed < 0)
	{
//		*src = *dest;
		src->vx = dest->vx;
		src->vz = dest->vz;
		return YES;
	}
	d.vx = dest->vx - src->vx;
	d.vy = 0;
	d.vz = dest->vz - src->vz;
	m = Magnitude2D(d.vx,d.vz);
	if(m <= speed)
	{
//		*src = *dest;
		src->vx = dest->vx;
		src->vz = dest->vz;
		return YES;
	}
	else
	{

// Get rid of overflow considerations...
		while(d.vx > 26754 || d.vx < -26754
			|| d.vz > 26754 || d.vz < -26754)
		{
			d.vx = d.vx >> 2;
			d.vz = d.vz >> 2;
			m = m >> 2;
		}
		while(speed > 26754)
		{
			speed = speed >> 2;
			m = m >> 2;
		}

		src->vx += d.vx * speed / m;
		src->vz += d.vz * speed / m;
		return NO;
	}
}

/*************************************************************************/

int SVectorMoveTo(SVECTOR *src, SVECTOR *dest, int speed)
{
	SVECTOR d;
	int m;

	if(speed < 0)
	{
		*src = *dest;
		return YES;
	}
	d.vx = dest->vx - src->vx;
	d.vy = dest->vy - src->vy;
	d.vz = dest->vz - src->vz;
	m = MagnitudeS(&d);
	if(m <= speed)
	{
		*src = *dest;
		return YES;
	}
	else
	{
		src->vx += d.vx * speed / m;
		src->vy += d.vy * speed / m;
		src->vz += d.vz * speed / m;
		return NO;
	}
}

void mathsPrintMatrix(MATRIX *pM)
{
	int c;

	for(c=0; c<3; c++)
	{
		DB("m[%d] = (%d, %d, %d)\n", c, pM->m[c][0], pM->m[c][1], pM->m[c][2]);
	}

	DB("\nt = (%d, %d, %d)\n\n", pM->t[0], pM->t[1], pM->t[2]);
}

void mathsCalcInverseMatrix(MATRIX *pDest, MATRIX *pSrc)
{
#ifndef DREAMCAST
	int r,c;
	MATRIX temp;
	SVECTOR sv;

	for(r=0; r<3; r++)
	{
		for(c=0; c<3; c++)
			temp.m[r][c] = pSrc->m[c][r];
	}

	gte_SetRotMatrix(&temp);

	sv.vx = (short)pSrc->t[0];
	sv.vy = (short)pSrc->t[1];
	sv.vz = (short)pSrc->t[2];

	gte_ldsv(&sv);
	gte_rtir();
	gte_mvlvtr();	// CPW: move long vector to translation vector

	gte_ReadRotMatrix(pDest);	// CPW: stores translation vector as well
#endif
}

void mathsMulMatrixProperly0(MATRIX *pC, MATRIX *pA, MATRIX *pB)
{
	// CPW: gte_MulMatrix0(&pE->head.pHead->matrixscale, pMatrix, &pE->override.data[pE->override.num].matrix);

	// CPW: do the rotatey bit
	gte_MulMatrix0(pA, pB, pC);

	// CPW: do the translatey bit
	gte_SetRotMatrix(pA);
	gte_SetTransMatrix(pA);
	gte_ldlvl(&pB->t);
	gte_rtirtr();
	gte_stlvl(&pC->t);
}


int mathsDistancePointToLine3D(VECTOR *p1, VECTOR *p2, VECTOR *point, VECTOR *out)
{
	VECTOR dirn;
	VECTOR dp;
	int mag;
	int dot;
	dirn.vx = p2->vx - p1->vx;
	dirn.vy = p2->vy - p1->vy;
	dirn.vz = p2->vz - p1->vz;

	dp.vx = point->vx - p1->vx;
	dp.vy = point->vy - p1->vy;
	dp.vz = point->vz - p1->vz;

	mag = MakeUnit(&dirn);
	if(mag == 0)
	{
		if(out)
		{
			*out = *p1;
		}
		return(Magnitude(&dp));
	}
	dot = DotProduct(&dp,&dirn);

	if(dot < 0)
	{
		if(out)
		{
			*out = *p1;
		}
		return Magnitude(&dp);
	}
	else if(dot > mag)
	{
		if(out)
		{
			*out = *p2;

		}
		dp.vx = point->vx - p2->vx;
		dp.vy = point->vy - p2->vy;
		dp.vz = point->vz - p2->vz;
		return (Magnitude(&dp));
	}
	else
	{
		int s = 12;
		while(dot > 0x7ffff)
		{
			dot = dot >> 2;
			s -= 2;
		}
		dp.vx = p1->vx + ((dot * dirn.vx) >> s);
		dp.vy = p1->vy + ((dot * dirn.vy) >> s);
		dp.vz = p1->vz + ((dot * dirn.vz) >> s);

		if(out)
		{
			*out = dp;
		}
		dp.vx = point->vx - dp.vx;
		dp.vy = point->vy - dp.vy;
		dp.vz = point->vz - dp.vz;
		return (Magnitude(&dp));
	}
}

int mathsDistancePointToLine2D(VECTOR *p1, VECTOR *p2, VECTOR *point, VECTOR *out)
{
	VECTOR dirn;
	VECTOR dp;
	int mag;
	int dot;
	dirn.vx = p2->vx - p1->vx;
	dirn.vy = 0;
	dirn.vz = p2->vz - p1->vz;

	dp.vx = point->vx - p1->vx;
	dp.vy = 0;
	dp.vz = point->vz - p1->vz;

//	utilPrintf("dp = %d %d %d, dirn = %d %d %d",dp.vx/8,-dp.vy/8,dp.vz/8, dirn.vx/8,-dirn.vy/8,dirn.vz/8);

	mag = MakeUnit(&dirn);

	if(mag == 0)
	{
		if(out)
		{
			*out = *p1;
//			utilPrintf("p2l: p1 = %d %d %d, p2 = %d %d %d. Zero \n",p1->vx/8,-p1->vy/8,p1->vz/8,p2->vx/8,-p2->vy/8,p2->vz/8);
		}

		return(Magnitude(&dp));
	}

	dot = DotProduct(&dp,&dirn);

//	utilPrintf("unitdirn = %d %d %d, magdirn = %d, dot = %d\n",dirn.vx, -dirn.vx, dirn.vz, mag/8,dot/8);

//	dot = dot / 4096;
//	utilPrintf("dot = %d, mag = %d\n",dot/8,mag/8);

	if(dot < 0)
	{
		if(out)
		{
			*out = *p1;
//			utilPrintf("p2l: p1 = %d %d %d, p2 = %d %d %d. End1 \n",p1->vx/8,-p1->vy/8,p1->vz/8,p2->vx/8,-p2->vy/8,p2->vz/8);
		}
		return Magnitude(&dp);
	}
	else if(dot > mag)
	{
		if(out)
		{
			*out = *p2;
//			utilPrintf("p2l: p1 = %d %d %d, p2 = %d %d %d. End2 \n",p1->vx/8,-p1->vy/8,p1->vz/8,p2->vx/8,-p2->vy/8,p2->vz/8);
		}
		dp.vx = point->vx - p2->vx;
		dp.vy = 0;
		dp.vz = point->vz - p2->vz;
		return (Magnitude(&dp));
	}
	else
	{
		int s = 12;
// Dirn is a unit vec
// dot is a number from 0 up to the magnitude of the (p1->p2) vector.
		while(dot > 0x7ffff)
		{
			dot = dot >> 2;
			s -= 2;
		}
//		utilPrintf("dot = %d, s= %d\n",dot,s);
		dp.vx = p1->vx + ((dot * dirn.vx) >> s);
		dp.vy = p1->vy + ((dot * dirn.vy) >> s);	// ummm
		dp.vz = p1->vz + ((dot * dirn.vz) >> s);

		if(out)
		{
			*out = dp;
//			utilPrintf("p2l: p1 = %d %d %d, p2 = %d %d %d. Mid \n",p1->vx/8,-p1->vy/8,p1->vz/8,p2->vx/8,-p2->vy/8,p2->vz/8);
//			utilPrintf("    out = %d %d %d\n",out->vx/8,-out->vy/8,out->vz/8);
		}
		dp.vx = point->vx - dp.vx;
		dp.vy = 0;
		dp.vz = point->vz - dp.vz;
		return (Magnitude(&dp));
	}
}

