Note: I use a lookup table in the global variable sintab. This holds 4096 short's, each is the sin in fixed point format.
i.e. sintab[x] = (short) sin( (x*2*PI)/4096.0 )
This is a code fragment from one of the games I am currently working on.
dpitch is a long holding the amount of pitch (usally between -16 and 16).
tmp is a temporary MATRIX
tmp2 is another MATRIX, but this may not be necessary ( I haven't checked if ApplyMatrixLV destroys the input matrix).
axisx, axisy, and axixz are VECTOR's holding the plane's axis.
v0 is a temporary VECTOR.
if (dpitch!=0)
{
// Rotate about wings
RotateArbitrary(&tmp,axisx,-dpitch);
tmp2 = tmp; // ApplyMatrixLV fragments matrix (???)
v0 = axisy;
ApplyMatrixLV(&tmp,&v0,&axisy);
v0 = axisz;
ApplyMatrixLV(&tmp2,&v0,&axisz);
}
To create a rotation matrix.
// Constructs a rotation matrix around an axis specified by v0 into matrix tmp
// Rotation angle is in ang (4096=360degrees)
void RotateArbitrary(MATRIX *tmp, VECTOR v0, int ang)
{
int asin,acos;
int onemcos;
int wrk,wrk2;
// Get SIN and COS
if (ang<0)
ang+=4096;
asin = sintab[ang%4096];
acos = sintab[(ang+1024)%4096];
// Get (1-cos(ang))
onemcos = ONE-acos;
// Do (1,2) and (2,1) first
wrk = (v0.vx*v0.vy)>>12; // n1.n2
wrk = (wrk*onemcos)>>12; // n1.n2(1-cos(ang))
wrk2 = (v0.vz*asin)>>12; // n3.sin(ang)
tmp->m[1][0] = wrk+wrk2; // m12 = n1.n2(1-cos(ang))+n3.sin(ang)
tmp->m[0][1] = wrk-wrk2; // m21 = n1.n2(1-cos(ang))-n3.sin(ang)
// Do (1,3) and (3,1) next
wrk = (v0.vx*v0.vz)>>12; // n1.n3
wrk = (wrk*onemcos)>>12; // n1.n3(1-cos(ang))
wrk2 = (v0.vy*asin)>>12; // n2.sin(ang)
tmp->m[2][0] = wrk-wrk2; // m13 = n1.n3(1-cos(ang))-n2.sin(ang)
tmp->m[0][2] = wrk+wrk2; // m31 = n1.n3(1-cos(ang))+n2.sin(ang)
// Do (2,3) and (3,2) next
wrk = (v0.vy*v0.vz)>>12; // n2.n3
wrk = (wrk*onemcos)>>12; // n2.n3(1-cos(ang))
wrk2 = (v0.vx*asin)>>12; // n1.sin(ang)
tmp->m[2][1] = wrk+wrk2; // m23 = n2.n3(1-cos(ang))+n1.sin(ang)
tmp->m[1][2] = wrk-wrk2; // m32 = n2.n3(1-cos(ang))-n1.sin(ang)
// Do (1,1)
wrk = (v0.vx*v0.vx)>>12; // n1.n1
wrk2 = ((ONE-wrk)*acos)>>12; // (1-n1.n1)cos(ang)
tmp->m[0][0] = wrk + wrk2; // m11 = n1.n1+(1-n1.n1)cos(ang)
// Do (2,2)
wrk = (v0.vy*v0.vy)>>12; // n2.n2
wrk2 = ((ONE-wrk)*acos)>>12; // (1-n2.n2)cos(ang)
tmp->m[1][1] = wrk + wrk2; // m22 = n2.n2+(1-n2.n2)cos(ang)
// Do (3,3)
wrk = (v0.vz*v0.vz)>>12; // n3.n3
wrk2 = ((ONE-wrk)*acos)>>12; // (1-n3.n3)cos(ang)
tmp->m[2][2] = wrk + wrk2; // m33 = n3.n3+(1-n3.n3)cos(ang)
}
To repair the axis vectors.
// Repairs degenerate axis vectors
void RepairAxis(VECTOR* x, VECTOR* y, VECTOR* z)
{
int mag,sq;
// Normalize z
mag = z->vx*z->vx + z->vy*z->vy + z->vz*z->vz;
mag = sqrt(mag);
if (mag!=0)
{
z->vx = (z->vx<<12)/mag;
z->vy = (z->vy<<12)/mag;
z->vz = (z->vz<<12)/mag;
}
// New x is calculated from y x z (Cross product of y and z)
x->vx = (y->vy*z->vz - y->vz*z->vy)>>12;
x->vy = (y->vz*z->vx - y->vx*z->vz)>>12;
x->vz = (y->vx*z->vy - y->vy*z->vx)>>12;
mag = x->vx*x->vx + x->vy*x->vy + x->vz*x->vz;
mag = sqrt(mag);
if (mag!=0)
{
x->vx = (x->vx<<12)/mag;
x->vy = (x->vy<<12)/mag;
x->vz = (x->vz<<12)/mag;
}
// New y is calculated from z x x
y->vx = (z->vy*x->vz - z->vz*x->vy)>>12;
y->vy = (z->vz*x->vx - z->vx*x->vz)>>12;
y->vz = (z->vx*x->vy - z->vy*x->vx)>>12;
mag = y->vx*y->vx + y->vy*y->vy + y->vz*y->vz;
mag = sqrt(mag);
if (mag!=0)
{
y->vx = (y->vx<<12)/mag;
y->vy = (y->vy<<12)/mag;
y->vz = (y->vz<<12)/mag;
}
}
Update coordinates for displaying.
object3d is a structure with a GsCOORDINATE in coord, and a VECTOR in origin holding the objects position in 3D space.
void UpdateObjectCoordinatesAxis(object3d *objpt, VECTOR x, VECTOR y, VECTOR z)
{
MATRIX tmp;
tmp.t[0] = objpt->origin.vx;
tmp.t[1] = objpt->origin.vy;
tmp.t[2] = objpt->origin.vz;
tmp.m[0][0] = x.vx; tmp.m[1][0] = x.vy; tmp.m[2][0] = x.vz;
tmp.m[0][1] = y.vx; tmp.m[1][1] = y.vy; tmp.m[2][1] = y.vz;
tmp.m[0][2] = z.vx; tmp.m[1][2] = z.vy; tmp.m[2][2] = z.vz;
objpt->coord.coord = tmp;
objpt->coord.flg = 0;
}