jetomit/avp
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions
avp/src/maths.c
Steven Fuller 9e5b7f430d Moved inline assembly to a separate file for debugging. 
Implemented GetTickCount/timeGetTime.

Added basic SDL/OpenGL support.

Draws something with no optimizations, but draws nothing with -O2. (What is
drawn looks like garbage.)
2019-08-20 02:22:36 +02:00

2861 lines
42 KiB
C
Raw Blame History

#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<39> 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<39> */
if(dotxy > dotxz && dotxy > dotyz) {
/* xy are the closest to 90<39> */
/*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<39> */
/*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<39> */
/*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<30> is Map North
090<39> is Map East
180<38> is Map South
270<37> 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;
}
Reference in a new issue View git blame Copy permalink