3 files changed, 114 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)
diff --git a/src/transform.h b/src/transform.h
index f8dc2ad..db582ac 100644
--- a/src/transform.h
+++ b/src/transform.h
@@ -5,6 +5,12 @@
 #include <ezxml.h>
 
 
+/* fill a ka_matrix_t with the identity matrix */
+int kai_identity(ka_matrix_t *m);
+
+/* multiply two matrices together */
+int kai_multiply(ka_matrix_t *dest, ka_matrix_t A, ka_matrix_t B);
+
 /* parse a <matrix> tag into a ka_matrix_t */
 int kai_parse_matrix(ka_matrix_t *m, ezxml_t tag);
 
diff --git a/src/transform.test.c b/src/transform.test.c
index ba663bb..4b1e329 100644
--- a/src/transform.test.c
+++ b/src/transform.test.c
@@ -3,6 +3,8 @@
 #include "test/test.h"
 #include "transform.h"
 
+void create_identity();
+void multiply();
 
 void parse_matrix_fail_nonmatrix();
 void parse_identity();
@@ -19,6 +21,8 @@ void parse_translate();
 
 void suite_transform()
 {
+	lily_run_test(create_identity);
+	lily_run_test(multiply);
 	lily_run_test(parse_matrix_fail_nonmatrix);
 	lily_run_test(parse_identity);
 	lily_run_test(parse_rotate_fail);
@@ -32,6 +36,75 @@ void suite_transform()
 
 /* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
 
+/* ======== basic ======== */
+
+void create_identity()
+{
+	ka_matrix_t mat;
+	mat[0] = 100;
+	int rc = kai_identity(&mat);
+	lily_assert_true(rc == 0);
+
+	lily_assert_float_equal(mat[0], 1.0f, 1e-3);
+	lily_assert_float_equal(mat[1], 0.0f, 1e-3);
+	lily_assert_float_equal(mat[2], 0.0f, 1e-3);
+	lily_assert_float_equal(mat[3], 0.0f, 1e-3);
+	lily_assert_float_equal(mat[4], 0.0f, 1e-3);
+	lily_assert_float_equal(mat[5], 1.0f, 1e-3);
+	lily_assert_float_equal(mat[6], 0.0f, 1e-3);
+	lily_assert_float_equal(mat[7], 0.0f, 1e-3);
+	lily_assert_float_equal(mat[8], 0.0f, 1e-3);
+	lily_assert_float_equal(mat[9], 0.0f, 1e-3);
+	lily_assert_float_equal(mat[10], 1.0f, 1e-3);
+	lily_assert_float_equal(mat[11], 0.0f, 1e-3);
+	lily_assert_float_equal(mat[12], 0.0f, 1e-3);
+	lily_assert_float_equal(mat[13], 0.0f, 1e-3);
+	lily_assert_float_equal(mat[14], 0.0f, 1e-3);
+	lily_assert_float_equal(mat[15], 1.0f, 1e-3);
+}
+
+
+void multiply()
+{
+	ka_matrix_t A = {
+		2.0f,  0.0f,  1.0f,  0.0f,
+		0.0f,  3.0f,  0.0f,  0.0f,
+		0.0f,  0.0f,  1.0f,  0.0f,
+		0.0f,  0.0f,  0.0f,  1.0f,
+	};
+
+	ka_matrix_t B = {
+		0.0f,  3.0f,  0.0f,  0.0f,
+		1.0f,  0.0f,  1.0f,  0.0f,
+		0.0f,  2.0f,  2.0f,  0.0f,
+		0.0f,  0.0f,  0.0f,  1.0f,
+	};
+
+	int rc = kai_multiply(&A, A, B);
+	lily_assert_true(rc == 0);
+
+	lily_assert_float_equal(A[0],  0.0f, 1e-3);
+	lily_assert_float_equal(A[1],  8.0f, 1e-3);
+	lily_assert_float_equal(A[2],  2.0f, 1e-3);
+	lily_assert_float_equal(A[3],  0.0f, 1e-3);
+
+	lily_assert_float_equal(A[4],  3.0f, 1e-3);
+	lily_assert_float_equal(A[5],  0.0f, 1e-3);
+	lily_assert_float_equal(A[6],  3.0f, 1e-3);
+	lily_assert_float_equal(A[7],  0.0f, 1e-3);
+
+	lily_assert_float_equal(A[8],  0.0f, 1e-3);
+	lily_assert_float_equal(A[9],  2.0f, 1e-3);
+	lily_assert_float_equal(A[10], 2.0f, 1e-3);
+	lily_assert_float_equal(A[11], 0.0f, 1e-3);
+
+	lily_assert_float_equal(A[12], 0.0f, 1e-3);
+	lily_assert_float_equal(A[13], 0.0f, 1e-3);
+	lily_assert_float_equal(A[14], 0.0f, 1e-3);
+	lily_assert_float_equal(A[15], 1.0f, 1e-3);
+}
+
+
 /* ======== matrix ======== */
 void parse_matrix_fail_nonmatrix()
 {