Add matrix inversion

src/guf_math_linalg.h

@@ -40,7 +40,7 @@
         float w;       // Scalar (real) part.
     } guf_quaternion;
 
-    // Convention: Tait-Bryan angles, i.e. rotation sequence z - y' - x''
+    // Convention: Tait-Bryan angles
     // (order yaw-pitch-roll; intrinsic rotation; active rotation; right handed coordinate system)
     typedef struct guf_euler_angles {
         float yaw, pitch, roll; 
@@ -82,6 +82,8 @@
         GUF_MATH_LINALG_KWRDS void guf_mat4x4_mul(const guf_mat4x4f *a, const guf_mat4x4f *b, guf_mat4x4f * restrict result);
     #endif
 
+    GUF_MATH_LINALG_KWRDS bool guf_mat4x4_inverted(const guf_mat4x4 *m, guf_mat4x4 *inverted);
+
     #ifdef __cplusplus
         // C++ has no restrict keyword like C99...
         GUF_MATH_LINALG_KWRDS void guf_mat3x3_mul(const guf_mat3x3 *a, const guf_mat3x3 *b, guf_mat3x3 *result);
@@ -99,6 +101,8 @@
 
     GUF_MATH_LINALG_KWRDS void guf_mat4x4_set_rotmat(guf_mat4x4 *m, const guf_mat3x3 *rotmat_3x3);
 
+    GUF_MATH_LINALG_KWRDS bool guf_mat4x4_is_identity(const guf_mat4x4 *m, float eps);
+
     GUF_MATH_LINALG_KWRDS guf_vec3 guf_vec3_transformed(const guf_mat4x4 *mat, const guf_vec3 *v);
     GUF_MATH_LINALG_KWRDS guf_vec4 guf_vec4_transformed(const guf_mat4x4 *mat, const guf_vec4 *v);
 
@@ -224,13 +228,15 @@
     }
 
     GUF_MATH_LINALG_KWRDS guf_quaternion guf_quaternion_from_euler(guf_euler_angles euler);
-    GUF_MATH_LINALG_KWRDS guf_euler_angles guf_quaternion_to_euler(const guf_quaternion *q);
+    GUF_MATH_LINALG_KWRDS guf_euler_angles guf_quaternion_to_euler(guf_quaternion q);
 
 #endif 
 
 
 #if defined(GUF_MATH_LINALG_IMPL) || defined(GUF_MATH_LINALG_IMPL_STATIC)
 
+#include <string.h>
+
 GUF_MATH_LINALG_KWRDS void guf_mat4x4_init_zero(guf_mat4x4 *m)
 {
     GUF_ASSERT(m);
@@ -385,6 +391,132 @@ GUF_MATH_LINALG_KWRDS void guf_mat4x4_set_rotmat(guf_mat4x4 *m, const guf_mat3x3
     }
 }
 
+GUF_MATH_LINALG_KWRDS bool guf_mat4x4_is_identity(const guf_mat4x4 *m, float eps)
+{
+    for (int row = 0; row < 4; ++row) {
+        for (int col = 0; col < 4; ++col) {
+            if (row == col) { // Check 1 entries.
+                if (fabs(m->data[row][col] - 1) > eps) { 
+                    return false;
+                }
+            } else { // Check 0 entries.
+                if (fabs(m->data[row][col]) > eps) {
+                    return false;
+                }
+            }
+        }
+    }
+    return true;
+}
+
+static void guf_mat4x4_swap_rows(guf_mat4x4 *m, int row_a_idx, int row_b_idx)
+{
+    GUF_ASSERT(row_a_idx >= 0 && row_a_idx < 4);
+    GUF_ASSERT(row_b_idx >= 0 && row_b_idx < 4);
+
+    if (row_a_idx == row_b_idx) {
+        return;
+    }
+    
+    const size_t ROW_BYTES = sizeof(m->data[0]);
+    float row_a_cpy[4];
+    memcpy(row_a_cpy, m->data + row_a_idx, ROW_BYTES);           // row_a_cpy := row_a
+    memcpy(m->data + row_a_idx, m->data + row_b_idx, ROW_BYTES); // row_a := row_b
+    memcpy(m->data + row_b_idx, row_a_cpy, ROW_BYTES);           // row_b := row_a_cpy
+}
+
+/* 
+    Find the inverse of a 4x4 matrix using gaussian elimination (if it is invertible, returns true, false otherwise).
+    cf. https://en.wikipedia.org/wiki/Gaussian_elimination#Finding_the_inverse_of_a_matrix (last-retrieved 2025-03-07)
+    Note: In the special case of square orthonormal matrices, the matrix inverse can be calculated more efficiently
+          and accurately using matrix transposition (e.g. valid 3x3 rotation matrices can be inverted by transposing them).
+*/
+GUF_MATH_LINALG_KWRDS bool guf_mat4x4_inverted(const guf_mat4x4 *m, guf_mat4x4 *inverted)
+{
+    GUF_ASSERT(m && inverted);
+    
+    guf_mat4x4_init_identity(inverted); // This matrix starts as the identity matrix and will be transfomed into the inverse.
+    guf_mat4x4 rref = *m; // This initial matrix will be transformed into the identity matrix if m is invertible.
+
+    const float EPS = 1e-6f;
+    const float EPS_ASSERT = 0.05f;
+
+    #define guf_nearly_zero(x, eps) (fabsf((x)) < (eps))
+    #define guf_nearly_one(x, eps)  (fabsf((x) - 1) < (eps))
+
+    for (int row_start = 0; row_start < 3; ++row_start) { // 1.) Try to bring into row echelon form.
+        // Find pivot, i.e. the element with row in range [row_start, 3] which has
+        // the the largest value in the current_column (current_column = row_start)
+        int first_nonzero_row = -1;
+        float largest = -INFINITY;
+        for (int row = row_start; row < 4; ++row) {
+            const float candidate = fabsf(rref.data[row][row_start]);
+            if (!guf_nearly_zero(candidate, EPS) && candidate > largest) {
+                largest = candidate;
+                first_nonzero_row = row;
+            }   
+        }
+        if (first_nonzero_row == -1) {
+            GUF_ASSERT(largest == -INFINITY);
+            GUF_ASSERT(!guf_mat4x4_is_identity(&rref, EPS));
+            return false;
+        }
+        GUF_ASSERT(largest > 0);
+
+        // Swap the found pivot-row with the row at row_start.
+        guf_mat4x4_swap_rows(&rref, row_start, first_nonzero_row);
+        guf_mat4x4_swap_rows(inverted, row_start, first_nonzero_row);
+        const int pivot_row = row_start;
+        const int pivot_col = row_start;
+        const float pivot_val = rref.data[pivot_row][pivot_col];
+        GUF_ASSERT(!guf_nearly_zero(pivot_val, EPS));
+
+        for (int row = pivot_row + 1; row < 4; ++row) { // Make all values below the pivot element zero. 
+            const float val = rref.data[row][pivot_col]; // val: the current value we want to turn into zero.
+            if (guf_nearly_zero(val, EPS)) {
+                rref.data[row][pivot_col] = 0;
+                continue;
+            }
+            for (int col = 0; col < 4; ++col) {
+                // pivot_val * scale = -val <=> scale = -val / pivot_val
+                if (col >= pivot_col) {
+                    rref.data[row][col] += rref.data[pivot_row][col] * -val / pivot_val; // Scale pivot_row and add to row
+                } else {
+                    GUF_ASSERT(rref.data[pivot_row][col] == 0);
+                    GUF_ASSERT(rref.data[row][col] == 0);
+                }
+                inverted->data[row][col] += inverted->data[pivot_row][col] * -val / pivot_val;
+            }
+            rref.data[row][pivot_col] = 0; // += rref.data[pivot_row][pivot_col] * -rref.data[row][pivot_col] / pivot_val
+        }
+    }
+    
+    for (int i = 3; i >= 0; --i) { // 2.) Try to bring into *reduced* row echelon form.
+        if (guf_nearly_zero(rref.data[i][i], EPS)) {
+            return false;
+        } 
+        for (int col = 0; col < 4; ++col) { // Scale row_i to make elem[i][i] 1
+            inverted->data[i][col] *= 1.f / rref.data[i][i];
+            GUF_ASSERT((col != i) ? rref.data[i][col] == 0 : rref.data[i][col] != 0);
+        }
+        GUF_ASSERT(guf_nearly_one(rref.data[i][i], EPS_ASSERT));
+        rref.data[i][i] = 1; // *= 1.f / rref.data[i][i] (== 1)
+
+        for (int row = i - 1; row >= 0; --row) { // Make all elements in column above rref.data[i][i] zero (by adding a scaled row_i to row).
+            for (int col = 0; col < 4; ++col) {
+                inverted->data[row][col] += inverted->data[i][col] * -rref.data[row][i];
+            }
+            GUF_ASSERT(guf_nearly_zero(rref.data[row][i], EPS_ASSERT));
+            rref.data[row][i] = 0; // += rref.data[i][i] * -rref.data[row][i] (== 0)
+        }
+    }
+
+    GUF_ASSERT(guf_mat4x4_is_identity(&rref, EPS));
+    (void)EPS_ASSERT;
+    return true;
+    #undef guf_nearly_zero
+}
+
 // Assumes the last row of mat is [0, 0, 0, 1] and v->w implicitly is 1 (affine transformation)
 GUF_MATH_LINALG_KWRDS guf_vec3 guf_vec3_transformed(const guf_mat4x4 *mat, const guf_vec3 *v) 
 {
@@ -450,7 +582,10 @@ GUF_MATH_LINALG_KWRDS guf_vec3 guf_vec4_to_vec3(guf_vec4 v)
     }
 };
 
-// cf. https://danceswithcode.net/engineeringnotes/quaternions/quaternions.html (last retrieved 2025-03-06)
+/* 
+    (Unit)quaternion operations: 
+    cf. https://danceswithcode.net/engineeringnotes/quaternions/quaternions.html (last-retrieved 2025-03-06)
+*/
 
 GUF_MATH_LINALG_KWRDS guf_quaternion guf_quaternion_from_axis_angle(float angle, guf_vec3 unit_axis)
 {
@@ -474,11 +609,11 @@ GUF_MATH_LINALG_KWRDS guf_quaternion guf_quaternion_from_euler(guf_euler_angles
         .x = sinf(r) * cosf(p) * cosf(y) - cosf(r) * sinf(p) * sinf(y),
         .y = cosf(r) * sinf(p) * cosf(y) + sinf(r) * cosf(p) * sinf(y),
         .z = cosf(r) * cosf(p) * sinf(y) + sinf(r) * sinf(p) * cosf(y)
-    }
+    };
     return q;
 }
 
-GUF_MATH_LINALG_KWRDS guf_euler_angles guf_quaternion_to_euler(const guf_quaternion *q)
+GUF_MATH_LINALG_KWRDS guf_euler_angles guf_quaternion_to_euler(guf_quaternion q)
 {
     guf_euler_angles euler = {0};
     euler.pitch = asinf(2 * (q.w * q.y - q.x * q.z));
@@ -596,7 +731,7 @@ GUF_MATH_LINALG_KWRDS guf_vec3 *guf_vec3_rotate_by_quat(guf_vec3 *p, const guf_q
     // Active rotation of the point/vector p by the quaternion q. 
     GUF_ASSERT(p && q);
     const guf_quaternion inv_q = guf_quaternion_inverted(*q);
-    guf_quaternion p_quat = {.w = 0, .x = p.x, .y = p.y, .z = p.z};
+    guf_quaternion p_quat = {.w = 0, .x = p->x, .y = p->y, .z = p->z};
 
     // (x) active rotation: inv(q) * p_quat * q  (the point is rotated with respect to the coordinate system)
     // ( ) passive rotation: q * p_quat * inv(q) (the coordinate system is rotated with respect to the point)
@@ -604,9 +739,9 @@ GUF_MATH_LINALG_KWRDS guf_vec3 *guf_vec3_rotate_by_quat(guf_vec3 *p, const guf_q
     guf_quaternion_mul(&inv_q, &p_quat, &tmp); // tmp := inv(q) * pquat
     guf_quaternion_mul(&tmp, q, &p_quat);      // pquat := tmp * q 
 
-    p->x = p_quat->x; 
+    p->x = p_quat.x; 
-    p->y = p_quat->y; 
+    p->y = p_quat.y; 
-    p->z = p_quat->z;
+    p->z = p_quat.z;
     return p;
 }
 

src/test/guf_dict_impl.h

@@ -5,6 +5,8 @@
 #include "guf_cstr.h"
 #include "guf_str.h"
 
+#include "guf_hash.h"
+
 #define GUF_DICT_KEY_T guf_cstr_const
 #define GUF_DICT_KEY_HASH guf_cstr_const_hash
 #define GUF_DICT_KEY_T_EQ guf_cstr_const_eq