Initial revision

1 files changed, 2921 insertions, 0 deletions

diff --git a/src/maths.c b/src/maths.c
new file mode 100644
index 0000000..42546bb
--- /dev/null
+++ b/src/maths.c
@@ -0,0 +1,2921 @@
+
+#if PSX
+#include <kernel.h>
+#include <sys/types.h>		   	  
+#include <libetc.h>
+#include <libgte.h>		
+#include <libgpu.h>
+#include <stdlib.h>					 
+#include <inline_c.h>
+#include <gtemac.h>
+#endif
+
+#include "3dc.h"
+#include "inline.h"
+
+#define UseTimsPinp Yes
+
+#define trip_debugger No
+
+#if trip_debugger
+int testa = 0;
+int testb = 100;
+int testc = 0;
+#endif
+
+
+/*
+
+ externs for commonly used global variables and arrays
+
+*/
+
+	#if platform_pc
+	extern int sine[];
+	extern int cosine[];
+	#endif
+
+	extern short ArcCosTable[];
+	extern short ArcSineTable[];
+	extern short ArcTanTable[];
+
+	extern LONGLONGCH ll_zero;
+
+	extern int NormalFrameTime;
+
+
+#if PSX
+extern unsigned long *scratchp;
+#endif
+
+/*
+
+ Globals
+
+*/
+
+	MATRIXCH IdentityMatrix = {
+
+		ONE_FIXED, 0, 0,
+		0, ONE_FIXED, 0,
+		0, 0, ONE_FIXED
+
+	};
+
+
+
+/*
+
+ Maths functions used by the system
+
+*/
+
+
+
+#if PSX
+inline void ch2psx(MATRIXCH *chm, MATRIX *psxm)
+{
+  psxm->m[0][0] = chm->mat11 >> 4;
+  psxm->m[0][1] = chm->mat21 >> 4;
+  psxm->m[0][2] = chm->mat31 >> 4;
+  psxm->m[1][0] = chm->mat12 >> 4;
+  psxm->m[1][1] = chm->mat22 >> 4;
+  psxm->m[1][2] = chm->mat32 >> 4;
+  psxm->m[2][0] = chm->mat13 >> 4;
+  psxm->m[2][1] = chm->mat23 >> 4;
+  psxm->m[2][2] = chm->mat33 >> 4;
+}
+
+inline void psx2ch(MATRIX *psxm, MATRIXCH *chm)
+{
+  
+  chm->mat11 = psxm->m[0][0] << 4;
+  chm->mat21 = psxm->m[0][1] << 4;
+  chm->mat31 = psxm->m[0][2] << 4;
+  chm->mat12 = psxm->m[1][0] << 4;
+  chm->mat22 = psxm->m[1][1] << 4;
+  chm->mat32 = psxm->m[1][2] << 4;
+  chm->mat13 = psxm->m[2][0] << 4;
+  chm->mat23 = psxm->m[2][1] << 4;
+  chm->mat33 = psxm->m[2][2] << 4;
+}
+
+#endif
+
+/* One over sin functions - CDF 4/2/98 */
+
+extern int oneoversin[4096];
+
+void ConstructOneOverSinTable(void) {
+
+	int a,sin;
+
+	for (a=0; a<4096; a++) {
+		sin=GetSin(a);
+
+		if (sin!=0) {
+			oneoversin[a]=DIV_FIXED(ONE_FIXED,sin);
+		} else {
+			sin=100;
+			oneoversin[a]=DIV_FIXED(ONE_FIXED,sin);
+		}
+	}
+
+}
+
+int GetOneOverSin(int a) {
+
+	int b;
+
+	b=a&wrap360;
+	
+	return(oneoversin[b]);
+
+}
+
+/*
+
+ Dot Product Function
+
+ It accepts two pointers to vectors and returns an int result
+
+*/
+
+int _DotProduct(VECTORCH *vptr1, VECTORCH *vptr2)
+
+{
+
+	int dp;
+
+	dp =  MUL_FIXED(vptr1->vx, vptr2->vx);
+	dp += MUL_FIXED(vptr1->vy, vptr2->vy);
+	dp += MUL_FIXED(vptr1->vz, vptr2->vz);
+
+	return(dp);
+
+}
+
+
+int DotProduct2d(VECTOR2D *vptr1, VECTOR2D *vptr2)
+
+{
+
+	int dp;
+
+
+	dp  = MUL_FIXED(vptr1->vx, vptr2->vx);
+	dp += MUL_FIXED(vptr1->vy, vptr2->vy);
+
+	return dp;
+
+}
+
+
+/*
+
+ This function returns the distance between two vectors
+
+*/
+
+int VectorDistance(VECTORCH *v1, VECTORCH *v2)
+
+{
+
+	VECTORCH v;
+
+
+	v.vx = v1->vx - v2->vx;
+	v.vy = v1->vy - v2->vy;
+	v.vz = v1->vz - v2->vz;
+
+	return Magnitude(&v);
+
+}
+
+
+/*
+
+ This function compares the distance between two vectors along each of
+ the major axes and returns Yes or No if they are within the cube defined
+ by the argument passed.
+
+*/
+
+int OutcodeVectorDistance(VECTORCH *v1, VECTORCH *v2, int d)
+
+{
+
+	int i;
+
+
+	i = v1->vx - v2->vx;
+	if(i < 0) i = -i;
+
+	if(i >= d) return No;
+
+	i = v1->vy - v2->vy;
+	if(i < 0) i = -i;
+
+	if(i >= d) return No;
+
+	i = v1->vz - v2->vz;
+	if(i < 0) i = -i;
+
+	if(i >= d) return No;
+
+	return Yes;
+
+}
+
+
+/*
+
+ Subtract one VECTORCH from another and return the result as a normal
+
+ v3 = Normal(v1 - v2)
+
+*/
+
+void GetNormalVector(VECTORCH *v1, VECTORCH *v2, VECTORCH *v3)
+
+{
+
+	v3->vx = v1->vx - v2->vx;
+	v3->vy = v1->vy - v2->vy;
+	v3->vz = v1->vz - v2->vz;
+
+	Normalise(v3);
+
+}
+
+
+/*
+
+ Normalise a vector close to, but less than, unit length
+
+*/
+
+void Renormalise(VECTORCH *nvector)
+
+{
+
+	int m;
+	int xsq, ysq, zsq;
+
+
+/* Scale x, y and z */
+
+	nvector->vx >>= 2;
+	nvector->vy >>= 2;
+	nvector->vz >>= 2;
+
+
+/* Normalise */
+
+	xsq = nvector->vx * nvector->vx;
+	ysq = nvector->vy * nvector->vy;
+	zsq = nvector->vz * nvector->vz;
+
+	m = SqRoot32(xsq + ysq + zsq);
+
+	if(m == 0) m = 1;			/* Just in case */
+
+	nvector->vx = (nvector->vx * ONE_FIXED) / m;
+	nvector->vy = (nvector->vy * ONE_FIXED) / m;
+	nvector->vz = (nvector->vz * ONE_FIXED) / m;
+
+}
+
+
+
+
+
+
+
+
+
+/*
+
+ Return the shift value required to get one value LTE the other value
+
+*/
+
+int FindShift32(int value, int limit)
+
+{
+
+	int shift = 0;
+
+
+	/*if(limit == 0) exit(0xfa11fa11);*/
+
+
+	if(value < 0) value = -value;
+
+	while(value > limit) {
+
+		#if trip_debugger
+		if(shift > 32) {
+			testa = testb / testc;
+		}
+		#endif
+
+		shift++;
+
+		value >>= 1;
+
+	}
+
+	return shift;
+
+}
+
+
+/*
+
+ Return the largest value of an int array
+
+*/
+
+int MaxInt(int *iarray, int iarraysize)
+
+{
+
+	int imax = smallint;
+	int i;
+
+	for(i = iarraysize; i!=0; i--) {
+
+		if(imax < *iarray) imax = *iarray;
+
+		iarray++;
+
+	}
+
+	return imax;
+
+}
+
+
+/*
+
+ Return the smallest value of an int array
+
+*/
+
+int MinInt(int *iarray, int iarraysize)
+
+{
+
+	int imin = bigint;
+	int i;
+
+	for(i = iarraysize; i!=0; i--) {
+
+		if(imin > *iarray) imin = *iarray;
+
+		iarray++;
+
+	}
+
+	return imin;
+
+}
+
+
+
+/*
+
+ Create Matrix from Euler Angles
+
+ It requires a pointer to some euler angles and a pointer to a matrix
+
+ Construct the matrix elements using the following formula
+
+ Formula for ZXY Matrix
+
+ m11 = cy*cz + sx*sy*sz    m12 = -cy*sz + sx*sy*cz   m13 = cx*sy
+ m21 = cx*sz               m22 = cx*cz               m23 = -sx
+ m31 = -sy*cz + sx*cy*sz   m32 = sy*sz + sx*cy*cz    m33 = cx*cy
+
+*/
+
+void CreateEulerMatrix(e, m1)
+
+	EULER *e;
+	MATRIXCH *m1;
+
+{
+
+#if 0
+
+	SVECTOR eulers;
+
+	eulers.vx=(e->EulerX)&4095;
+	eulers.vy=(e->EulerY)&4095;
+	eulers.vz=(e->EulerZ)&4095;
+
+	RotMatrix(&eulers,(MATRIX *)scratchp);
+
+	psx2ch((MATRIX *)scratchp,m1);
+
+#else
+
+	int t, sx, sy, sz, cx, cy, cz;
+
+
+	sx = GetSin(e->EulerX);
+	sy = GetSin(e->EulerY);
+	sz = GetSin(e->EulerZ);
+
+	cx = GetCos(e->EulerX);
+	cy = GetCos(e->EulerY);
+	cz = GetCos(e->EulerZ);
+
+
+	#if 0
+	textprint("Euler Matrix Sines & Cosines\n");
+	textprint("%d, %d, %d\n", sx, sy, sz);
+	textprint("%d, %d, %d\n", cx, cy, cz);
+	#endif
+
+
+/* m11 = cy*cz + sx*sy*sz */
+
+	m1->mat11 = MUL_FIXED(cy, cz);		/* cy*cz	*/
+	t = MUL_FIXED(sx, sy);					/* sx*sy */
+	t = MUL_FIXED(t, sz);					/* *sz	*/
+	m1->mat11 += t;
+
+
+/* m12 = -cy*sz + sx*sy*cz */
+
+	m1->mat12=MUL_FIXED(-cy,sz);
+	t=MUL_FIXED(sx,sy);
+	t=MUL_FIXED(t,cz);
+	m1->mat12+=t;
+
+
+/* m13 = cx*sy */
+
+	m1->mat13=MUL_FIXED(cx,sy);
+
+
+/* m21 = cx*sz */
+
+	m1->mat21=MUL_FIXED(cx,sz);
+
+
+/* m22 = cx*cz */
+
+	m1->mat22=MUL_FIXED(cx,cz);
+
+
+/* m23 = -sx */
+
+	m1->mat23=-sx;
+
+
+/* m31 = -sy*cz + sx*cy*sz */
+
+	m1->mat31=MUL_FIXED(-sy,cz);
+	t=MUL_FIXED(sx,cy);
+	t=MUL_FIXED(t,sz);
+	m1->mat31+=t;
+
+
+/* m32 = sy*sz + sx*cy*cz */
+
+	m1->mat32=MUL_FIXED(sy,sz);
+	t=MUL_FIXED(sx,cy);
+	t=MUL_FIXED(t,cz);
+	m1->mat32+=t;
+
+
+/* m33 = cx*cy */
+
+	m1->mat33=MUL_FIXED(cx,cy);
+
+#endif
+
+}
+
+
+/*
+
+ Create a Unit Vector from three Euler Angles
+
+*/
+
+void CreateEulerVector(EULER *e, VECTORCH *v)
+
+{
+
+	int t, sx, sy, sz, cx, cy, cz;
+
+
+	sx = GetSin(e->EulerX);
+	sy = GetSin(e->EulerY);
+	sz = GetSin(e->EulerZ);
+
+	cx = GetCos(e->EulerX);
+	cy = GetCos(e->EulerY);
+	cz = GetCos(e->EulerZ);
+
+
+	/* x = -sy*cz + sx*cy*sz */
+
+	v->vx  = MUL_FIXED(-sy, cz);
+	t      = MUL_FIXED(sx, cy);
+	t      = MUL_FIXED(t, sz);
+	v->vx += t;
+
+
+	/* y = sy*sz + sx*cy*cz */
+
+	v->vy  = MUL_FIXED(sy, sz);
+	t      = MUL_FIXED(sx, cy);
+	t      = MUL_FIXED(t, cz);
+	v->vy += t;
+
+
+	/* z = cx*cy */
+
+	v->vz = MUL_FIXED(cx,cy);
+
+}
+
+
+
+/*
+
+ Matrix Multiply Function
+
+ A 3x3 Matrix is represented here as
+
+ m11 m12 m13
+ m21 m22 m23
+ m31 m32 m33
+
+ Row #1 (r1) of the matrix is m11 m12 m13
+ Column #1 (c1) of the matrix is m11 m32 m31
+
+ Under multiplication
+
+ m'' = m x m'
+
+ where
+
+ m11'' = c1.r1'
+ m12'' = c2.r1'
+ m13'' = c3.r1'
+
+ m21'' = c1.r2'
+ m22'' = c2.r2'
+ m23'' = c3.r2'
+
+ m31'' = c1.r3'
+ m32'' = c2.r3'
+ m33'' = c3.r3'
+
+*/
+
+void MatrixMultiply(m1, m2, m3)
+
+	struct matrixch *m1, *m2, *m3;
+
+{
+  
+	#if 0
+ 		
+	PushMatrix();
+
+	ch2psx(m1,(MATRIX *)scratchp);
+	ch2psx(m2,(MATRIX *)(scratchp+(sizeof(MATRIX))));
+
+	MulMatrix0((MATRIX *)scratchp,(MATRIX *)(scratchp+(sizeof(MATRIX))),(MATRIX *)(scratchp+((sizeof(MATRIX)<<1))));
+
+	psx2ch((MATRIX *)(scratchp+((sizeof(MATRIX)<<1))),m3);
+
+ 	PopMatrix();
+		
+	#else
+		 
+	MATRIXCH TmpMat;
+	 
+/* m11'' = c1.r1' */
+
+	TmpMat.mat11=MUL_FIXED(m1->mat11,m2->mat11);
+	TmpMat.mat11+=MUL_FIXED(m1->mat21,m2->mat12);
+	TmpMat.mat11+=MUL_FIXED(m1->mat31,m2->mat13);
+
+/* m12'' = c2.r1' */
+
+	TmpMat.mat12=MUL_FIXED(m1->mat12,m2->mat11);
+	TmpMat.mat12+=MUL_FIXED(m1->mat22,m2->mat12);
+	TmpMat.mat12+=MUL_FIXED(m1->mat32,m2->mat13);
+
+/* m13'' = c3.r1' */
+
+	TmpMat.mat13=MUL_FIXED(m1->mat13,m2->mat11);
+	TmpMat.mat13+=MUL_FIXED(m1->mat23,m2->mat12);
+	TmpMat.mat13+=MUL_FIXED(m1->mat33,m2->mat13);
+
+/* m21'' = c1.r2' */
+
+	TmpMat.mat21=MUL_FIXED(m1->mat11,m2->mat21);
+	TmpMat.mat21+=MUL_FIXED(m1->mat21,m2->mat22);
+	TmpMat.mat21+=MUL_FIXED(m1->mat31,m2->mat23);
+
+/* m22'' = c2.r2' */
+
+	TmpMat.mat22=MUL_FIXED(m1->mat12,m2->mat21);
+	TmpMat.mat22+=MUL_FIXED(m1->mat22,m2->mat22);
+	TmpMat.mat22+=MUL_FIXED(m1->mat32,m2->mat23);
+
+/* m23'' = c3.r2' */
+
+	TmpMat.mat23=MUL_FIXED(m1->mat13,m2->mat21);
+	TmpMat.mat23+=MUL_FIXED(m1->mat23,m2->mat22);
+	TmpMat.mat23+=MUL_FIXED(m1->mat33,m2->mat23);
+
+/* m31'' = c1.r3' */
+
+	TmpMat.mat31=MUL_FIXED(m1->mat11,m2->mat31);
+	TmpMat.mat31+=MUL_FIXED(m1->mat21,m2->mat32);
+	TmpMat.mat31+=MUL_FIXED(m1->mat31,m2->mat33);
+
+/* m32'' = c2.r3' */
+
+	TmpMat.mat32=MUL_FIXED(m1->mat12,m2->mat31);
+	TmpMat.mat32+=MUL_FIXED(m1->mat22,m2->mat32);
+	TmpMat.mat32+=MUL_FIXED(m1->mat32,m2->mat33);
+
+/* m33'' = c3.r3' */
+
+	TmpMat.mat33=MUL_FIXED(m1->mat13,m2->mat31);
+	TmpMat.mat33+=MUL_FIXED(m1->mat23,m2->mat32);
+	TmpMat.mat33+=MUL_FIXED(m1->mat33,m2->mat33);
+
+/* Finally, copy TmpMat to m3 */
+
+	CopyMatrix(&TmpMat, m3);
+
+	#endif
+
+}
+
+
+void PSXAccurateMatrixMultiply(m1, m2, m3)
+
+	struct matrixch *m1, *m2, *m3;
+
+{
+ 	 
+	MATRIXCH TmpMat;
+	 
+/* m11'' = c1.r1' */
+
+	TmpMat.mat11=MUL_FIXED(m1->mat11,m2->mat11);
+	TmpMat.mat11+=MUL_FIXED(m1->mat21,m2->mat12);
+	TmpMat.mat11+=MUL_FIXED(m1->mat31,m2->mat13);
+
+/* m12'' = c2.r1' */
+
+	TmpMat.mat12=MUL_FIXED(m1->mat12,m2->mat11);
+	TmpMat.mat12+=MUL_FIXED(m1->mat22,m2->mat12);
+	TmpMat.mat12+=MUL_FIXED(m1->mat32,m2->mat13);
+
+/* m13'' = c3.r1' */
+
+	TmpMat.mat13=MUL_FIXED(m1->mat13,m2->mat11);
+	TmpMat.mat13+=MUL_FIXED(m1->mat23,m2->mat12);
+	TmpMat.mat13+=MUL_FIXED(m1->mat33,m2->mat13);
+
+/* m21'' = c1.r2' */
+
+	TmpMat.mat21=MUL_FIXED(m1->mat11,m2->mat21);
+	TmpMat.mat21+=MUL_FIXED(m1->mat21,m2->mat22);
+	TmpMat.mat21+=MUL_FIXED(m1->mat31,m2->mat23);
+
+/* m22'' = c2.r2' */
+
+	TmpMat.mat22=MUL_FIXED(m1->mat12,m2->mat21);
+	TmpMat.mat22+=MUL_FIXED(m1->mat22,m2->mat22);
+	TmpMat.mat22+=MUL_FIXED(m1->mat32,m2->mat23);
+
+/* m23'' = c3.r2' */
+
+	TmpMat.mat23=MUL_FIXED(m1->mat13,m2->mat21);
+	TmpMat.mat23+=MUL_FIXED(m1->mat23,m2->mat22);
+	TmpMat.mat23+=MUL_FIXED(m1->mat33,m2->mat23);
+
+/* m31'' = c1.r3' */
+
+	TmpMat.mat31=MUL_FIXED(m1->mat11,m2->mat31);
+	TmpMat.mat31+=MUL_FIXED(m1->mat21,m2->mat32);
+	TmpMat.mat31+=MUL_FIXED(m1->mat31,m2->mat33);
+
+/* m32'' = c2.r3' */
+
+	TmpMat.mat32=MUL_FIXED(m1->mat12,m2->mat31);
+	TmpMat.mat32+=MUL_FIXED(m1->mat22,m2->mat32);
+	TmpMat.mat32+=MUL_FIXED(m1->mat32,m2->mat33);
+
+/* m33'' = c3.r3' */
+
+	TmpMat.mat33=MUL_FIXED(m1->mat13,m2->mat31);
+	TmpMat.mat33+=MUL_FIXED(m1->mat23,m2->mat32);
+	TmpMat.mat33+=MUL_FIXED(m1->mat33,m2->mat33);
+
+/* Finally, copy TmpMat to m3 */
+
+	CopyMatrix(&TmpMat, m3);
+
+}
+
+
+
+
+
+/*
+
+ Transpose Matrix
+
+*/
+
+void TransposeMatrixCH(m1)
+
+	MATRIXCH *m1;
+
+{
+
+	int t;
+
+	t=m1->mat12;
+	m1->mat12=m1->mat21;
+	m1->mat21=t;
+
+	t=m1->mat13;
+	m1->mat13=m1->mat31;
+	m1->mat31=t;
+
+	t=m1->mat23;
+	m1->mat23=m1->mat32;
+	m1->mat32=t;
+
+}
+
+
+/*
+
+ Copy Vector
+
+*/
+
+void CopyVector(VECTORCH *v1, VECTORCH *v2)
+
+{
+
+/* Copy VECTORCH v1 -> VECTORCH v2 */
+
+	v2->vx=v1->vx;
+	v2->vy=v1->vy;
+	v2->vz=v1->vz;
+
+}
+
+
+/*
+
+ Copy Location
+
+*/
+
+void CopyLocation(VECTORCH *v1, VECTORCH *v2)
+
+{
+
+/* Copy VECTORCH v1 -> VECTORCH v2 */
+
+	v2->vx=v1->vx;
+	v2->vy=v1->vy;
+	v2->vz=v1->vz;
+
+}
+
+
+
+
+
+/*
+
+ Copy Euler
+
+*/
+
+void CopyEuler(EULER *e1, EULER *e2)
+
+{
+
+/* Copy EULER e1 -> EULER e2 */
+
+	e2->EulerX=e1->EulerX;
+	e2->EulerY=e1->EulerY;
+	e2->EulerZ=e1->EulerZ;
+
+}
+
+
+/*
+
+ Copy Matrix
+
+*/
+
+void CopyMatrix(MATRIXCH *m1, MATRIXCH *m2)
+
+{
+
+/* Copy MATRIXCH m1 -> MATRIXCH m2 */
+
+	m2->mat11=m1->mat11;
+	m2->mat12=m1->mat12;
+	m2->mat13=m1->mat13;
+
+	m2->mat21=m1->mat21;
+	m2->mat22=m1->mat22;
+	m2->mat23=m1->mat23;
+
+	m2->mat31=m1->mat31;
+	m2->mat32=m1->mat32;
+	m2->mat33=m1->mat33;
+
+}
+
+
+/*
+
+ Make a Vector.
+
+ v3 = v1 - v2
+
+*/
+
+void MakeVector(VECTORCH *v1, VECTORCH *v2, VECTORCH *v3)
+
+{
+
+	v3->vx = v1->vx - v2->vx;
+	v3->vy = v1->vy - v2->vy;
+	v3->vz = v1->vz - v2->vz;
+
+}
+
+
+/*
+
+ Add a Vector.
+
+ v2 = v2 + v1
+
+*/
+
+void AddVector(VECTORCH *v1, VECTORCH *v2)
+
+{
+
+	v2->vx += v1->vx;
+	v2->vy += v1->vy;
+	v2->vz += v1->vz;
+
+}
+
+
+/*
+
+ Subtract a Vector.
+
+ v2 = v2 - v1
+
+*/
+
+void SubVector(VECTORCH *v1, VECTORCH *v2)
+
+{
+
+	v2->vx -= v1->vx;
+	v2->vy -= v1->vy;
+	v2->vz -= v1->vz;
+
+}
+
+
+
+/*
+
+ Matrix Rotatation of a Vector
+
+ Overwrite the Source Vector with the Rotated Vector
+
+ x' = v.c1
+ y' = v.c2
+ z' = v.c3
+
+*/
+
+void _RotateVector(VECTORCH *v, MATRIXCH*  m)
+{
+
+	int x, y, z;
+	
+
+	x =  MUL_FIXED(m->mat11, v->vx);
+	x += MUL_FIXED(m->mat21, v->vy);
+	x += MUL_FIXED(m->mat31, v->vz);
+
+	y  = MUL_FIXED(m->mat12, v->vx);
+	y += MUL_FIXED(m->mat22, v->vy);
+	y += MUL_FIXED(m->mat32, v->vz);
+
+	z  = MUL_FIXED(m->mat13, v->vx);
+	z += MUL_FIXED(m->mat23, v->vy);
+	z += MUL_FIXED(m->mat33, v->vz);
+
+	v->vx = x;
+	v->vy = y;
+	v->vz = z;
+}
+
+
+/*
+
+ Matrix Rotation of a Source Vector using a Matrix
+ Copying to a Destination Vector
+
+ x' = v.c1
+ y' = v.c2
+ z' = v.c3
+
+*/
+
+void _RotateAndCopyVector(v1, v2, m)
+
+	VECTORCH *v1;
+	VECTORCH *v2;
+	MATRIXCH *m;
+
+{
+
+	v2->vx=MUL_FIXED(m->mat11,v1->vx);
+	v2->vx+=MUL_FIXED(m->mat21,v1->vy);
+	v2->vx+=MUL_FIXED(m->mat31,v1->vz);
+
+	v2->vy=MUL_FIXED(m->mat12,v1->vx);
+	v2->vy+=MUL_FIXED(m->mat22,v1->vy);
+	v2->vy+=MUL_FIXED(m->mat32,v1->vz);
+
+	v2->vz=MUL_FIXED(m->mat13,v1->vx);
+	v2->vz+=MUL_FIXED(m->mat23,v1->vy);
+	v2->vz+=MUL_FIXED(m->mat33,v1->vz);
+
+}
+
+
+
+
+
+
+/*
+
+ Matrix to Euler Angles
+
+ Maths overflow is a real problem for this function. To prevent overflows
+ the matrix Sines and Cosines are calculated using values scaled down by 4.
+
+
+ sinx	=	-M23
+
+ cosx	=	sqr ( 1 - sinx^2 )
+
+
+ siny	=	M13 / cosx
+
+ cosy	=	M33 / cosx
+
+
+ sinz	=	M21 / cosx
+
+ cosz	=	M22 / cosx
+
+
+*/
+
+
+#define m2e_scale 2
+#define ONE_FIXED_S ((ONE_FIXED >> m2e_scale) - 1)
+#define m2e_shift 14
+
+#define j_and_r_change Yes
+
+
+void MatrixToEuler(MATRIXCH *m, EULER *e)
+
+{
+
+	int x, sinx, cosx, siny, cosy, sinz, cosz;
+	int abs_cosx, abs_cosy, abs_cosz;
+	int SineMatrixPitch, SineMatrixYaw, SineMatrixRoll;
+	int CosMatrixPitch, CosMatrixYaw, CosMatrixRoll;
+
+
+
+
+	#if 0
+	textprint("CosMatrixPitch = %d\n", CosMatrixPitch);
+	/* WaitForReturn(); */
+	#endif
+
+
+	if(m->mat32 >-65500 && m->mat32<65500)
+	{
+			/* Yaw */
+
+		/* Pitch */
+
+		#if j_and_r_change
+		SineMatrixPitch = -m->mat32;
+		#else
+		SineMatrixPitch = -m->mat23;
+		#endif
+
+		SineMatrixPitch >>= m2e_scale;
+
+		#if 0
+		textprint("SineMatrixPitch = %d\n", SineMatrixPitch);
+		/* WaitForReturn(); */
+		#endif
+
+		CosMatrixPitch = SineMatrixPitch * SineMatrixPitch;
+		CosMatrixPitch >>= m2e_shift;
+
+		CosMatrixPitch = -CosMatrixPitch;
+		CosMatrixPitch += ONE_FIXED_S;
+		CosMatrixPitch *= ONE_FIXED_S;
+		CosMatrixPitch = SqRoot32(CosMatrixPitch);
+
+		if(CosMatrixPitch) {
+
+			if(CosMatrixPitch > ONE_FIXED_S) CosMatrixPitch = ONE_FIXED_S;
+			else if(CosMatrixPitch < -ONE_FIXED_S) CosMatrixPitch = -ONE_FIXED_S;
+
+		}
+
+		else CosMatrixPitch = 1;
+	
+		SineMatrixYaw = WideMulNarrowDiv(
+			#if j_and_r_change
+			m->mat31 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);
+			#else
+			m->mat13 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);
+			#endif
+
+		#if 0
+		textprint("SineMatrixYaw = %d\n", SineMatrixYaw);
+		/* WaitForReturn(); */
+		#endif
+
+		CosMatrixYaw = WideMulNarrowDiv(
+			m->mat33 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);
+
+		#if 0
+		textprint("CosMatrixYaw = %d\n", CosMatrixYaw);
+		/* WaitForReturn(); */
+		#endif
+
+
+		/* Roll */
+
+		SineMatrixRoll = WideMulNarrowDiv(
+			#if j_and_r_change
+			m->mat12 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);
+			#else
+			m->mat21 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);
+			#endif
+
+		#if 0
+		textprint("SineMatrixRoll = %d\n", SineMatrixRoll);
+		/* WaitForReturn(); */
+		#endif
+
+		CosMatrixRoll = WideMulNarrowDiv(
+			m->mat22 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);
+
+		#if 0
+		textprint("CosMatrixRoll = %d\n", CosMatrixRoll);
+		/* WaitForReturn(); */
+		#endif
+	
+		/* Tables are for values +- 2^16 */
+
+		sinx = SineMatrixPitch << m2e_scale;
+		siny = SineMatrixYaw   << m2e_scale;
+		sinz = SineMatrixRoll  << m2e_scale;
+
+		cosx = CosMatrixPitch << m2e_scale;
+		cosy = CosMatrixYaw   << m2e_scale;
+		cosz = CosMatrixRoll  << m2e_scale;
+
+		#if 0
+		textprint("sines = %d, %d, %d\n", sinx, siny, sinz);
+		textprint("cos's = %d, %d, %d\n", cosx, cosy, cosz);
+		/* WaitForReturn(); */
+		#endif
+
+		/* Absolute Cosines */
+
+		abs_cosx = cosx;
+		if(abs_cosx < 0) abs_cosx = -abs_cosx;
+
+		abs_cosy = cosy;
+		if(abs_cosy < 0) abs_cosy = -abs_cosy;
+
+		abs_cosz = cosz;
+		if(abs_cosz < 0) abs_cosz = -abs_cosz;
+
+
+		/* Euler X */
+
+		if(abs_cosx > Cosine45) {
+
+			x = ArcSin(sinx);
+
+			if(cosx < 0) {
+				x =  -x;
+				x += deg180;
+				x &= wrap360;
+			}
+		}
+
+		else {
+
+			x = ArcCos(cosx);
+
+			if(sinx < 0) {
+				x =  -x;
+				x &= wrap360;			
+			}
+		}
+
+		#if (j_and_r_change == No)
+		x = -x;
+		x &= wrap360;
+		#endif
+
+		e->EulerX = x;
+
+
+		/* Euler Y */
+
+		if(abs_cosy > Cosine45) {
+
+			x = ArcSin(siny);
+
+			if(cosy < 0) {
+				x =  -x;
+				x += deg180;
+				x &= wrap360;
+			}
+
+		}
+
+		else {
+
+			x = ArcCos(cosy);
+
+			if(siny < 0) {
+				x =  -x;
+				x &= wrap360;			
+			}
+
+		}
+
+		#if (j_and_r_change == No)
+		x = -x;
+		x &= wrap360;
+		#endif
+
+		e->EulerY = x;
+
+
+		/* Euler Z */
+
+		if(abs_cosz > Cosine45) {
+
+			x = ArcSin(sinz);
+
+			if(cosz < 0) {
+				x =  -x;
+				x += deg180;
+				x &= wrap360;
+			}
+		}
+
+		else {
+
+			x = ArcCos(cosz);
+
+			if(sinz < 0) {
+				x =  -x;
+				x &= wrap360;			
+			}
+		}
+
+		#if (j_and_r_change == No)
+		x =  -x;
+		x &= wrap360;
+		#endif
+
+		e->EulerZ = x;
+	}
+	else //singularity case
+	{
+
+		if(m->mat32>0)
+			e->EulerX = 3072;
+		else
+			e->EulerX = 1024;
+		
+		e->EulerZ=0;
+
+
+		
+		/* Yaw */
+		
+		siny = -m->mat13 ;
+
+		cosy = 	m->mat11 ;
+
+		abs_cosy = cosy;
+		if(abs_cosy < 0) abs_cosy = -abs_cosy;
+
+
+		if(abs_cosy > Cosine45) {
+
+			x = ArcSin(siny);
+
+			if(cosy < 0) {
+				x =  -x;
+				x += deg180;
+				x &= wrap360;
+			}
+
+		}
+
+		else {
+
+			x = ArcCos(cosy);
+
+			if(siny < 0) {
+				x =  -x;
+				x &= wrap360;			
+			}
+
+		}
+
+		#if (j_and_r_change == No)
+		x = -x;
+		x &= wrap360;
+		#endif
+
+		e->EulerY = x;
+
+	}
+
+
+
+
+	#if 0
+	textprint("\nEuler from VDB Matrix is:\n%d\n%d\n%d\n",
+	e->EulerX,
+	e->EulerY,
+	e->EulerZ
+	);
+	/* WaitForReturn(); */
+	#endif
+
+}
+
+
+
+#if 1
+
+
+
+
+#define j_and_r_change_2 Yes
+
+void MatrixToEuler2(MATRIXCH *m, EULER *e)
+
+{
+
+	int x, sinx, cosx, siny, cosy, sinz, cosz;
+	int abs_cosx, abs_cosy, abs_cosz;
+	int SineMatrixPitch, SineMatrixYaw, SineMatrixRoll;
+	int CosMatrixPitch, CosMatrixYaw, CosMatrixRoll;
+
+
+	/* Pitch */
+
+	#if j_and_r_change_2
+	SineMatrixPitch = -m->mat32;
+	#else
+	SineMatrixPitch = -m->mat23;
+	#endif
+
+	SineMatrixPitch >>= m2e_scale;
+
+	#if 0
+	textprint("SineMatrixPitch = %d\n", SineMatrixPitch);
+	/* WaitForReturn(); */
+	#endif
+
+	CosMatrixPitch = SineMatrixPitch * SineMatrixPitch;
+	CosMatrixPitch >>= m2e_shift;
+
+	CosMatrixPitch = -CosMatrixPitch;
+	CosMatrixPitch += ONE_FIXED_S;
+	CosMatrixPitch *= ONE_FIXED_S;
+	CosMatrixPitch = SqRoot32(CosMatrixPitch);
+
+	if(CosMatrixPitch) {
+
+		if(CosMatrixPitch > ONE_FIXED_S) CosMatrixPitch = ONE_FIXED_S;
+		else if(CosMatrixPitch < -ONE_FIXED_S) CosMatrixPitch = -ONE_FIXED_S;
+
+	}
+
+	else CosMatrixPitch = 1;
+
+
+	#if 0
+	textprint("CosMatrixPitch = %d\n", CosMatrixPitch);
+	/* WaitForReturn(); */
+	#endif
+
+
+	/* Yaw */
+
+	SineMatrixYaw = WideMulNarrowDiv(
+		#if j_and_r_change_2
+		m->mat31 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);
+		#else
+		m->mat13 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);
+		#endif
+
+	#if 0
+	textprint("SineMatrixYaw = %d\n", SineMatrixYaw);
+	/* WaitForReturn(); */
+	#endif
+
+	CosMatrixYaw = WideMulNarrowDiv(
+		m->mat33 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);
+
+	#if 0
+	textprint("CosMatrixYaw = %d\n", CosMatrixYaw);
+	/* WaitForReturn(); */
+	#endif
+
+
+	/* Roll */
+
+	SineMatrixRoll = WideMulNarrowDiv(
+		#if j_and_r_change_2
+		m->mat12 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);
+		#else
+		m->mat21 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);
+		#endif
+
+	#if 0
+	textprint("SineMatrixRoll = %d\n", SineMatrixRoll);
+	/* WaitForReturn(); */
+	#endif
+
+	CosMatrixRoll = WideMulNarrowDiv(
+		m->mat22 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);
+
+	#if 0
+	textprint("CosMatrixRoll = %d\n", CosMatrixRoll);
+	/* WaitForReturn(); */
+	#endif
+
+
+	/* Tables are for values +- 2^16 */
+
+	sinx = SineMatrixPitch << m2e_scale;
+	siny = SineMatrixYaw   << m2e_scale;
+	sinz = SineMatrixRoll  << m2e_scale;
+
+	cosx = CosMatrixPitch << m2e_scale;
+	cosy = CosMatrixYaw   << m2e_scale;
+	cosz = CosMatrixRoll  << m2e_scale;
+
+	#if 0
+	textprint("sines = %d, %d, %d\n", sinx, siny, sinz);
+	textprint("cos's = %d, %d, %d\n", cosx, cosy, cosz);
+	/* WaitForReturn(); */
+	#endif
+
+	/* Absolute Cosines */
+
+	abs_cosx = cosx;
+	if(abs_cosx < 0) abs_cosx = -abs_cosx;
+
+	abs_cosy = cosy;
+	if(abs_cosy < 0) abs_cosy = -abs_cosy;
+
+	abs_cosz = cosz;
+	if(abs_cosz < 0) abs_cosz = -abs_cosz;
+
+
+	/* Euler X */
+
+	if(abs_cosx > Cosine45) {
+
+		x = ArcSin(sinx);
+
+		if(cosx < 0) {
+			x =  -x;
+			x += deg180;
+			x &= wrap360;
+		}
+	}
+
+	else {
+
+		x = ArcCos(cosx);
+
+		if(sinx < 0) {
+			x =  -x;
+			x &= wrap360;			
+		}
+	}
+
+	#if (j_and_r_change_2 == No)
+	x = -x;
+	x &= wrap360;
+	#endif
+
+	e->EulerX = x;
+
+
+	/* Euler Y */
+
+	if(abs_cosy > Cosine45) {
+
+		x = ArcSin(siny);
+
+		if(cosy < 0) {
+			x =  -x;
+			x += deg180;
+			x &= wrap360;
+		}
+
+	}
+
+	else {
+
+		x = ArcCos(cosy);
+
+		if(siny < 0) {
+			x =  -x;
+			x &= wrap360;			
+		}
+
+	}
+
+	#if (j_and_r_change_2 == No)
+	x = -x;
+	x &= wrap360;
+	#endif
+
+	e->EulerY = x;
+
+
+	/* Euler Z */
+
+	if(abs_cosz > Cosine45) {
+
+		x = ArcSin(sinz);
+
+		if(cosz < 0) {
+			x =  -x;
+			x += deg180;
+			x &= wrap360;
+		}
+	}
+
+	else {
+
+		x = ArcCos(cosz);
+
+		if(sinz < 0) {
+			x =  -x;
+			x &= wrap360;			
+		}
+	}
+
+	#if (j_and_r_change_2 == No)
+	x =  -x;
+	x &= wrap360;
+	#endif
+
+	e->EulerZ = x;
+
+
+	#if 0
+	textprint("\nEuler from VDB Matrix is:\n%d\n%d\n%d\n",
+	e->EulerX,
+	e->EulerY,
+	e->EulerZ
+	);
+	/* WaitForReturn(); */
+	#endif
+
+}
+
+
+
+#endif
+
+
+
+/*
+
+ Normalise a Matrix
+
+ Dot the three vectors together (XY, XZ, YZ) and take the two nearest to
+ 90ø from each other. Cross them to create a new third vector, then cross
+ the first and third to create a new second.
+
+*/
+
+void MNormalise(MATRIXCH *m)
+
+{
+
+	VECTORCH *x = (VECTORCH *) &m->mat11;
+	VECTORCH *y = (VECTORCH *) &m->mat21;
+	VECTORCH *z = (VECTORCH *) &m->mat31;
+	int dotxy = Dot(x, y);
+	int dotxz = Dot(x, z);
+	int dotyz = Dot(y, z);
+	VECTORCH *s;
+	VECTORCH *t;
+	VECTORCH u;
+	VECTORCH v;
+	VECTORCH zero = {0, 0, 0};
+
+
+	#if 0
+	textprint("dotxy = %d\n", dotxy);
+	textprint("dotxz = %d\n", dotxz);
+	textprint("dotyz = %d\n", dotyz);
+	#endif
+
+	#if 0
+	/* TEST */
+	dotxy = 0;
+	dotxz = 0;
+	dotyz = 1;
+	#endif
+
+
+	#if 0
+	textprint("%d	%d	%d\n",
+		x->vx,
+		x->vy,
+		x->vz
+	);
+
+	textprint("%d	%d	%d\n",
+		y->vx,
+		y->vy,
+		y->vz
+	);
+
+	textprint("%d	%d	%d\n",
+		z->vx,
+		z->vy,
+		z->vz
+	);
+	#endif
+
+
+	/* Find the two vectors nearest 90ø */
+
+	if(dotxy > dotxz && dotxy > dotyz) {
+
+		/* xy are the closest to 90ø */
+
+		/*textprint("xy\n");*/
+
+		s = x;
+		t = y;
+
+		MakeNormal(&zero, s, t, &u);		/* Cross them for a new 3rd vector */
+
+		MakeNormal(&zero, s, &u, &v);		/* Cross 1st & 3rd for a new 2nd */
+		v.vx = -v.vx;
+		v.vy = -v.vy;
+		v.vz = -v.vz;
+
+		CopyVector(&u, z);
+		CopyVector(&v, y);
+
+	}
+
+	else if(dotxz > dotxy && dotxz > dotyz) {
+
+		/* xz are the closest to 90ø */
+
+		/*textprint("xz\n");*/
+
+		s = x;
+		t = z;
+
+		MakeNormal(&zero, s, t, &u);		/* Cross them for a new 3rd vector */
+		u.vx = -u.vx;
+		u.vy = -u.vy;
+		u.vz = -u.vz;
+
+		MakeNormal(&zero, s, &u, &v);		/* Cross 1st & 3rd for a new 2nd */
+
+		CopyVector(&u, y);
+		CopyVector(&v, z);
+
+	}
+
+	else {
+
+		/* yz are the closest to 90ø */
+
+		/*textprint("yz\n");*/
+
+		s = y;
+		t = z;
+
+		MakeNormal(&zero, s, t, &u);		/* Cross them for a new 3rd vector */
+
+		MakeNormal(&zero, s, &u, &v);		/* Cross 1st & 3rd for a new 2nd */
+		v.vx = -v.vx;
+		v.vy = -v.vy;
+		v.vz = -v.vz;
+
+		CopyVector(&u, x);
+		CopyVector(&v, z);
+
+	}
+
+
+	#if 0
+	textprint("%d	%d	%d\n",
+		x->vx,
+		x->vy,
+		x->vz
+	);
+
+	textprint("%d	%d	%d\n",
+		y->vx,
+		y->vy,
+		y->vz
+	);
+
+	textprint("%d	%d	%d\n",
+		z->vx,
+		z->vy,
+		z->vz
+	);
+	#endif
+
+	#if 0
+	textprint("mag. x = %d\n", Magnitude(x));
+	textprint("mag. y = %d\n", Magnitude(y));
+	textprint("mag. z = %d\n", Magnitude(z));
+	#endif
+
+	/*WaitForReturn();*/
+
+
+}
+
+
+
+
+/*
+
+ ArcCos
+
+ In:  COS value as -65,536 -> +65,536.
+ Out: Angle in 0 -> 4095 form.
+
+ Notes:
+
+ The angle returned is in the range 0 -> 2,047 since the sign of SIN
+ is not known.
+
+ ArcSin(x) = ArcTan ( x, sqr ( 1-x*x ) )
+ ArcCos(x) = ArcTan ( sqr ( 1-x*x ), x)
+
+ -65,536 = 180 Degrees
+ 0	  = 90 Degrees
+ +65,536 = 0 Degrees
+
+ The table has 4,096 entries.
+
+*/
+
+int ArcCos(int c)
+
+{
+
+	short acos;
+
+	if(c < (-(ONE_FIXED - 1))) c = -(ONE_FIXED - 1);
+	else if(c > (ONE_FIXED - 1)) c = ONE_FIXED - 1;
+
+	#if 0
+	c =  c >> 5;		/* -64k -> +64k becomes -2k -> +2k */
+	c += 2048;			/* -2k -> +2k becomes 0 -> 4k */
+	#endif
+
+	acos = ArcCosTable[(c >> 5) + 2048];
+
+	return (int) (acos & wrap360);
+
+}
+
+
+/*
+
+ ArcSin
+
+ In:  SIN value in ax as -65,536 -> +65,536.
+ Out: Angle in 0 -> 4095 form in ax.
+
+ Notes:
+
+ The angle returned is in the range -1,024 -> 1,023 since the sign of COS
+ is not known.
+
+ ArcSin(x) = ArcTan ( x, sqr ( 1-x*x ) )
+ ArcCos(x) = ArcTan ( sqr ( 1-x*x ), x)
+
+ -65,536 = 270 Degrees
+ 0	  = 0 Degrees
+ +65,536 = 90 Degrees
+
+ The table has 4,096 entries.
+
+*/
+
+int ArcSin(int s)
+
+{
+
+	short asin;
+
+
+	if(s < (-(ONE_FIXED - 1))) s = -(ONE_FIXED - 1);
+	else if(s > (ONE_FIXED - 1)) s = ONE_FIXED - 1;
+
+	#if 0
+	s =  s >> 5;		/* -64k -> +64k becomes -2k -> +2k */
+	s += 2048;			/* -2k -> +2k becomes 0 -> 4k */
+	#endif
+
+	asin = ArcSineTable[(s >> 5) + 2048];
+
+	return (int) (asin & wrap360);
+
+}
+
+
+/*
+
+ ArcTan
+
+ Pass (x,z)
+
+ And ATN(x/z) is returned such that:
+
+ 000ø is Map North
+ 090ø is Map East
+ 180ø is Map South
+ 270ø is Map West
+
+*/
+
+int ArcTan(height_x, width_z)
+
+	int height_x,width_z;
+
+{
+
+	int abs_height_x, abs_width_z, angle, sign, signsame, temp;
+
+	sign=0;
+
+	if((height_x<0 && width_z<0) || (height_x>=0 && width_z>=0))
+		signsame=Yes;
+	else
+		signsame=No;
+
+	abs_height_x=height_x;
+	if(abs_height_x<0) abs_height_x=-abs_height_x;
+
+	abs_width_z=width_z;
+	if(abs_width_z<0) abs_width_z=-abs_width_z;
+
+/*
+
+ Find ATN
+
+*/
+
+	if(width_z==0) angle=-deg90;
+
+	else if(abs_width_z==abs_height_x)
+		angle=deg45;
+
+	else {
+
+		if(abs_width_z>abs_height_x) {
+			temp=abs_width_z;
+			abs_width_z=abs_height_x;
+			abs_height_x=temp;
+			sign=-1;
+		}
+
+		if(abs_height_x!=0)
+
+			/* angle = (abs_width_z << 8) / abs_height_x; */
+
+
+
+			angle = DIV_INT((abs_width_z << 8), abs_height_x);
+
+
+
+
+
+		else
+			angle=deg22pt5;
+
+		angle=ArcTanTable[angle];
+
+		if(sign>=0) {
+			angle=-angle;
+			angle+=deg90;
+		}
+
+	}
+
+	if(signsame==No) angle=-angle;
+
+	if(width_z<=0) angle+=deg180;
+
+	angle&=wrap360;
+
+	return(angle);
+
+}
+
+
+/*
+
+ Matrix from Z-Vector
+
+*/
+
+void MatrixFromZVector(VECTORCH *v, MATRIXCH *m)
+
+{
+
+	VECTORCH XVector;
+	VECTORCH YVector;
+
+	VECTORCH zero = {0, 0, 0};
+
+
+	XVector.vx = v->vz;
+	XVector.vy = 0;
+	XVector.vz = -v->vx;
+
+	Normalise(&XVector);
+
+	MakeNormal(&zero, &XVector, v, &YVector);
+
+	m->mat11 = XVector.vx;
+	m->mat12 = XVector.vy;
+	m->mat13 = XVector.vz;
+
+	m->mat21 = -YVector.vx;
+	m->mat22 = -YVector.vy;
+	m->mat23 = -YVector.vz;
+
+	m->mat31 = v->vx;
+	m->mat32 = v->vy;
+	m->mat33 = v->vz;
+
+}
+
+
+
+
+
+
+
+
+
+
+/*
+
+ Distance Functions
+
+*/
+
+
+/*
+
+ Foley and Van Dam 2d distance function
+
+ WARNING! Returns distance x 3
+
+ Here is the F & VD distance function:
+
+ x + z + (max(x,z) * 2)
+ ----------------------
+          3
+
+*/
+
+int FandVD_Distance_2d(VECTOR2D *v0, VECTOR2D *v1)
+
+{
+
+	int max;
+	int d;
+
+
+	int dx = v1->vx - v0->vx;
+	int dy = v1->vy - v0->vy;
+
+	if(dx < 0) dx = -dx;
+	if(dy < 0) dy = -dy;
+
+	if(dx > dy) max = dx;
+	else max = dy;
+
+	d = (dx + dy + (max * 2));
+
+	return d;
+
+}
+
+
+/*
+
+ Foley and Van Dam 3d distance function
+
+ WARNING! Returns distance x 9
+
+ For a 3d version, calculate (f(f(x,y), y*3))/9
+
+*/
+
+int FandVD_Distance_3d(VECTORCH *v0, VECTORCH *v1)
+
+{
+
+	int dxy, max;
+
+	int dz = v1->vz - v0->vz;
+
+	if(dz < 0) dz = -dz;
+
+	dz *= 3;
+
+	dxy = FandVD_Distance_2d((VECTOR2D *) v0, (VECTOR2D *) v1);
+
+	if(dxy > dz) max = dxy;
+	else max = dz;
+
+	return (dxy + dz + (max * 2));
+
+}
+
+
+/*
+
+ NextLowPower2() returns the next lowest power of 2 of the passed value.
+
+ e.g. 18 is returned as 16.
+
+*/
+
+int NextLowPower2(int i)
+
+{
+
+	int n = 1;
+
+
+	while(n <= i)
+		n <<= 1;
+
+	return n >> 1;
+
+}
+
+
+/*
+
+ Transform a world location into the local space of the passed matrix and
+ location.
+
+ Vector v1 is transformed to v2
+ It is made relative to vector v3 and rotated using matrix m transposed
+
+ A possible use is the transformation of world points into the local space
+ of a display block
+
+ e.g.
+
+	MakeVectorLocal(&v1, &v2, &dptr->ObWorld, &dptr->ObMat);
+
+ This would place vector v2 into the local space of display block dptr
+
+*/
+
+void MakeVectorLocal(VECTORCH *v1, VECTORCH *v2, VECTORCH *v3, MATRIXCH *m)
+
+{
+
+	MATRIXCH transmat;
+
+
+	CopyMatrix(m, &transmat);
+	TransposeMatrixCH(&transmat);
+
+	v2->vx = v1->vx - v3->vx;
+	v2->vy = v1->vy - v3->vy;
+	v2->vz = v1->vz - v3->vz;
+
+	RotateVector(v2, &transmat);
+
+}
+
+
+
+
+
+/*
+
+ Returns "Yes" if "point" is inside "polygon"
+
+
+ **************************************************
+
+ WARNING!! Point and Polygon Data are OVERWRITTEN!!
+
+ **************************************************
+
+
+ The function requires point to be an integer array containing a single
+ XY pair. The number of points must be passed too.
+
+ Pass the size of the polygon point e.g. A Gouraud polygon has points X,Y,I
+ so its point size would be 3.
+
+
+ Item								Polygon Point Size
+ ----								------------------
+
+ I_Polygon							2
+ I_GouraudPolygon					3
+ I_2dTexturedPolygon				4
+ I_3dTexturedPolygon,			5
+ I_Gouraud2dTexturedPolygon	5
+ I_Polygon_ZBuffer				3
+ I_GouraudPolygon_ZBuffer		4
+
+
+ PASS ONLY POSITIVE COORDINATES!
+
+*/
+
+int PointInPolygon(int *point, int *polygon, int c, int ppsize)
+
+{
+
+
+	#if UseTimsPinp
+
+
+  /* Tim's New Point In Polygon test-- hopefully much faster, */
+  /* certainly much smaller. */
+  /* Uses Half-Line test for point-in-2D-polygon test */
+  /* Tests the half-line going from the point in the direction of positive z */
+
+  int  x, z;        /* point */
+  int  sx, sz;      /* vertex 1 */
+  int *polyp;       /* vertex 2 pointer */
+  int  t;
+  int  dx, dz;      /* ABS(vertex 2 - vertex 1) */
+  int  sgnx;        /* going left or going right */
+  int  intersects;  /* number of intersections so far discovered */
+  LONGLONGCH a_ll, b_ll;
+
+  /* reject lines and points */
+  if (c < 3) return(No);
+
+  intersects = 0;
+
+  x = point[ix];
+  z = point[iy];  /* ! */
+
+  /* get last point */
+  polyp = polygon + ((c - 1) * ppsize);
+  sx = polyp[0];
+  sz = polyp[1];
+
+  /* go back to first point */
+  polyp = polygon;
+
+  /* for each point */
+  while (0 != c)
+  {
+    /* is this line straddling the x co-ordinate of the point? */
+    /* if not it is not worth testing for intersection with the half-line */
+    /* we must be careful to get the strict and non-stict inequalities */
+    /* correct, or we may count intersections with vertices the wrong number */
+    /* of times. */
+    sgnx = 0;
+    if (sx < x && x <= polyp[0])
+    {
+      /* going right */
+      sgnx = 1;
+      dx   = polyp[0] - sx;
+    }
+    if (polyp[0] < x && x <= sx)
+    {
+      /* going left */
+      sgnx = -1;
+      dx   = sx - polyp[0];
+    }
+
+    /* if sgnx is zero then neither of the above conditions are true, */
+    /* hence the line does not straddle the point in x */
+    if (0 != sgnx)
+    {
+      /* next do trivial cases of line totally above or below point */
+      if (z < sz && z < polyp[1])
+      {
+        /* line totally above point -- intersection */
+        intersects++;
+      }
+      else if (z <= sz || z <= polyp[1])
+      {
+        /* line straddles point in both x and z -- we must do interpolation */
+
+        /* get absolute differences between line end z co-ordinates */
+        dz = (sz < polyp[1])?(polyp[1] - sz):(sz - polyp[1]);
+
+        /* B504 is the square root of 7FFFFFFF */
+        if (0xB504L < dx || 0xB504L < dz)
+        {
+          /* LARGE line -- use 64-bit values */
+          /* interpolate z */
+          MUL_I_WIDE(polyp[1] - sz, x - sx, &a_ll);
+          MUL_I_WIDE(polyp[0] - sx, z - sz, &b_ll);
+          if(CMP_LL(&a_ll, &b_ll) == sgnx)
+          {
+            /* we have an intersection */
+            intersects++;
+          }
+        }
+        else
+        {
+          /* small line -- use 32-bit values */
+          /* interpolate z */
+          t = (polyp[1] - sz) * (x - sx) - (polyp[0] - sx) * (z - sz);
+          if (t < 0 && sgnx < 0 || 0 < t && 0 < sgnx)
+          {
+            /* we have an intersection */
+            intersects++;
+          }
+        }
+      } /* (if line straddles point in z) */
+    } /* (if line straddles point in x) */
+
+    /* get next line : */
+    /* new vertex 1 is old vertex 2 */
+    sx = polyp[0];
+    sz = polyp[1];
+
+    /* new vertex 2 is next point */
+    polyp += ppsize;
+
+    /* next vertex */
+    c--;
+  }
+
+  if (intersects & 1)
+  {
+    /* Odd number of intersections -- point is inside polygon */
+    return(Yes);
+  }
+  else
+  {
+    /* even number of intersections -- point is outside polygon */
+    return(No);
+  }
+
+
+
+#else
+
+
+	int i;
+	int si, ti;
+	int s0, t0;
+	int s1, t1;
+	int *v0;
+	int *v1;
+	int ivdot, ivdotcnt, sgn_currivdot, sgn_ivdot, ivstate;
+	int ns, nt;
+	int x_scale, y_scale;
+	int DotNudge;
+
+	int x, z;
+	LONGLONGCH xx;
+	LONGLONGCH zz;
+	LONGLONGCH xx_tmp;
+	LONGLONGCH zz_tmp;
+	VECTORCH PolyAvgPt;
+
+
+	/* Reject points and lines */
+
+	if(c < 3) return No;
+
+
+	/* Find the average point */
+
+	v0 = polygon;
+
+	EQUALS_LL(&xx, &ll_zero);
+	EQUALS_LL(&zz, &ll_zero);
+
+	for(i = c; i!=0; i--) {
+
+		x = v0[0];
+		z = v0[1];
+
+		IntToLL(&xx_tmp, &x);		/* xx_tmp = (long long)x */
+		IntToLL(&zz_tmp, &z);		/* zz_tmp = (long long)z */
+
+		ADD_LL_PP(&xx, &xx_tmp);	/* xx += xx_tmp */
+		ADD_LL_PP(&zz, &zz_tmp);	/* zz += zz_tmp */
+
+		v0 += ppsize;
+
+	}
+
+	PolyAvgPt.vx = NarrowDivide(&xx, c);
+	PolyAvgPt.vz = NarrowDivide(&zz, c);
+
+
+	/* Centre the polygon */
+
+	v0 = polygon;
+
+	for(i = c; i!=0; i--) {
+
+		v0[0] -= PolyAvgPt.vx;
+		v0[1] -= PolyAvgPt.vz;
+
+		v0 += ppsize;
+
+	}
+
+
+	/* Centre the test point */
+
+	point[0] -= PolyAvgPt.vx;
+	point[1] -= PolyAvgPt.vz;
+
+
+	/* Scale to avoid maths overflow */
+
+	v0 = polygon;
+
+	s0 = 0;
+	t0 = 0;
+
+	for(i = c; i!=0; i--) {
+
+		si = v0[0]; if(si < 0) si = -si;
+		if(si > s0) s0 = si;
+
+		ti = v0[1]; if(ti < 0) ti = -ti;
+		if(ti > t0) t0 = ti;
+
+		v0 += ppsize;
+
+	}
+
+	si = point[ix]; if(si < 0) si = -si;
+	if(si > s0) s0 = si;
+
+	ti = point[iy]; if(ti < 0) ti = -ti;
+	if(ti > t0) t0 = ti;
+
+
+	#if 0
+	textprint("\nmax x = %d\n", s0);
+	textprint("max y = %d\n", t0);
+	#endif
+
+
+	x_scale = FindShift32(s0, 16383);
+	y_scale = FindShift32(t0, 16383);
+
+
+	#if 0
+	textprint("scales = %d, %d\n", x_scale, y_scale);
+	#endif
+
+
+	v0 = polygon;
+
+	for(i = c; i!=0; i--) {
+
+		v0[0] >>= x_scale;
+		v0[1] >>= y_scale;
+
+		/*textprint("(%d, %d)\n", v0[0], v0[1]);*/
+
+		v0 += ppsize;
+
+	}
+
+	point[ix] >>= x_scale;
+	point[iy] >>= y_scale;
+
+
+
+
+#if 1
+
+	/* Clockwise or Anti-Clockwise? */
+
+	ns = -(polygon[iy + ppsize] - polygon[iy]);
+	nt =  (polygon[ix + ppsize] - polygon[ix]);
+
+	si = polygon[(ppsize*2) + ix] - polygon[ix];
+	ti = polygon[(ppsize*2) + iy] - polygon[iy];
+
+	ivdot = (ns * si) + (nt * ti);
+
+	if(ivdot < 0) DotNudge = -1;
+	else DotNudge = 1;
+
+#endif
+
+
+
+	#if 0
+	if(ivdot < 0) textprint("Clockwise\n");
+	WaitForReturn();
+	#endif
+
+
+	/* Point to test */
+
+	si = point[ix];
+	ti = point[iy];
+
+
+	#if 0
+	textprint("p_test %d, %d\n", si, ti);
+	#endif
+
+
+	/* Polygon Vector pointers */
+
+	v0 = polygon;
+	v1 = v0 + ppsize;
+
+
+	/* Dot result monitor */
+
+	ivdotcnt = 0;
+	ivstate  = Yes;			/* assume inside */
+
+
+	/* Test v(s, t) against the vectors */
+
+	for(i = c; i!=0 && ivstate == Yes; i--) {
+
+
+		/* second vector pointer wraps once */
+
+		if(i == 1) v1 = polygon;
+
+
+		/* get the vector */
+
+		s0 = v0[ix];
+		t0 = v0[iy];
+
+		s1 = v1[ix];
+		t1 = v1[iy];
+
+
+		#if 0
+		textprint("%d,%d; %d,%d\n", s0, t0, s1, t1);
+		#endif
+
+
+		/* get the vector normal */
+
+		ns = -(t1 - t0);		/* s -> -t */
+		nt = s1 - s0;			/* t -> s  */
+
+
+		/* Dot with intersection point */
+
+		ivdot = (ns * (si - s0)) + (nt * (ti - t0));
+
+
+		/* TEST */
+		ivdot += DotNudge;
+
+
+		sgn_ivdot = 1;
+		if(ivdot < 0) sgn_ivdot = -1;
+
+
+		/* only continue if current dot is same as last, else quit */
+
+		if(ivdotcnt == 0) sgn_currivdot = sgn_ivdot;
+
+		else {
+
+			if(sgn_ivdot != sgn_currivdot) ivstate = No;
+			sgn_currivdot = sgn_ivdot;
+
+		}
+
+		v0 += ppsize;
+		v1 += ppsize;
+
+		ivdotcnt++;
+
+	}
+
+	if(ivstate) return Yes;
+	else return No;
+
+
+#endif
+
+
+}
+
+
+
+
+
+
+
+/*
+
+ #defines and statics required for Jamie's Most Excellent 
+ random number generator
+
+*/
+
+#define DEG_3	31
+#define SEP_3	3
+
+static long table [DEG_3] =
+{
+  -851904987, -43806228, -2029755270, 1390239686, -1912102820,
+  -485608943, 1969813258, -1590463333, -1944053249, 455935928,
+  508023712, -1714531963, 1800685987, -2015299881, 654595283,
+  -1149023258, -1470005550, -1143256056, -1325577603, -1568001885,
+  1275120390, -607508183, -205999574, -1696891592, 1492211999,
+  -1528267240, -952028296, -189082757, 362343714, 1424981831,
+  2039449641
+};
+
+#define TABLE_END (table + sizeof (table) / sizeof (table [0]))
+
+static long * front_ptr = table + SEP_3;
+static long * rear_ptr = table;
+
+
+void SetSeededFastRandom(int seed);
+void SetFastRandom(void)
+
+{
+
+	int i;
+	long number = GetTickCount();
+
+
+	for(i = 0; i < DEG_3; ++i) {
+
+      number   = 1103515145 * number + 12345;
+      table[i] = number;
+
+	}
+
+	front_ptr = table + SEP_3;
+	rear_ptr  = table;
+
+	for(i = 0; i < 10 * DEG_3; ++i)
+		(void) FastRandom ();
+
+	SetSeededFastRandom(FastRandom());
+
+}
+
+
+int FastRandom(void)
+
+{
+
+	long i;
+
+	/*
+
+	Discard least random bit.
+	Shift as unsigned to avoid replicating sign bit.
+	Faster than masking.
+
+	*/
+
+	*front_ptr += *rear_ptr;
+	i = (long) ((unsigned long) *front_ptr >> 1);
+
+	/* `front_ptr' and `rear_ptr' can't wrap at the same time. */
+
+	++front_ptr;
+
+	if(front_ptr < TABLE_END) {
+
+      ++rear_ptr;
+
+      if (rear_ptr < TABLE_END) return i;
+
+      rear_ptr = table;
+
+	}
+
+	else {				/* front_ptr >= TABLE_END */
+
+		front_ptr = table;
+		++rear_ptr;
+
+	}
+
+	return (int) i;
+
+}
+
+/*a second copy of the random number generator for getting random numbers from a single seed*/
+
+#define SEEDED_DEG_3	13
+#define SEEDED_SEP_3	3
+
+static long seeded_table [SEEDED_DEG_3];
+
+#define SEEDED_TABLE_END (seeded_table + sizeof (seeded_table) / sizeof (seeded_table [0]))
+
+static long * seeded_front_ptr = seeded_table + SEEDED_SEP_3;
+static long * seeded_rear_ptr = seeded_table;
+
+
+
+int SeededFastRandom(void)
+
+{
+
+	long i;
+
+	/*
+
+	Discard least random bit.
+	Shift as unsigned to avoid replicating sign bit.
+	Faster than masking.
+
+	*/
+
+	*seeded_front_ptr += *seeded_rear_ptr;
+	i = (long) ((unsigned long) *seeded_front_ptr >> 1);
+
+	/* `front_ptr' and `rear_ptr' can't wrap at the same time. */
+
+	++seeded_front_ptr;
+
+	if(seeded_front_ptr < SEEDED_TABLE_END) {
+
+      ++seeded_rear_ptr;
+
+      if (seeded_rear_ptr < SEEDED_TABLE_END) return i;
+
+      seeded_rear_ptr = seeded_table;
+
+	}
+
+	else {				/* front_ptr >= TABLE_END */
+
+		seeded_front_ptr = seeded_table;
+		++seeded_rear_ptr;
+
+	}
+
+	return (int) i;
+
+}
+
+void SetSeededFastRandom(int seed)
+
+{
+
+	int i;
+	long number = seed;
+
+
+	for(i = 0; i < SEEDED_DEG_3; ++i) {
+
+      number   = 1103515145 * number + 12345;
+      seeded_table[i] = number;
+
+	}
+
+	seeded_front_ptr = seeded_table + SEEDED_SEP_3;
+	seeded_rear_ptr  = seeded_table;
+
+	for(i = 0; i < 2 * SEEDED_DEG_3; ++i)
+		(void) SeededFastRandom ();
+
+}
+
+#if StandardShapeLanguage
+
+/*
+
+ Calculate the average point on this polygon
+
+*/
+
+void PolyAveragePoint(POLYHEADER *pheader, int *spts, VECTORCH *apt)
+
+{
+
+	int x, y, z;
+	LONGLONGCH xx;
+	LONGLONGCH yy;
+	LONGLONGCH zz;
+	LONGLONGCH xx_tmp;
+	LONGLONGCH yy_tmp;
+	LONGLONGCH zz_tmp;
+	int *mypolystart = &pheader->Poly1stPt;
+	int numpolypts;
+
+
+	/* Find the average point */
+
+	EQUALS_LL(&xx, &ll_zero);
+	EQUALS_LL(&yy, &ll_zero);
+	EQUALS_LL(&zz, &ll_zero);
+
+	numpolypts = 0;
+
+	while(*mypolystart != Term) {
+
+		x = *(spts + *mypolystart + ix);
+		y = *(spts + *mypolystart + iy);
+		z = *(spts + *mypolystart + iz);
+
+		IntToLL(&xx_tmp, &x);		/* xx_tmp = (long long)x */
+		IntToLL(&yy_tmp, &y);		/* yy_tmp = (long long)y */
+		IntToLL(&zz_tmp, &z);		/* zz_tmp = (long long)z */
+
+		ADD_LL_PP(&xx, &xx_tmp);	/* xx += xx_tmp */
+		ADD_LL_PP(&yy, &yy_tmp);	/* yy += yy_tmp */
+		ADD_LL_PP(&zz, &zz_tmp);	/* zz += zz_tmp */
+
+		numpolypts++;
+		mypolystart++;
+
+	}
+
+	apt->vx = NarrowDivide(&xx, numpolypts);
+	apt->vy = NarrowDivide(&yy, numpolypts);
+	apt->vz = NarrowDivide(&zz, numpolypts);
+
+}
+
+#endif	/* StandardShapeLanguage */
+
+
+
+
+
+
+/* KJL 15:07:39 01/08/97 - Returns the magnitude of the 
+   cross product of two vectors a and b. */
+int MagnitudeOfCrossProduct(VECTORCH *a, VECTORCH *b)
+
+{
+	VECTORCH c;				 
+    
+	c.vx = MUL_FIXED(a->vy,b->vz) - MUL_FIXED(a->vz,b->vy);
+	c.vy = MUL_FIXED(a->vz,b->vx) - MUL_FIXED(a->vx,b->vz);
+	c.vz = MUL_FIXED(a->vx,b->vy) - MUL_FIXED(a->vy,b->vx);
+    
+	return Magnitude(&c);
+}
+
+/* KJL 15:08:01 01/08/97 - sets the vector c to be the
+   cross product of the vectors a and b. */
+void CrossProduct(VECTORCH *a, VECTORCH *b, VECTORCH *c)
+
+{
+	c->vx = MUL_FIXED(a->vy,b->vz) - MUL_FIXED(a->vz,b->vy);
+	c->vy = MUL_FIXED(a->vz,b->vx) - MUL_FIXED(a->vx,b->vz);
+	c->vz = MUL_FIXED(a->vx,b->vy) - MUL_FIXED(a->vy,b->vx);
+}
+
+
+
+/* KJL 12:01:08 7/16/97 - returns the magnitude of a vector - max error about 13%, though average error
+less than half this. Very fast compared to other approaches. */
+int Approximate3dMagnitude(VECTORCH *v)
+{
+	int dx,dy,dz;
+
+	dx = v->vx;
+	if (dx<0) dx = -dx;
+	
+	dy = v->vy;
+	if (dy<0) dy = -dy;
+	
+	dz = v->vz;	 	 
+	if (dz<0) dz = -dz;
+						 
+	
+	if (dx>dy)
+	{
+		if (dx>dz)
+		{
+			return dx + ((dy+dz)>>2);
+		}
+		else
+		{
+			return dz + ((dy+dx)>>2);
+		}
+	}
+	else
+	{
+		if (dy>dz)
+		{
+			return dy + ((dx+dz)>>2);
+		}
+		else
+		{
+			return dz + ((dx+dy)>>2);
+		}
+	}
+}
+
+
+/*
+
+ Quaternion to Matrix
+
+ This is the column(row) matrix that is produced. Our matrices are
+ row(column) and so are a transpose of this.
+
+   1 - 2yy - 2zz           2xy + 2wz             2xz - 2wy
+
+   2xy - 2wz               1 - 2xx - 2zz         2yz + 2wx
+
+   2xz + 2wy               2yz - 2wx             1 - 2xx - 2yy
+
+*/
+
+void QuatToMat(QUAT *q,MATRIXCH *m)
+{
+
+	int q_w, q_x, q_y, q_z;
+
+	int q_2x, q_2y, q_2z;
+
+	int q_2xw;
+	int q_2xx;
+	int q_2xy;
+	int q_2xz;
+	int q_2yw;
+	int q_2yy;
+	int q_2yz;
+	int q_2zw;
+	int q_2zz;
+
+/*
+
+ The most efficient way to create the matrix is as follows
+
+ 1/ Double x, y & z
+
+*/
+
+	q_w=q->quatw;
+	q_x=q->quatx;
+	q_y=q->quaty;
+	q_z=q->quatz;
+
+	q_2x=q_x*2;
+	q_2y=q_y*2;
+	q_2z=q_z*2;
+
+/*
+
+ 2/ Form their products with w, x, y & z
+    These are
+
+    (2x)w   (2y)w  (2z)w
+    (2x)x
+    (2x)y   (2y)y
+    (2x)z   (2y)z  (2z)z
+
+*/
+
+	q_2xw=MUL_FIXED(q_2x,q_w);
+	q_2yw=MUL_FIXED(q_2y,q_w);
+	q_2zw=MUL_FIXED(q_2z,q_w);
+
+	q_2xx=MUL_FIXED(q_2x,q_x);
+
+	q_2xy=MUL_FIXED(q_2x,q_y);
+	q_2yy=MUL_FIXED(q_2y,q_y);
+
+	q_2xz=MUL_FIXED(q_2x,q_z);
+	q_2yz=MUL_FIXED(q_2y,q_z);
+	q_2zz=MUL_FIXED(q_2z,q_z);
+
+
+/* mat11 = 1 - 2y^2 - 2z^2	 */
+
+	m->mat11=ONE_FIXED-q_2yy-q_2zz;
+
+/* mat12 = 2xy - 2wz */
+
+	m->mat12=q_2xy-q_2zw;
+
+/* mat13 = 2xz + 2wy */
+
+	m->mat13=q_2xz+q_2yw;
+
+/* mat21 = 2xy + 2wz */
+
+	m->mat21=q_2xy+q_2zw;
+
+/* mat22 = 1 - 2x^2 - 2z^2 */
+
+	m->mat22=ONE_FIXED-q_2xx-q_2zz;
+
+/* mat23 = 2yz - 2wx	 */
+
+	m->mat23=q_2yz-q_2xw;
+
+/* mat31 = 2xz - 2wy */
+
+	m->mat31=q_2xz-q_2yw;
+
+/* mat32 = 2yz + 2wx */
+
+	m->mat32=q_2yz+q_2xw;
+
+/* mat33 = 1 - 2x^2 - 2y^2 */
+
+	m->mat33=ONE_FIXED-q_2xx-q_2yy;
+
+}
+