From 84fbb711e10bc3eeda1997a059c48fd99723f5ef Mon Sep 17 00:00:00 2001
From: Abdelrahman <said.abdelrahman89@gmail.com>
Date: Sun, 1 Sep 2024 22:28:11 +0100
Subject: [PATCH] Add function to invert 3x3 matrix

---
 src/vec/vec.c | 36 ++++++++++++++++++++++++++++++++++++
 src/vec/vec.h |  1 +
 2 files changed, 37 insertions(+)

diff --git a/src/vec/vec.c b/src/vec/vec.c
index 5fcaab3..af86ae8 100644
--- a/src/vec/vec.c
+++ b/src/vec/vec.c
@@ -56,6 +56,42 @@ M4x4f viewport(f32 x, f32 y, u64 w, u64 h) {
   return output;
 }
 
+M3x3f mat3x3_inv(M3x3f matrix) {
+  M3x3f dest = {0};
+  float det;
+
+  float a = matrix.rows[0].elements[0], b = matrix.rows[0].elements[1],
+        c = matrix.rows[0].elements[2], d = matrix.rows[1].elements[0],
+        e = matrix.rows[1].elements[1], f = matrix.rows[1].elements[2],
+        g = matrix.rows[2].elements[0], h = matrix.rows[2].elements[1],
+        i = matrix.rows[2].elements[2];
+
+  dest.rows[0].elements[0] = e * i - f * h;
+  dest.rows[0].elements[1] = -(b * i - h * c);
+  dest.rows[0].elements[2] = b * f - e * c;
+  dest.rows[1].elements[0] = -(d * i - g * f);
+  dest.rows[1].elements[1] = a * i - c * g;
+  dest.rows[1].elements[2] = -(a * f - d * c);
+  dest.rows[2].elements[0] = d * h - g * e;
+  dest.rows[2].elements[1] = -(a * h - g * b);
+  dest.rows[2].elements[2] = a * e - b * d;
+
+  det = 1.0f / (a * dest.rows[0].elements[0] + b * dest.rows[1].elements[0] +
+                c * dest.rows[2].elements[0]);
+
+  dest.rows[0].elements[0] *= det;
+  dest.rows[0].elements[1] *= det;
+  dest.rows[0].elements[2] *= det;
+  dest.rows[1].elements[0] *= det;
+  dest.rows[1].elements[1] *= det;
+  dest.rows[1].elements[2] *= det;
+  dest.rows[2].elements[0] *= det;
+  dest.rows[2].elements[1] *= det;
+  dest.rows[2].elements[2] *= det;
+
+  return dest;
+}
+
 M4x4f mat4x4_inv(M4x4f matrix) {
   f32 mat[4][4] = mat4x4_to_array(matrix);
   f32 dest[4][4] = {0};
diff --git a/src/vec/vec.h b/src/vec/vec.h
index c94116c..1cce546 100644
--- a/src/vec/vec.h
+++ b/src/vec/vec.h
@@ -293,6 +293,7 @@ MAKE_LIST_TYPE(V2f);
 M4x4f lookat(V3f eye, V3f target, V3f up);
 M4x4f projection(f32 coeff);
 M4x4f viewport(f32 x, f32 y, u64 w, u64 h);
+M3x3f mat3x3_inv(M3x3f matrix);
 M4x4f mat4x4_inv(M4x4f matrix);
 
 #endif // VEC_H