From 9eccbe986a131f7985d68afd7fa84847732035b4 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Miloslav=20=C4=8C=C3=AD=C5=BE?= <tastyfish@seznam.cz>
Date: Wed, 21 Nov 2018 16:58:26 +0100
Subject: [PATCH] Continue matrices

---
 s3l.h  | 27 ++++++++++++++++++++++++++-
 test.c |  8 +++++---
 2 files changed, 31 insertions(+), 4 deletions(-)

diff --git a/s3l.h b/s3l.h
index 94265d8..de7344b 100644
--- a/s3l.h
+++ b/s3l.h
@@ -194,9 +194,10 @@ static inline void S3L_initMat4(S3L_Mat4 *m)
 
 /**
   Multiplies a vector by a matrix with normalization by S3L_FRACTIONS_PER_UNIT.
+  Result is stored in the input vector.
 */
 
-static inline void S3L_vec4Xmat4(S3L_Vec4 *v, S3L_Mat4 *m)
+void S3L_vec4Xmat4(S3L_Vec4 *v, S3L_Mat4 *m)
 {
   S3L_Vec4 vBackup;
 
@@ -221,6 +222,30 @@ static inline void S3L_vec4Xmat4(S3L_Vec4 *v, S3L_Mat4 *m)
   #undef dot
 }
 
+/**
+  Multiplies two matrices with normalization by S3L_FRACTIONS_PER_UNIT. Result
+  is stored in the first matrix.
+*/
+
+void S3L_mat4Xmat4(S3L_Mat4 *m1, S3L_Mat4 *m2)
+{
+  S3L_Mat4 mat1;
+
+  for (uint16_t row = 0; row < 4; ++row)
+    for (uint16_t col = 0; col < 4; ++col)
+      mat1[col][row] = (*m1)[col][row];
+
+  for (uint16_t row = 0; row < 4; ++row)
+    for (uint16_t col = 0; col < 4; ++col)
+    {
+      (*m1)[col][row] = 0;
+
+      for (uint16_t i = 0; i < 4; ++i)
+        (*m1)[col][row] +=
+          (mat1[i][row] * (*m2)[col][i]) / S3L_FRACTIONS_PER_UNIT;
+    }
+}
+
 static inline void S3L_makeTranslationMat(S3L_Unit offsetX, S3L_Unit offsetY,
   S3L_Unit offsetZ, S3L_Mat4 *m)
 {
diff --git a/test.c b/test.c
index fee55bd..72a4d8e 100644
--- a/test.c
+++ b/test.c
@@ -8,7 +8,7 @@ void pixelFunc(S3L_PixelInfo *pixel)
 
 int main()
 {
-  S3L_Mat4 m;
+  S3L_Mat4 m, m2;
   S3L_Vec4 v;
 
   S3L_initMat4(&m);
@@ -21,9 +21,11 @@ int main()
   S3L_vec4Xmat4(&v,&m); 
   S3L_writeVec4(v);
 
-  S3L_makeTranslationMat(100,200,300,&m);
-  S3L_writeMat4(m);
+  S3L_makeTranslationMat(100,200,300,&m2);
+  S3L_writeMat4(m2);
 
+  S3L_mat4Xmat4(&m,&m2);
+  S3L_writeMat4(m);
 
 
   return 0;