From 218ca90543758a20ac326e444ca0643174ca7384 Mon Sep 17 00:00:00 2001 From: Rebellion Developments Date: Thu, 16 Mar 2000 11:25:00 +0100 Subject: Import Aliens vs Predator - Gold (Build 116) Source code release, imported from: https://www.gamefront.com/games/aliens-vs-predator-3/file/avp-gold-complete-source-code All text files were converted to Unix format. --- 3dc/Maths.c | 2944 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 2944 insertions(+) create mode 100644 3dc/Maths.c (limited to '3dc/Maths.c') diff --git a/3dc/Maths.c b/3dc/Maths.c new file mode 100644 index 0000000..5cd3f49 --- /dev/null +++ b/3dc/Maths.c @@ -0,0 +1,2944 @@ + +#if PSX +#include +#include +#include +#include +#include +#include +#include +#include +#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; + + +/* + This code (FastRandom and SetFastRandom) stolen from Jamie Lokier + September 95. The original version was part of a C library + implementation +*/ + + +/* This is derived from the GNU C library source, which is in turn + derived from Berkeley source. The algorithm, the polynomial, and the + initial numbers are the same, but the code has been reworked for the + needs of this version. + + This version doesn't support different types of random number + generators, or saving and restoring the state. It is fast, short and + as simple as it can be while still generating numbers as good as the + Berkeley one. The basic algorithm is to have a linear-feedback shift + register, whose bits are the least significant bits of each word in + the `table' array. The higher-order bits are generated by carries + from the arithmetic on the shift register bits, and have an even + longer period than the shift register. */ + +/* x**31 + x**3 + 1. */ + +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; + +} + -- cgit v1.3