Imported pristine psxsdk-20190410 from official repo

1 files changed, 45 insertions, 0 deletions

diff --git a/libmeidogte/matrix.c b/libmeidogte/matrix.c
new file mode 100644
index 0000000..1c226e1
--- /dev/null
+++ b/libmeidogte/matrix.c
@@ -0,0 +1,45 @@
+#include <meidogte.h>
+
+MATRIX *RotMatrix(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);
+
+	// mX
+	m->m[0][0] = ONE;		m->m[0][1] = 0;			m->m[0][2] = 0;
+	m->m[1][0] = 0;			m->m[1][1] = c[0];		m->m[1][2] = -s[0];
+	m->m[2][0] = 0;			m->m[2][1] = s[0];		m->m[2][2] = c[0];
+
+	// mY
+	tm[0].m[0][0] = c[1];	tm[0].m[0][1] = 0;		tm[0].m[0][2] = s[1];
+	tm[0].m[1][0] = 0;		tm[0].m[1][1] = ONE;	tm[0].m[1][2] = 0;
+	tm[0].m[2][0] = -s[1];	tm[0].m[2][1] = 0;		tm[0].m[2][2] = c[1];
+
+	// mZ
+	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[0], &tm[2] );
+	MulMatrix0( &tm[2], &tm[1], m );
+
+	PopMatrix();
+
+	return m;
+
+}
+
+MATRIX *TransMatrix(MATRIX *m, VECTOR *r) {
+
+	m->t[0] = r->vx;
+	m->t[1] = r->vy;
+	m->t[2] = r->vz;
+
+	return m;
+
+}