From dac1d159b1cfa7d6cd71236d693b2cab6218ba51 Mon Sep 17 00:00:00 2001 From: jun <83899451+zeichensystem@users.noreply.github.com> Date: Fri, 7 Mar 2025 15:37:55 +0100 Subject: [PATCH] Fix linalg bugs --- src/guf_linalg.h | 109 +++++++++++++++++++++++++++++++++++-- src/test/example.c | 54 ++++++++++++++++++ src/test/guf_linalg_impl.c | 2 +- 3 files changed, 158 insertions(+), 7 deletions(-) diff --git a/src/guf_linalg.h b/src/guf_linalg.h index 41d9952..b07b5f5 100644 --- a/src/guf_linalg.h +++ b/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. - if (!guf_nearly_one_f32(m->data[row][col], eps)) { + } else { // Check 0-entries. + 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 diff --git a/src/test/example.c b/src/test/example.c index 061e420..f81ef31 100644 --- a/src/test/example.c +++ b/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; } diff --git a/src/test/guf_linalg_impl.c b/src/test/guf_linalg_impl.c index abd171f..8debd22 100644 --- a/src/test/guf_linalg_impl.c +++ b/src/test/guf_linalg_impl.c @@ -1,2 +1,2 @@ -#define GUF_MATH_LINALG_IMPL +#define GUF_LINALG_IMPL #include "guf_linalg.h"