Fix linalg bugs

src/guf_linalg.h

@@ -102,6 +102,10 @@
     GUF_LINALG_KWRDS void guf_mat4x4_set_rotmat(guf_mat4x4 *m, const guf_mat3x3 *rotmat_3x3);
 
     GUF_LINALG_KWRDS bool guf_mat4x4_is_identity(const guf_mat4x4 *m, float eps);
+    GUF_LINALG_KWRDS bool guf_mat3x3_is_identity(const guf_mat3x3 *m, float eps);
+
+    GUF_LINALG_KWRDS bool guf_mat4x4_nearly_equal(const guf_mat4x4 *a, const guf_mat4x4 *b, float rel_tol, float abs_tol);
+    GUF_LINALG_KWRDS bool guf_mat3x3_nearly_equal(const guf_mat3x3 *a, const guf_mat3x3 *b, float rel_tol, float abs_tol);
 
     GUF_LINALG_KWRDS guf_vec3 guf_vec3_transformed(const guf_mat4x4 *mat, const guf_vec3 *v);
     GUF_LINALG_KWRDS guf_vec4 guf_vec4_transformed(const guf_mat4x4 *mat, const guf_vec4 *v);
@@ -227,12 +231,15 @@
         return *guf_vec3_rotate_by_quat(&p, q);
     }
 
+    GUF_LINALG_KWRDS guf_quaternion guf_quaternion_from_axis_angle(float angle, guf_vec3 unit_axis);
+
     GUF_LINALG_KWRDS guf_quaternion guf_quaternion_from_euler(guf_euler_angles euler);
     GUF_LINALG_KWRDS guf_euler_angles guf_quaternion_to_euler(guf_quaternion q);
 
+    GUF_LINALG_KWRDS void guf_mat4x4_print(const guf_mat4x4 *m, FILE *outfile);
+    GUF_LINALG_KWRDS void guf_mat4x4_print_with_precision(const guf_mat4x4 *m, FILE *outfile, int num_frac_digits);
 #endif 
 
-#define GUF_LINALG_IMPL_STATIC /* debug */
 
 #if defined(GUF_LINALG_IMPL) || defined(GUF_LINALG_IMPL_STATIC)
 
@@ -392,16 +399,34 @@ GUF_LINALG_KWRDS void guf_mat4x4_set_rotmat(guf_mat4x4 *m, const guf_mat3x3 *rot
     }
 }
 
+GUF_LINALG_KWRDS bool guf_mat3x3_is_identity(const guf_mat3x3 *m, float eps)
+{
+    for (int row = 0; row < 3; ++row) {
+        for (int col = 0; col < 3; ++col) {
+            if (row == col) { // Check 1-entries.
+                if (!guf_nearly_one_f32(m->data[row][col], eps)) { 
+                    return false;
+                }
+            } else { // Check 0-entries.
+                if (!guf_nearly_zero_f32(m->data[row][col], eps)) {
+                    return false;
+                }
+            }
+        }
+    }
+    return true;
+}
+
 GUF_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 (row == col) { // Check 1-entries.
                 if (!guf_nearly_one_f32(m->data[row][col], eps)) { 
                     return false;
                 }
-            } else { // Check 0 entries.
+            } else { // Check 0-entries.
-                if (!guf_nearly_one_f32(m->data[row][col], eps)) {
+                if (!guf_nearly_zero_f32(m->data[row][col], eps)) {
                     return false;
                 }
             }
@@ -410,6 +435,32 @@ GUF_LINALG_KWRDS bool guf_mat4x4_is_identity(const guf_mat4x4 *m, float eps)
     return true;
 }
 
+GUF_LINALG_KWRDS bool guf_mat3x3_nearly_equal(const guf_mat3x3 *a, const guf_mat3x3 *b, float rel_tol, float abs_tol)
+{
+    GUF_ASSERT(a && b);
+    for (int row = 0; row < 3; ++row) {
+        for (int col = 0; col < 3; ++col) {
+            if (!guf_isclose_f32(a->data[row][col], b->data[row][col], rel_tol, abs_tol)) {
+                return false;
+            }
+        }
+    }
+    return true;
+}
+
+GUF_LINALG_KWRDS bool guf_mat4x4_nearly_equal(const guf_mat4x4 *a, const guf_mat4x4 *b, float rel_tol, float abs_tol)
+{
+    GUF_ASSERT(a && b);
+    for (int row = 0; row < 4; ++row) {
+        for (int col = 0; col < 4; ++col) {
+            if (!guf_isclose_f32(a->data[row][col], b->data[row][col], rel_tol, abs_tol)) {
+                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);
@@ -497,14 +548,12 @@ GUF_LINALG_KWRDS bool guf_mat4x4_inverted(const guf_mat4x4 *m, guf_mat4x4 *inver
             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_f32(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_f32(rref.data[row][i], EPS_ASSERT));
             rref.data[row][i] = 0; // += rref.data[i][i] * -rref.data[row][i] (== 0)
         }
     }
@@ -742,6 +791,54 @@ GUF_LINALG_KWRDS guf_vec3 *guf_vec3_rotate_by_quat(guf_vec3 *p, const guf_quater
     return p;
 }
 
+GUF_LINALG_KWRDS void guf_mat4x4_print_with_precision(const guf_mat4x4 *m, FILE *outfile, int num_frac_digits)
+{
+    if (!outfile) {
+        outfile = stdout;
+    }
+    if (!m) {
+        fprintf(outfile, "guf_mat4x4 NULL\n");
+        return;
+    }
+
+    int max_digits = 1;
+    for (int row = 0; row < 4; ++row) { // Find how many digits before the . we need.
+        for (int col = 0; col < 4; ++col) {
+            const float elem = m->data[row][col];
+            int add_digits = 0;
+            if (isnan(elem) || isinf(elem)) {
+                add_digits += 3;
+            }
+            if (elem == 0) {
+                continue;
+            }
+            const float elem_log10 = floorf(log10f(fabsf(elem)));
+            int digits = (int)elem_log10 + add_digits;
+            digits = GUF_CLAMP(digits, 0, 32);
+            if (digits > max_digits) {
+                max_digits = digits;
+            }
+        }
+    }
+    const int whole_digits = max_digits;
+    num_frac_digits = GUF_CLAMP(num_frac_digits, 0, 32);
+
+    for (int row = 0; row < 4; ++row) {
+        for (int col = 0; col < 4; ++col) {
+            fprintf(outfile, "% *.*f ", whole_digits, num_frac_digits, (double)m->data[row][col]);
+        }
+        fputc('\n', outfile);
+    }
+    fputc('\n', outfile);
+}
+
+
+GUF_LINALG_KWRDS void guf_mat4x4_print(const guf_mat4x4 *m, FILE *outfile)
+{
+    const int DEFAULT_FRAC_DIGITS = 3;
+    guf_mat4x4_print_with_precision(m, outfile, DEFAULT_FRAC_DIGITS);
+}
+
 #undef GUF_LINALG_IMPL
 #undef GUF_LINALG_IMPL_STATIC
 

src/test/example.c

@@ -7,6 +7,7 @@
 
 #include "guf_alloc_libc.h"
 #include "guf_cstr.h"
+#include "guf_linalg.h"
 
 #define GUF_T float
 #define GUF_SORT_IMPL_STATIC
@@ -235,5 +236,58 @@ int main(void)
         puts("");
     }
 
+
+    for (float angle = 0; angle <= 8.f * GUF_PI_F32; angle += 0.001f) {
+        guf_quaternion rotq = guf_quaternion_from_axis_angle(angle, guf_vec3_normalised((guf_vec3){-2324234.3f, 1.4f, -1.3f}));
+        guf_mat4x4 rotmat, rotmat_inv;
+        guf_mat4x4_init_from_quaternion(&rotmat, rotq);
+        GUF_ASSERT(guf_mat4x4_inverted(&rotmat, &rotmat_inv))
+
+        guf_mat4x4_set_trans(&rotmat, (guf_vec3){42.1234f, -512.2f, 3.1415926f});
+        GUF_ASSERT(guf_mat4x4_inverted(&rotmat, &rotmat_inv));
+
+        GUF_ASSERT(guf_mat4x4_inverted(&rotmat_inv, &rotmat));
+        GUF_ASSERT(guf_mat4x4_inverted(&rotmat, &rotmat_inv));
+    }
+
+    guf_quaternion q = guf_quaternion_from_axis_angle(GUF_PI_F32 / 8.f, guf_vec3_normalised((guf_vec3){0.3f, 10.2f, -25.f}));
+
+    guf_mat4x4 mat;
+    guf_mat4x4_init_from_quaternion(&mat, q);
+    guf_mat4x4_set_trans(&mat, (guf_vec3){42.1234f, -512.2f, 3.1415926f});
+
+    const guf_mat4x4 mat_cpy = mat;
+
+    printf("Matrix:\n");
+    guf_mat4x4_print_with_precision(&mat, stdout, 8);
+
+    guf_mat4x4 mat_inv;
+    bool invertible = guf_mat4x4_inverted(&mat, &mat_inv);
+    if (!invertible) {
+        printf("Not invertible\n");
+    } else {
+        printf("Inverse:\n");
+        guf_mat4x4_print_with_precision(&mat_inv, stdout, 8);
+        GUF_ASSERT(guf_mat4x4_inverted(&mat_inv, &mat));
+        GUF_ASSERT(guf_mat4x4_nearly_equal(&mat, &mat_cpy, 1e-3f, 1e-4f));
+    }
+
+
+    mat = (guf_mat4x4) {.data = {
+        {1, 1.3f, 1, 1},
+        {2, 2.6f, 2, 2},
+        {0, 0, 2, 4},
+        {0, 0, 0, 1}
+    }};
+    printf("Matrix:\n");
+    guf_mat4x4_print_with_precision(&mat, stdout, 8);
+    invertible = guf_mat4x4_inverted(&mat, &mat_inv);
+    if (!invertible) {
+        printf("Not invertible\n");
+    } else {
+        printf("Inverse:\n");
+        guf_mat4x4_print_with_precision(&mat_inv, stdout, 8);
+    }
+
     return EXIT_SUCCESS;
 }

src/test/guf_linalg_impl.c

@@ -1,2 +1,2 @@
-#define GUF_MATH_LINALG_IMPL
+#define GUF_LINALG_IMPL
 #include "guf_linalg.h"