jetomit/avp
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions
avp/3dc/Maths.c
Rebellion Developments 218ca90543 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.
2019-08-19 05:45:17 +02:00

2944 lines
44 KiB
C
Raw Blame History

#if PSX
#include <kernel.h>
#include <sys/types.h>
#include <libetc.h>
#include <libgte.h>
#include <libgpu.h>
#include <stdlib.h>
#include <inline_c.h>
#include <gtemac.h>
#endif
#include "3dc.h"
#include "inline.h"
#define UseTimsPinp Yes
#define trip_debugger No
#if trip_debugger
int testa = 0;
int testb = 100;
int testc = 0;
#endif
/*
externs for commonly used global variables and arrays
*/
#if platform_pc
extern int sine[];
extern int cosine[];
#endif
extern short ArcCosTable[];
extern short ArcSineTable[];
extern short ArcTanTable[];
extern LONGLONGCH ll_zero;
extern int NormalFrameTime;
#if PSX
extern unsigned long *scratchp;
#endif
/*
Globals
*/
MATRIXCH IdentityMatrix = {
ONE_FIXED, 0, 0,
0, ONE_FIXED, 0,
0, 0, ONE_FIXED
};
/*
Maths functions used by the system
*/
#if PSX
inline void ch2psx(MATRIXCH *chm, MATRIX *psxm)
{
psxm->m[0][0] = chm->mat11 >> 4;
psxm->m[0][1] = chm->mat21 >> 4;
psxm->m[0][2] = chm->mat31 >> 4;
psxm->m[1][0] = chm->mat12 >> 4;
psxm->m[1][1] = chm->mat22 >> 4;
psxm->m[1][2] = chm->mat32 >> 4;
psxm->m[2][0] = chm->mat13 >> 4;
psxm->m[2][1] = chm->mat23 >> 4;
psxm->m[2][2] = chm->mat33 >> 4;
}
inline void psx2ch(MATRIX *psxm, MATRIXCH *chm)
{
chm->mat11 = psxm->m[0][0] << 4;
chm->mat21 = psxm->m[0][1] << 4;
chm->mat31 = psxm->m[0][2] << 4;
chm->mat12 = psxm->m[1][0] << 4;
chm->mat22 = psxm->m[1][1] << 4;
chm->mat32 = psxm->m[1][2] << 4;
chm->mat13 = psxm->m[2][0] << 4;
chm->mat23 = psxm->m[2][1] << 4;
chm->mat33 = psxm->m[2][2] << 4;
}
#endif
/* One over sin functions - CDF 4/2/98 */
extern int oneoversin[4096];
void ConstructOneOverSinTable(void) {
int a,sin;
for (a=0; a<4096; a++) {
sin=GetSin(a);
if (sin!=0) {
oneoversin[a]=DIV_FIXED(ONE_FIXED,sin);
} else {
sin=100;
oneoversin[a]=DIV_FIXED(ONE_FIXED,sin);
}
}
}
int GetOneOverSin(int a) {
int b;
b=a&wrap360;
return(oneoversin[b]);
}
/*
Dot Product Function
It accepts two pointers to vectors and returns an int result
*/
int _DotProduct(VECTORCH *vptr1, VECTORCH *vptr2)
{
int dp;
dp = MUL_FIXED(vptr1->vx, vptr2->vx);
dp += MUL_FIXED(vptr1->vy, vptr2->vy);
dp += MUL_FIXED(vptr1->vz, vptr2->vz);
return(dp);
}
int DotProduct2d(VECTOR2D *vptr1, VECTOR2D *vptr2)
{
int dp;
dp = MUL_FIXED(vptr1->vx, vptr2->vx);
dp += MUL_FIXED(vptr1->vy, vptr2->vy);
return dp;
}
/*
This function returns the distance between two vectors
*/
int VectorDistance(VECTORCH *v1, VECTORCH *v2)
{
VECTORCH v;
v.vx = v1->vx - v2->vx;
v.vy = v1->vy - v2->vy;
v.vz = v1->vz - v2->vz;
return Magnitude(&v);
}
/*
This function compares the distance between two vectors along each of
the major axes and returns Yes or No if they are within the cube defined
by the argument passed.
*/
int OutcodeVectorDistance(VECTORCH *v1, VECTORCH *v2, int d)
{
int i;
i = v1->vx - v2->vx;
if(i < 0) i = -i;
if(i >= d) return No;
i = v1->vy - v2->vy;
if(i < 0) i = -i;
if(i >= d) return No;
i = v1->vz - v2->vz;
if(i < 0) i = -i;
if(i >= d) return No;
return Yes;
}
/*
Subtract one VECTORCH from another and return the result as a normal
v3 = Normal(v1 - v2)
*/
void GetNormalVector(VECTORCH *v1, VECTORCH *v2, VECTORCH *v3)
{
v3->vx = v1->vx - v2->vx;
v3->vy = v1->vy - v2->vy;
v3->vz = v1->vz - v2->vz;
Normalise(v3);
}
/*
Normalise a vector close to, but less than, unit length
*/
void Renormalise(VECTORCH *nvector)
{
int m;
int xsq, ysq, zsq;
/* Scale x, y and z */
nvector->vx >>= 2;
nvector->vy >>= 2;
nvector->vz >>= 2;
/* Normalise */
xsq = nvector->vx * nvector->vx;
ysq = nvector->vy * nvector->vy;
zsq = nvector->vz * nvector->vz;
m = SqRoot32(xsq + ysq + zsq);
if(m == 0) m = 1; /* Just in case */
nvector->vx = (nvector->vx * ONE_FIXED) / m;
nvector->vy = (nvector->vy * ONE_FIXED) / m;
nvector->vz = (nvector->vz * ONE_FIXED) / m;
}
/*
Return the shift value required to get one value LTE the other value
*/
int FindShift32(int value, int limit)
{
int shift = 0;
/*if(limit == 0) exit(0xfa11fa11);*/
if(value < 0) value = -value;
while(value > limit) {
#if trip_debugger
if(shift > 32) {
testa = testb / testc;
}
#endif
shift++;
value >>= 1;
}
return shift;
}
/*
Return the largest value of an int array
*/
int MaxInt(int *iarray, int iarraysize)
{
int imax = smallint;
int i;
for(i = iarraysize; i!=0; i--) {
if(imax < *iarray) imax = *iarray;
iarray++;
}
return imax;
}
/*
Return the smallest value of an int array
*/
int MinInt(int *iarray, int iarraysize)
{
int imin = bigint;
int i;
for(i = iarraysize; i!=0; i--) {
if(imin > *iarray) imin = *iarray;
iarray++;
}
return imin;
}
/*
Create Matrix from Euler Angles
It requires a pointer to some euler angles and a pointer to a matrix
Construct the matrix elements using the following formula
Formula for ZXY Matrix
m11 = cy*cz + sx*sy*sz m12 = -cy*sz + sx*sy*cz m13 = cx*sy
m21 = cx*sz m22 = cx*cz m23 = -sx
m31 = -sy*cz + sx*cy*sz m32 = sy*sz + sx*cy*cz m33 = cx*cy
*/
void CreateEulerMatrix(e, m1)
EULER *e;
MATRIXCH *m1;
{
#if 0
SVECTOR eulers;
eulers.vx=(e->EulerX)&4095;
eulers.vy=(e->EulerY)&4095;
eulers.vz=(e->EulerZ)&4095;
RotMatrix(&eulers,(MATRIX *)scratchp);
psx2ch((MATRIX *)scratchp,m1);
#else
int t, sx, sy, sz, cx, cy, cz;
sx = GetSin(e->EulerX);
sy = GetSin(e->EulerY);
sz = GetSin(e->EulerZ);
cx = GetCos(e->EulerX);
cy = GetCos(e->EulerY);
cz = GetCos(e->EulerZ);
#if 0
textprint("Euler Matrix Sines & Cosines\n");
textprint("%d, %d, %d\n", sx, sy, sz);
textprint("%d, %d, %d\n", cx, cy, cz);
#endif
/* m11 = cy*cz + sx*sy*sz */
m1->mat11 = MUL_FIXED(cy, cz); /* cy*cz */
t = MUL_FIXED(sx, sy); /* sx*sy */
t = MUL_FIXED(t, sz); /* *sz */
m1->mat11 += t;
/* m12 = -cy*sz + sx*sy*cz */
m1->mat12=MUL_FIXED(-cy,sz);
t=MUL_FIXED(sx,sy);
t=MUL_FIXED(t,cz);
m1->mat12+=t;
/* m13 = cx*sy */
m1->mat13=MUL_FIXED(cx,sy);
/* m21 = cx*sz */
m1->mat21=MUL_FIXED(cx,sz);
/* m22 = cx*cz */
m1->mat22=MUL_FIXED(cx,cz);
/* m23 = -sx */
m1->mat23=-sx;
/* m31 = -sy*cz + sx*cy*sz */
m1->mat31=MUL_FIXED(-sy,cz);
t=MUL_FIXED(sx,cy);
t=MUL_FIXED(t,sz);
m1->mat31+=t;
/* m32 = sy*sz + sx*cy*cz */
m1->mat32=MUL_FIXED(sy,sz);
t=MUL_FIXED(sx,cy);
t=MUL_FIXED(t,cz);
m1->mat32+=t;
/* m33 = cx*cy */
m1->mat33=MUL_FIXED(cx,cy);
#endif
}
/*
Create a Unit Vector from three Euler Angles
*/
void CreateEulerVector(EULER *e, VECTORCH *v)
{
int t, sx, sy, sz, cx, cy, cz;
sx = GetSin(e->EulerX);
sy = GetSin(e->EulerY);
sz = GetSin(e->EulerZ);
cx = GetCos(e->EulerX);
cy = GetCos(e->EulerY);
cz = GetCos(e->EulerZ);
/* x = -sy*cz + sx*cy*sz */
v->vx = MUL_FIXED(-sy, cz);
t = MUL_FIXED(sx, cy);
t = MUL_FIXED(t, sz);
v->vx += t;
/* y = sy*sz + sx*cy*cz */
v->vy = MUL_FIXED(sy, sz);
t = MUL_FIXED(sx, cy);
t = MUL_FIXED(t, cz);
v->vy += t;
/* z = cx*cy */
v->vz = MUL_FIXED(cx,cy);
}
/*
Matrix Multiply Function
A 3x3 Matrix is represented here as
m11 m12 m13
m21 m22 m23
m31 m32 m33
Row #1 (r1) of the matrix is m11 m12 m13
Column #1 (c1) of the matrix is m11 m32 m31
Under multiplication
m'' = m x m'
where
m11'' = c1.r1'
m12'' = c2.r1'
m13'' = c3.r1'
m21'' = c1.r2'
m22'' = c2.r2'
m23'' = c3.r2'
m31'' = c1.r3'
m32'' = c2.r3'
m33'' = c3.r3'
*/
void MatrixMultiply(m1, m2, m3)
struct matrixch *m1, *m2, *m3;
{
#if 0
PushMatrix();
ch2psx(m1,(MATRIX *)scratchp);
ch2psx(m2,(MATRIX *)(scratchp+(sizeof(MATRIX))));
MulMatrix0((MATRIX *)scratchp,(MATRIX *)(scratchp+(sizeof(MATRIX))),(MATRIX *)(scratchp+((sizeof(MATRIX)<<1))));
psx2ch((MATRIX *)(scratchp+((sizeof(MATRIX)<<1))),m3);
PopMatrix();
#else
MATRIXCH TmpMat;
/* m11'' = c1.r1' */
TmpMat.mat11=MUL_FIXED(m1->mat11,m2->mat11);
TmpMat.mat11+=MUL_FIXED(m1->mat21,m2->mat12);
TmpMat.mat11+=MUL_FIXED(m1->mat31,m2->mat13);
/* m12'' = c2.r1' */
TmpMat.mat12=MUL_FIXED(m1->mat12,m2->mat11);
TmpMat.mat12+=MUL_FIXED(m1->mat22,m2->mat12);
TmpMat.mat12+=MUL_FIXED(m1->mat32,m2->mat13);
/* m13'' = c3.r1' */
TmpMat.mat13=MUL_FIXED(m1->mat13,m2->mat11);
TmpMat.mat13+=MUL_FIXED(m1->mat23,m2->mat12);
TmpMat.mat13+=MUL_FIXED(m1->mat33,m2->mat13);
/* m21'' = c1.r2' */
TmpMat.mat21=MUL_FIXED(m1->mat11,m2->mat21);
TmpMat.mat21+=MUL_FIXED(m1->mat21,m2->mat22);
TmpMat.mat21+=MUL_FIXED(m1->mat31,m2->mat23);
/* m22'' = c2.r2' */
TmpMat.mat22=MUL_FIXED(m1->mat12,m2->mat21);
TmpMat.mat22+=MUL_FIXED(m1->mat22,m2->mat22);
TmpMat.mat22+=MUL_FIXED(m1->mat32,m2->mat23);
/* m23'' = c3.r2' */
TmpMat.mat23=MUL_FIXED(m1->mat13,m2->mat21);
TmpMat.mat23+=MUL_FIXED(m1->mat23,m2->mat22);
TmpMat.mat23+=MUL_FIXED(m1->mat33,m2->mat23);
/* m31'' = c1.r3' */
TmpMat.mat31=MUL_FIXED(m1->mat11,m2->mat31);
TmpMat.mat31+=MUL_FIXED(m1->mat21,m2->mat32);
TmpMat.mat31+=MUL_FIXED(m1->mat31,m2->mat33);
/* m32'' = c2.r3' */
TmpMat.mat32=MUL_FIXED(m1->mat12,m2->mat31);
TmpMat.mat32+=MUL_FIXED(m1->mat22,m2->mat32);
TmpMat.mat32+=MUL_FIXED(m1->mat32,m2->mat33);
/* m33'' = c3.r3' */
TmpMat.mat33=MUL_FIXED(m1->mat13,m2->mat31);
TmpMat.mat33+=MUL_FIXED(m1->mat23,m2->mat32);
TmpMat.mat33+=MUL_FIXED(m1->mat33,m2->mat33);
/* Finally, copy TmpMat to m3 */
CopyMatrix(&TmpMat, m3);
#endif
}
void PSXAccurateMatrixMultiply(m1, m2, m3)
struct matrixch *m1, *m2, *m3;
{
MATRIXCH TmpMat;
/* m11'' = c1.r1' */
TmpMat.mat11=MUL_FIXED(m1->mat11,m2->mat11);
TmpMat.mat11+=MUL_FIXED(m1->mat21,m2->mat12);
TmpMat.mat11+=MUL_FIXED(m1->mat31,m2->mat13);
/* m12'' = c2.r1' */
TmpMat.mat12=MUL_FIXED(m1->mat12,m2->mat11);
TmpMat.mat12+=MUL_FIXED(m1->mat22,m2->mat12);
TmpMat.mat12+=MUL_FIXED(m1->mat32,m2->mat13);
/* m13'' = c3.r1' */
TmpMat.mat13=MUL_FIXED(m1->mat13,m2->mat11);
TmpMat.mat13+=MUL_FIXED(m1->mat23,m2->mat12);
TmpMat.mat13+=MUL_FIXED(m1->mat33,m2->mat13);
/* m21'' = c1.r2' */
TmpMat.mat21=MUL_FIXED(m1->mat11,m2->mat21);
TmpMat.mat21+=MUL_FIXED(m1->mat21,m2->mat22);
TmpMat.mat21+=MUL_FIXED(m1->mat31,m2->mat23);
/* m22'' = c2.r2' */
TmpMat.mat22=MUL_FIXED(m1->mat12,m2->mat21);
TmpMat.mat22+=MUL_FIXED(m1->mat22,m2->mat22);
TmpMat.mat22+=MUL_FIXED(m1->mat32,m2->mat23);
/* m23'' = c3.r2' */
TmpMat.mat23=MUL_FIXED(m1->mat13,m2->mat21);
TmpMat.mat23+=MUL_FIXED(m1->mat23,m2->mat22);
TmpMat.mat23+=MUL_FIXED(m1->mat33,m2->mat23);
/* m31'' = c1.r3' */
TmpMat.mat31=MUL_FIXED(m1->mat11,m2->mat31);
TmpMat.mat31+=MUL_FIXED(m1->mat21,m2->mat32);
TmpMat.mat31+=MUL_FIXED(m1->mat31,m2->mat33);
/* m32'' = c2.r3' */
TmpMat.mat32=MUL_FIXED(m1->mat12,m2->mat31);
TmpMat.mat32+=MUL_FIXED(m1->mat22,m2->mat32);
TmpMat.mat32+=MUL_FIXED(m1->mat32,m2->mat33);
/* m33'' = c3.r3' */
TmpMat.mat33=MUL_FIXED(m1->mat13,m2->mat31);
TmpMat.mat33+=MUL_FIXED(m1->mat23,m2->mat32);
TmpMat.mat33+=MUL_FIXED(m1->mat33,m2->mat33);
/* Finally, copy TmpMat to m3 */
CopyMatrix(&TmpMat, m3);
}
/*
Transpose Matrix
*/
void TransposeMatrixCH(m1)
MATRIXCH *m1;
{
int t;
t=m1->mat12;
m1->mat12=m1->mat21;
m1->mat21=t;
t=m1->mat13;
m1->mat13=m1->mat31;
m1->mat31=t;
t=m1->mat23;
m1->mat23=m1->mat32;
m1->mat32=t;
}
/*
Copy Vector
*/
void CopyVector(VECTORCH *v1, VECTORCH *v2)
{
/* Copy VECTORCH v1 -> VECTORCH v2 */
v2->vx=v1->vx;
v2->vy=v1->vy;
v2->vz=v1->vz;
}
/*
Copy Location
*/
void CopyLocation(VECTORCH *v1, VECTORCH *v2)
{
/* Copy VECTORCH v1 -> VECTORCH v2 */
v2->vx=v1->vx;
v2->vy=v1->vy;
v2->vz=v1->vz;
}
/*
Copy Euler
*/
void CopyEuler(EULER *e1, EULER *e2)
{
/* Copy EULER e1 -> EULER e2 */
e2->EulerX=e1->EulerX;
e2->EulerY=e1->EulerY;
e2->EulerZ=e1->EulerZ;
}
/*
Copy Matrix
*/
void CopyMatrix(MATRIXCH *m1, MATRIXCH *m2)
{
/* Copy MATRIXCH m1 -> MATRIXCH m2 */
m2->mat11=m1->mat11;
m2->mat12=m1->mat12;
m2->mat13=m1->mat13;
m2->mat21=m1->mat21;
m2->mat22=m1->mat22;
m2->mat23=m1->mat23;
m2->mat31=m1->mat31;
m2->mat32=m1->mat32;
m2->mat33=m1->mat33;
}
/*
Make a Vector.
v3 = v1 - v2
*/
void MakeVector(VECTORCH *v1, VECTORCH *v2, VECTORCH *v3)
{
v3->vx = v1->vx - v2->vx;
v3->vy = v1->vy - v2->vy;
v3->vz = v1->vz - v2->vz;
}
/*
Add a Vector.
v2 = v2 + v1
*/
void AddVector(VECTORCH *v1, VECTORCH *v2)
{
v2->vx += v1->vx;
v2->vy += v1->vy;
v2->vz += v1->vz;
}
/*
Subtract a Vector.
v2 = v2 - v1
*/
void SubVector(VECTORCH *v1, VECTORCH *v2)
{
v2->vx -= v1->vx;
v2->vy -= v1->vy;
v2->vz -= v1->vz;
}
/*
Matrix Rotatation of a Vector
Overwrite the Source Vector with the Rotated Vector
x' = v.c1
y' = v.c2
z' = v.c3
*/
void _RotateVector(VECTORCH *v, MATRIXCH* m)
{
int x, y, z;
x = MUL_FIXED(m->mat11, v->vx);
x += MUL_FIXED(m->mat21, v->vy);
x += MUL_FIXED(m->mat31, v->vz);
y = MUL_FIXED(m->mat12, v->vx);
y += MUL_FIXED(m->mat22, v->vy);
y += MUL_FIXED(m->mat32, v->vz);
z = MUL_FIXED(m->mat13, v->vx);
z += MUL_FIXED(m->mat23, v->vy);
z += MUL_FIXED(m->mat33, v->vz);
v->vx = x;
v->vy = y;
v->vz = z;
}
/*
Matrix Rotation of a Source Vector using a Matrix
Copying to a Destination Vector
x' = v.c1
y' = v.c2
z' = v.c3
*/
void _RotateAndCopyVector(v1, v2, m)
VECTORCH *v1;
VECTORCH *v2;
MATRIXCH *m;
{
v2->vx=MUL_FIXED(m->mat11,v1->vx);
v2->vx+=MUL_FIXED(m->mat21,v1->vy);
v2->vx+=MUL_FIXED(m->mat31,v1->vz);
v2->vy=MUL_FIXED(m->mat12,v1->vx);
v2->vy+=MUL_FIXED(m->mat22,v1->vy);
v2->vy+=MUL_FIXED(m->mat32,v1->vz);
v2->vz=MUL_FIXED(m->mat13,v1->vx);
v2->vz+=MUL_FIXED(m->mat23,v1->vy);
v2->vz+=MUL_FIXED(m->mat33,v1->vz);
}
/*
Matrix to Euler Angles
Maths overflow is a real problem for this function. To prevent overflows
the matrix Sines and Cosines are calculated using values scaled down by 4.
sinx = -M23
cosx = sqr ( 1 - sinx^2 )
siny = M13 / cosx
cosy = M33 / cosx
sinz = M21 / cosx
cosz = M22 / cosx
*/
#define m2e_scale 2
#define ONE_FIXED_S ((ONE_FIXED >> m2e_scale) - 1)
#define m2e_shift 14
#define j_and_r_change Yes
void MatrixToEuler(MATRIXCH *m, EULER *e)
{
int x, sinx, cosx, siny, cosy, sinz, cosz;
int abs_cosx, abs_cosy, abs_cosz;
int SineMatrixPitch, SineMatrixYaw, SineMatrixRoll;
int CosMatrixPitch, CosMatrixYaw, CosMatrixRoll;
#if 0
textprint("CosMatrixPitch = %d\n", CosMatrixPitch);
/* WaitForReturn(); */
#endif
if(m->mat32 >-65500 && m->mat32<65500)
{
/* Yaw */
/* Pitch */
#if j_and_r_change
SineMatrixPitch = -m->mat32;
#else
SineMatrixPitch = -m->mat23;
#endif
SineMatrixPitch >>= m2e_scale;
#if 0
textprint("SineMatrixPitch = %d\n", SineMatrixPitch);
/* WaitForReturn(); */
#endif
CosMatrixPitch = SineMatrixPitch * SineMatrixPitch;
CosMatrixPitch >>= m2e_shift;
CosMatrixPitch = -CosMatrixPitch;
CosMatrixPitch += ONE_FIXED_S;
CosMatrixPitch *= ONE_FIXED_S;
CosMatrixPitch = SqRoot32(CosMatrixPitch);
if(CosMatrixPitch) {
if(CosMatrixPitch > ONE_FIXED_S) CosMatrixPitch = ONE_FIXED_S;
else if(CosMatrixPitch < -ONE_FIXED_S) CosMatrixPitch = -ONE_FIXED_S;
}
else CosMatrixPitch = 1;
SineMatrixYaw = WideMulNarrowDiv(
#if j_and_r_change
m->mat31 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);
#else
m->mat13 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);
#endif
#if 0
textprint("SineMatrixYaw = %d\n", SineMatrixYaw);
/* WaitForReturn(); */
#endif
CosMatrixYaw = WideMulNarrowDiv(
m->mat33 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);
#if 0
textprint("CosMatrixYaw = %d\n", CosMatrixYaw);
/* WaitForReturn(); */
#endif
/* Roll */
SineMatrixRoll = WideMulNarrowDiv(
#if j_and_r_change
m->mat12 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);
#else
m->mat21 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);
#endif
#if 0
textprint("SineMatrixRoll = %d\n", SineMatrixRoll);
/* WaitForReturn(); */
#endif
CosMatrixRoll = WideMulNarrowDiv(
m->mat22 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);
#if 0
textprint("CosMatrixRoll = %d\n", CosMatrixRoll);
/* WaitForReturn(); */
#endif
/* Tables are for values +- 2^16 */
sinx = SineMatrixPitch << m2e_scale;
siny = SineMatrixYaw << m2e_scale;
sinz = SineMatrixRoll << m2e_scale;
cosx = CosMatrixPitch << m2e_scale;
cosy = CosMatrixYaw << m2e_scale;
cosz = CosMatrixRoll << m2e_scale;
#if 0
textprint("sines = %d, %d, %d\n", sinx, siny, sinz);
textprint("cos's = %d, %d, %d\n", cosx, cosy, cosz);
/* WaitForReturn(); */
#endif
/* Absolute Cosines */
abs_cosx = cosx;
if(abs_cosx < 0) abs_cosx = -abs_cosx;
abs_cosy = cosy;
if(abs_cosy < 0) abs_cosy = -abs_cosy;
abs_cosz = cosz;
if(abs_cosz < 0) abs_cosz = -abs_cosz;
/* Euler X */
if(abs_cosx > Cosine45) {
x = ArcSin(sinx);
if(cosx < 0) {
x = -x;
x += deg180;
x &= wrap360;
}
}
else {
x = ArcCos(cosx);
if(sinx < 0) {
x = -x;
x &= wrap360;
}
}
#if (j_and_r_change == No)
x = -x;
x &= wrap360;
#endif
e->EulerX = x;
/* Euler Y */
if(abs_cosy > Cosine45) {
x = ArcSin(siny);
if(cosy < 0) {
x = -x;
x += deg180;
x &= wrap360;
}
}
else {
x = ArcCos(cosy);
if(siny < 0) {
x = -x;
x &= wrap360;
}
}
#if (j_and_r_change == No)
x = -x;
x &= wrap360;
#endif
e->EulerY = x;
/* Euler Z */
if(abs_cosz > Cosine45) {
x = ArcSin(sinz);
if(cosz < 0) {
x = -x;
x += deg180;
x &= wrap360;
}
}
else {
x = ArcCos(cosz);
if(sinz < 0) {
x = -x;
x &= wrap360;
}
}
#if (j_and_r_change == No)
x = -x;
x &= wrap360;
#endif
e->EulerZ = x;
}
else //singularity case
{
if(m->mat32>0)
e->EulerX = 3072;
else
e->EulerX = 1024;
e->EulerZ=0;
/* Yaw */
siny = -m->mat13 ;
cosy = m->mat11 ;
abs_cosy = cosy;
if(abs_cosy < 0) abs_cosy = -abs_cosy;
if(abs_cosy > Cosine45) {
x = ArcSin(siny);
if(cosy < 0) {
x = -x;
x += deg180;
x &= wrap360;
}
}
else {
x = ArcCos(cosy);
if(siny < 0) {
x = -x;
x &= wrap360;
}
}
#if (j_and_r_change == No)
x = -x;
x &= wrap360;
#endif
e->EulerY = x;
}
#if 0
textprint("\nEuler from VDB Matrix is:\n%d\n%d\n%d\n",
e->EulerX,
e->EulerY,
e->EulerZ
);
/* WaitForReturn(); */
#endif
}
#if 1
#define j_and_r_change_2 Yes
void MatrixToEuler2(MATRIXCH *m, EULER *e)
{
int x, sinx, cosx, siny, cosy, sinz, cosz;
int abs_cosx, abs_cosy, abs_cosz;
int SineMatrixPitch, SineMatrixYaw, SineMatrixRoll;
int CosMatrixPitch, CosMatrixYaw, CosMatrixRoll;
/* Pitch */
#if j_and_r_change_2
SineMatrixPitch = -m->mat32;
#else
SineMatrixPitch = -m->mat23;
#endif
SineMatrixPitch >>= m2e_scale;
#if 0
textprint("SineMatrixPitch = %d\n", SineMatrixPitch);
/* WaitForReturn(); */
#endif
CosMatrixPitch = SineMatrixPitch * SineMatrixPitch;
CosMatrixPitch >>= m2e_shift;
CosMatrixPitch = -CosMatrixPitch;
CosMatrixPitch += ONE_FIXED_S;
CosMatrixPitch *= ONE_FIXED_S;
CosMatrixPitch = SqRoot32(CosMatrixPitch);
if(CosMatrixPitch) {
if(CosMatrixPitch > ONE_FIXED_S) CosMatrixPitch = ONE_FIXED_S;
else if(CosMatrixPitch < -ONE_FIXED_S) CosMatrixPitch = -ONE_FIXED_S;
}
else CosMatrixPitch = 1;
#if 0
textprint("CosMatrixPitch = %d\n", CosMatrixPitch);
/* WaitForReturn(); */
#endif
/* Yaw */
SineMatrixYaw = WideMulNarrowDiv(
#if j_and_r_change_2
m->mat31 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);
#else
m->mat13 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);
#endif
#if 0
textprint("SineMatrixYaw = %d\n", SineMatrixYaw);
/* WaitForReturn(); */
#endif
CosMatrixYaw = WideMulNarrowDiv(
m->mat33 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);
#if 0
textprint("CosMatrixYaw = %d\n", CosMatrixYaw);
/* WaitForReturn(); */
#endif
/* Roll */
SineMatrixRoll = WideMulNarrowDiv(
#if j_and_r_change_2
m->mat12 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);
#else
m->mat21 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);
#endif
#if 0
textprint("SineMatrixRoll = %d\n", SineMatrixRoll);
/* WaitForReturn(); */
#endif
CosMatrixRoll = WideMulNarrowDiv(
m->mat22 >> m2e_scale, ONE_FIXED_S, CosMatrixPitch);
#if 0
textprint("CosMatrixRoll = %d\n", CosMatrixRoll);
/* WaitForReturn(); */
#endif
/* Tables are for values +- 2^16 */
sinx = SineMatrixPitch << m2e_scale;
siny = SineMatrixYaw << m2e_scale;
sinz = SineMatrixRoll << m2e_scale;
cosx = CosMatrixPitch << m2e_scale;
cosy = CosMatrixYaw << m2e_scale;
cosz = CosMatrixRoll << m2e_scale;
#if 0
textprint("sines = %d, %d, %d\n", sinx, siny, sinz);
textprint("cos's = %d, %d, %d\n", cosx, cosy, cosz);
/* WaitForReturn(); */
#endif
/* Absolute Cosines */
abs_cosx = cosx;
if(abs_cosx < 0) abs_cosx = -abs_cosx;
abs_cosy = cosy;
if(abs_cosy < 0) abs_cosy = -abs_cosy;
abs_cosz = cosz;
if(abs_cosz < 0) abs_cosz = -abs_cosz;
/* Euler X */
if(abs_cosx > Cosine45) {
x = ArcSin(sinx);
if(cosx < 0) {
x = -x;
x += deg180;
x &= wrap360;
}
}
else {
x = ArcCos(cosx);
if(sinx < 0) {
x = -x;
x &= wrap360;
}
}
#if (j_and_r_change_2 == No)
x = -x;
x &= wrap360;
#endif
e->EulerX = x;
/* Euler Y */
if(abs_cosy > Cosine45) {
x = ArcSin(siny);
if(cosy < 0) {
x = -x;
x += deg180;
x &= wrap360;
}
}
else {
x = ArcCos(cosy);
if(siny < 0) {
x = -x;
x &= wrap360;
}
}
#if (j_and_r_change_2 == No)
x = -x;
x &= wrap360;
#endif
e->EulerY = x;
/* Euler Z */
if(abs_cosz > Cosine45) {
x = ArcSin(sinz);
if(cosz < 0) {
x = -x;
x += deg180;
x &= wrap360;
}
}
else {
x = ArcCos(cosz);
if(sinz < 0) {
x = -x;
x &= wrap360;
}
}
#if (j_and_r_change_2 == No)
x = -x;
x &= wrap360;
#endif
e->EulerZ = x;
#if 0
textprint("\nEuler from VDB Matrix is:\n%d\n%d\n%d\n",
e->EulerX,
e->EulerY,
e->EulerZ
);
/* WaitForReturn(); */
#endif
}
#endif
/*
Normalise a Matrix
Dot the three vectors together (XY, XZ, YZ) and take the two nearest to
90<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;
/* 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;
}
Reference in a new issue View git blame Copy permalink