fruit/libguf
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add matrix inversion

Browse Source
This commit is contained in:
jun 2025-03-06 21:35:27 +01:00
parent 364dd603cf
commit 7b433cd776
2 changed files with 146 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

153
src/guf_math_linalg.h
View File

@ -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->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;
}

2
src/test/guf_dict_impl.h
View File

@ -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