From 7b433cd776256b88f8908761b4108c38b67157d1 Mon Sep 17 00:00:00 2001 From: jun <83899451+zeichensystem@users.noreply.github.com> Date: Thu, 6 Mar 2025 21:35:27 +0100 Subject: [PATCH] Add matrix inversion --- src/guf_math_linalg.h | 153 ++++++++++++++++++++++++++++++++++++--- src/test/guf_dict_impl.h | 2 + 2 files changed, 146 insertions(+), 9 deletions(-) diff --git a/src/guf_math_linalg.h b/src/guf_math_linalg.h index ca6a31d..e47ee24 100644 --- a/src/guf_math_linalg.h +++ b/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 + 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->y = p_quat->y; - p->z = p_quat->z; + p->x = p_quat.x; + p->y = p_quat.y; + p->z = p_quat.z; return p; } diff --git a/src/test/guf_dict_impl.h b/src/test/guf_dict_impl.h index cf4407a..27a21fe 100644 --- a/src/test/guf_dict_impl.h +++ b/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