#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;
}