add matrix multiplication

1 files changed, 35 insertions, 0 deletions

diff --git a/src/transform.c b/src/transform.c
index 465841d..b0f6dc9 100644
--- a/src/transform.c
+++ b/src/transform.c
@@ -10,6 +10,41 @@
 #define TWO_PI 6.2831853071796f
 
 
+int kai_identity(ka_matrix_t *m)
+{
+	memset(*m, 0, sizeof(ka_matrix_t));
+	(*m)[0] = 1.0f;
+	(*m)[5] = 1.0f;
+	(*m)[10] = 1.0f;
+	(*m)[15] = 1.0f;
+
+	return 0;
+}
+
+
+int kai_multiply(ka_matrix_t *dest, ka_matrix_t A, ka_matrix_t B)
+{
+	ka_matrix_t C;
+	memset(C, 0, sizeof(ka_matrix_t));
+
+	int i, j, k;
+	int a_ind, b_ind, c_ind;
+	for (i=0; i<4; i++) {
+		for (j=0; j<4; j++) {
+			c_ind = (4*i) + j;
+			for (k=0; k<4; k++) {
+				a_ind = (4*i) + k;
+				b_ind = (4*k) + j;
+				C[c_ind] += A[a_ind] * B[b_ind];
+			}
+		}
+	}
+
+	memcpy(*dest, C, sizeof(ka_matrix_t));
+	return 0;
+}
+
+
 int kai_parse_matrix(ka_matrix_t *m, ezxml_t tag)
 {
 	if (strcmp("matrix", ezxml_name(tag)) != 0)