You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
34 lines
941 B
34 lines
941 B
#include "common.h"
|
|
|
|
MATRIX *RotMatrixQ(SVECTOR *r, MATRIX *m)
|
|
{
|
|
short s[3],c[3];
|
|
MATRIX tm[3];
|
|
|
|
s[0] = isin(r->vx); s[1] = isin(r->vy); s[2] = isin(r->vz);
|
|
c[0] = icos(r->vx); c[1] = icos(r->vy); c[2] = icos(r->vz);
|
|
|
|
// mY (roll)
|
|
m->m[0][0] = c[1]; m->m[0][1] = 0; m->m[0][2] = s[1];
|
|
m->m[1][0] = 0; m->m[1][1] = ONE; m->m[1][2] = 0;
|
|
m->m[2][0] = -s[1]; m->m[2][1] = 0; m->m[2][2] = c[1];
|
|
|
|
// mX (pitch)
|
|
tm[0].m[0][0] = ONE; tm[0].m[0][1] = 0; tm[0].m[0][2] = 0;
|
|
tm[0].m[1][0] = 0; tm[0].m[1][1] = c[0]; tm[0].m[1][2] = -s[0];
|
|
tm[0].m[2][0] = 0; tm[0].m[2][1] = s[0]; tm[0].m[2][2] = c[0];
|
|
|
|
// mZ (yaw)
|
|
tm[1].m[0][0] = c[2]; tm[1].m[0][1] = -s[2]; tm[1].m[0][2] = 0;
|
|
tm[1].m[1][0] = s[2]; tm[1].m[1][1] = c[2]; tm[1].m[1][2] = 0;
|
|
tm[1].m[2][0] = 0; tm[1].m[2][1] = 0; tm[1].m[2][2] = ONE;
|
|
|
|
PushMatrix();
|
|
|
|
MulMatrix0( m, &tm[1], &tm[2] );
|
|
MulMatrix0( &tm[2], &tm[0], m );
|
|
|
|
PopMatrix();
|
|
|
|
return m;
|
|
}
|