#include "3dc.h" #include "inline.h" #define UseTimsPinp Yes /* externs for commonly used global variables and arrays */ extern int sine[]; extern int cosine[]; extern short ArcCosTable[]; extern short ArcSineTable[]; extern short ArcTanTable[]; extern LONGLONGCH ll_zero; extern int NormalFrameTime; /* Globals */ MATRIXCH IdentityMatrix = { ONE_FIXED, 0, 0, 0, ONE_FIXED, 0, 0, 0, ONE_FIXED }; /* Maths functions used by the system */ /* 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) { 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; dx = 0; /* TODO: uninitialized?? */ /* 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; }