848 lines
29 KiB
C
848 lines
29 KiB
C
#if defined(GUF_LINALG_IMPL_STATIC)
|
||
#define GUF_LINALG_KWRDS static
|
||
#else
|
||
#define GUF_LINALG_KWRDS
|
||
#endif
|
||
|
||
#ifndef GUF_LINALG_H
|
||
#define GUF_LINALG_H
|
||
#include "guf_assert.h"
|
||
#include "guf_math.h"
|
||
|
||
#define GUF_QUAT_TO_EULER_EPS (1e-6f)
|
||
#define GUF_QUAT_TO_AXIS_ANGLE_EPS (1e-6f)
|
||
// #define GUF_QUAT_NORMALISE_EPS (1e-6f)
|
||
|
||
typedef struct guf_vec2 {
|
||
float x, y;
|
||
} guf_vec2;
|
||
|
||
typedef struct guf_vec3 {
|
||
float x, y, z;
|
||
} guf_vec3;
|
||
|
||
typedef struct guf_vec4 {
|
||
float x, y, z, w;
|
||
} guf_vec4;
|
||
|
||
// Row-major-matrix (vectors "are" column-vectors; correct multiplication order: mat * vec)
|
||
// (e.g. the x-basis vector is (guf_vec4f){.x = m[0][0], .y = m[1][0], .z = m[2][0], .w = m[3][0]})
|
||
typedef struct guf_mat4x4 {
|
||
float data[4][4];
|
||
} guf_mat4x4;
|
||
|
||
typedef struct guf_mat3x3 {
|
||
float data[3][3];
|
||
} guf_mat3x3;
|
||
|
||
typedef struct guf_quaternion {
|
||
float x, y, z; // Vectorial (imaginary) part.
|
||
float w; // Scalar (real) part.
|
||
} guf_quaternion;
|
||
|
||
// 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;
|
||
} guf_euler_angles;
|
||
|
||
typedef struct guf_axis_angle {
|
||
guf_vec3 axis;
|
||
float angle;
|
||
} guf_axis_angle;
|
||
|
||
GUF_LINALG_KWRDS void guf_mat4x4_init_zero(guf_mat4x4 *m);
|
||
GUF_LINALG_KWRDS void guf_mat4x4_init_identity(guf_mat4x4 *m);
|
||
GUF_LINALG_KWRDS void guf_mat4x4_init_with_basis(guf_mat4x4 *m, guf_vec3 x_basis, guf_vec3 y_basis, guf_vec3 z_basis);
|
||
GUF_LINALG_KWRDS void guf_mat4x4_init_from_quaternion(guf_mat4x4 *rotmat, guf_quaternion q);
|
||
|
||
GUF_LINALG_KWRDS void guf_mat3x3_init_zero(guf_mat3x3 *m);
|
||
GUF_LINALG_KWRDS void guf_mat3x3_init_identity(guf_mat3x3 *m);
|
||
GUF_LINALG_KWRDS void guf_mat3x3_init_with_basis(guf_mat3x3 *m, guf_vec3 x_basis, guf_vec3 y_basis, guf_vec3 z_basis);
|
||
GUF_LINALG_KWRDS void guf_mat3x3_init_from_quaternion(guf_mat3x3 *rotmat, guf_quaternion q);
|
||
|
||
#ifdef __cplusplus
|
||
// C++ has no restrict keyword like C99...
|
||
GUF_LINALG_KWRDS void guf_mat4x4_transposed(const guf_mat4x4 *m, guf_mat4x4 *transposed);
|
||
#else
|
||
GUF_LINALG_KWRDS void guf_mat4x4_transposed(const guf_mat4x4 *m, guf_mat4x4 * restrict transposed);
|
||
#endif
|
||
|
||
#ifdef __cplusplus
|
||
// C++ has no restrict keyword like C99...
|
||
GUF_LINALG_KWRDS void guf_mat3x3_transposed(const guf_mat3x3 *m, guf_mat3x3 *transposed);
|
||
#else
|
||
GUF_LINALG_KWRDS void guf_mat3x3_transposed(const guf_mat3x3 *m, guf_mat3x3 * restrict transposed);
|
||
#endif
|
||
|
||
#ifdef __cplusplus
|
||
// C++ has no restrict keyword like C99...
|
||
GUF_LINALG_KWRDS void guf_mat4x4_mul(const guf_mat4x4 *a, const guf_mat4x4 *b, guf_mat4x4 *result);
|
||
#else
|
||
GUF_LINALG_KWRDS void guf_mat4x4_mul(const guf_mat4x4 *a, const guf_mat4x4 *b, guf_mat4x4 * restrict result);
|
||
#endif
|
||
|
||
GUF_LINALG_KWRDS bool guf_mat4x4_inverted(const guf_mat4x4 *m, guf_mat4x4 *inverted);
|
||
|
||
#ifdef __cplusplus
|
||
// C++ has no restrict keyword like C99...
|
||
GUF_LINALG_KWRDS void guf_mat3x3_mul(const guf_mat3x3 *a, const guf_mat3x3 *b, guf_mat3x3 *result);
|
||
#else
|
||
GUF_LINALG_KWRDS void guf_mat3x3_mul(const guf_mat3x3 *a, const guf_mat3x3 *b, guf_mat3x3 * restrict result);
|
||
#endif
|
||
|
||
static inline void guf_mat4x4_set_trans(guf_mat4x4 *m, guf_vec3 trans)
|
||
{
|
||
GUF_ASSERT(m);
|
||
m->data[0][3] = trans.x;
|
||
m->data[1][3] = trans.y;
|
||
m->data[2][3] = trans.z;
|
||
}
|
||
|
||
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);
|
||
|
||
static inline guf_vec2 guf_vec2_add(guf_vec2 a, guf_vec2 b)
|
||
{
|
||
guf_vec2 sum = {
|
||
.x = a.x + b.x,
|
||
.y = a.y + b.y
|
||
};
|
||
return sum;
|
||
}
|
||
static inline guf_vec2 guf_vec2_sub(guf_vec2 a, guf_vec2 b)
|
||
{
|
||
guf_vec2 diff = {
|
||
.x = a.x - b.x,
|
||
.y = a.y - b.y
|
||
};
|
||
return diff;
|
||
}
|
||
static inline guf_vec2 guf_vec2_scaled(guf_vec2 a, float scale)
|
||
{
|
||
guf_vec2 s = {.x = a.x * scale, .y = a.y * scale};
|
||
return s;
|
||
}
|
||
|
||
static inline guf_vec3 guf_vec3_add(guf_vec3 a, guf_vec3 b)
|
||
{
|
||
guf_vec3 sum = {
|
||
.x = a.x + b.x,
|
||
.y = a.y + b.y,
|
||
.z = a.z + b.z
|
||
};
|
||
return sum;
|
||
}
|
||
static inline guf_vec3 guf_vec3_sub(guf_vec3 a, guf_vec3 b)
|
||
{
|
||
guf_vec3 diff = {
|
||
.x = a.x - b.x,
|
||
.y = a.y - b.y,
|
||
.z = a.z - b.z
|
||
};
|
||
return diff;
|
||
}
|
||
static inline guf_vec3 guf_vec3_scaled(guf_vec3 a, float scale)
|
||
{
|
||
guf_vec3 s = {.x = a.x * scale, .y = a.y * scale, .z = a.z * scale};
|
||
return s;
|
||
}
|
||
|
||
static inline float guf_vec2_mag(guf_vec2 v) {return sqrtf(v.x * v.x + v.y * v.y);}
|
||
static inline float guf_vec3_mag(guf_vec3 v) {return sqrtf(v.x * v.x + v.y * v.y + v.z * v.z);}
|
||
static inline float guf_vec2_mag_squared(guf_vec2 v) {return v.x * v.x + v.y * v.y;}
|
||
static inline float guf_vec3_mag_squared(guf_vec3 v) {return v.x * v.x + v.y * v.y + v.z * v.z;}
|
||
|
||
static inline float guf_vec2_dot(guf_vec2 a, guf_vec2 b) {return a.x * b.x + a.y * b.y;}
|
||
static inline float guf_vec3_dot(guf_vec3 a, guf_vec3 b) {return a.x * b.x + a.y * b.y + a.z * b.z;}
|
||
|
||
static inline guf_vec3 guf_vec3_cross(guf_vec3 a, guf_vec3 b)
|
||
{
|
||
guf_vec3 cross = {
|
||
.x = a.y * b.z - a.z * b.y,
|
||
.y = a.z * b.x - a.x * b.z,
|
||
.z = a.x * b.y - a.y * b.x
|
||
};
|
||
return cross;
|
||
}
|
||
static inline float guf_vec2_perpdot(guf_vec2 a, guf_vec2 b)
|
||
{
|
||
return a.x * b.y - a.y * b.x; // The z-component of a 3d cross-product of two vectors in a plane.
|
||
}
|
||
|
||
GUF_LINALG_KWRDS guf_vec2 *guf_vec2_normalise(guf_vec2 *v);
|
||
GUF_LINALG_KWRDS guf_vec3 *guf_vec3_normalise(guf_vec3 *v);
|
||
|
||
static inline guf_vec2 guf_vec2_normalised(guf_vec2 v) {return *guf_vec2_normalise(&v);}
|
||
static inline guf_vec3 guf_vec3_normalised(guf_vec3 v) {return *guf_vec3_normalise(&v);}
|
||
|
||
GUF_LINALG_KWRDS guf_vec3 guf_vec4_to_vec3(guf_vec4 v);
|
||
|
||
static inline void guf_quaternion_init_identity(guf_quaternion *q)
|
||
{
|
||
GUF_ASSERT(q);
|
||
q->w = 1;
|
||
q->x = q->y = q->z = 0;
|
||
}
|
||
|
||
static inline float guf_quaternion_mag(guf_quaternion q)
|
||
{
|
||
return sqrtf(q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z);
|
||
}
|
||
|
||
static inline float guf_quaternion_mag_squared(guf_quaternion q)
|
||
{
|
||
return q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z;
|
||
}
|
||
|
||
GUF_LINALG_KWRDS guf_quaternion *guf_quaternion_normalise(guf_quaternion *q);
|
||
|
||
static inline guf_quaternion guf_quaternion_normalised(guf_quaternion q)
|
||
{
|
||
return *guf_quaternion_normalise(&q);
|
||
}
|
||
|
||
static inline guf_quaternion guf_quaternion_inverted(guf_quaternion q)
|
||
{
|
||
q.x = -q.x;
|
||
q.y = -q.y;
|
||
q.z = -q.z;
|
||
return q;
|
||
}
|
||
|
||
#ifdef __cplusplus
|
||
// C++ has no restrict keyword like C99...
|
||
GUF_LINALG_KWRDS void guf_quaternion_mul(const guf_quaternion *a, const guf_quaternion *b, guf_quaternion *result);
|
||
#else
|
||
GUF_LINALG_KWRDS void guf_quaternion_mul(const guf_quaternion *a, const guf_quaternion *b, guf_quaternion * restrict result);
|
||
#endif
|
||
|
||
GUF_LINALG_KWRDS guf_vec3 *guf_vec3_rotate_by_quat(guf_vec3 *p, const guf_quaternion *q);
|
||
static inline guf_vec3 guf_vec3_rotated_by_quat(guf_vec3 p, const guf_quaternion *q)
|
||
{
|
||
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
|
||
|
||
|
||
#if defined(GUF_LINALG_IMPL) || defined(GUF_LINALG_IMPL_STATIC)
|
||
|
||
#include <string.h>
|
||
|
||
GUF_LINALG_KWRDS void guf_mat4x4_init_zero(guf_mat4x4 *m)
|
||
{
|
||
GUF_ASSERT(m);
|
||
for (int row = 0; row < 4; ++row) {
|
||
for (int col = 0; col < 4; ++col) {
|
||
m->data[row][col] = 0;
|
||
}
|
||
}
|
||
}
|
||
|
||
GUF_LINALG_KWRDS void guf_mat3x3_init_zero(guf_mat3x3 *m)
|
||
{
|
||
GUF_ASSERT(m);
|
||
for (int row = 0; row < 3; ++row) {
|
||
for (int col = 0; col < 3; ++col) {
|
||
m->data[row][col] = 0;
|
||
}
|
||
}
|
||
}
|
||
|
||
GUF_LINALG_KWRDS void guf_mat4x4_init_identity(guf_mat4x4 *m)
|
||
{
|
||
GUF_ASSERT(m);
|
||
guf_mat4x4_init_zero(m);
|
||
for (int i = 0; i < 4; ++i) {
|
||
m->data[i][i] = 1;
|
||
}
|
||
}
|
||
|
||
GUF_LINALG_KWRDS void guf_mat3x3_init_identity(guf_mat3x3 *m)
|
||
{
|
||
GUF_ASSERT(m);
|
||
guf_mat3x3_init_zero(m);
|
||
for (int i = 0; i < 3; ++i) {
|
||
m->data[i][i] = 1;
|
||
}
|
||
}
|
||
|
||
GUF_LINALG_KWRDS void guf_mat4x4_init_with_basis(guf_mat4x4 *m, guf_vec3 x_basis, guf_vec3 y_basis, guf_vec3 z_basis)
|
||
{
|
||
GUF_ASSERT(m);
|
||
|
||
m->data[0][0] = x_basis.x;
|
||
m->data[1][0] = x_basis.y;
|
||
m->data[2][0] = x_basis.z;
|
||
m->data[3][0] = 0;
|
||
|
||
m->data[0][1] = y_basis.x;
|
||
m->data[1][1] = y_basis.y;
|
||
m->data[2][1] = y_basis.z;
|
||
m->data[3][1] = 0;
|
||
|
||
m->data[0][2] = z_basis.x;
|
||
m->data[1][2] = z_basis.y;
|
||
m->data[2][2] = z_basis.z;
|
||
m->data[3][2] = 0;
|
||
|
||
m->data[0][3] = m->data[1][3] = m->data[2][3] = 0;
|
||
m->data[3][3] = 1;
|
||
}
|
||
|
||
GUF_LINALG_KWRDS void guf_mat3x3_init_with_basis(guf_mat3x3 *m, guf_vec3 x_basis, guf_vec3 y_basis, guf_vec3 z_basis)
|
||
{
|
||
GUF_ASSERT(m);
|
||
|
||
m->data[0][0] = x_basis.x;
|
||
m->data[1][0] = x_basis.y;
|
||
m->data[2][0] = x_basis.z;
|
||
|
||
m->data[0][1] = y_basis.x;
|
||
m->data[1][1] = y_basis.y;
|
||
m->data[2][1] = y_basis.z;
|
||
|
||
m->data[0][2] = z_basis.x;
|
||
m->data[1][2] = z_basis.y;
|
||
m->data[2][2] = z_basis.z;
|
||
}
|
||
|
||
#if defined(__cplusplus) && defined(GUF_LINALG_IMPL_STATIC)
|
||
GUF_LINALG_KWRDS void guf_mat4x4_mul(const guf_mat4x4 *a, const guf_mat4x4 *b, guf_mat4x4 *result)
|
||
#else
|
||
GUF_LINALG_KWRDS void guf_mat4x4_mul(const guf_mat4x4 *a, const guf_mat4x4 *b, guf_mat4x4 * restrict result)
|
||
#endif
|
||
{
|
||
GUF_ASSERT(a && b && result);
|
||
GUF_ASSERT(a != result && b != result);
|
||
for (int i = 0; i < 4; ++i) {
|
||
for (int j = 0; j < 4; ++j) {
|
||
result->data[j][i] = 0;
|
||
for (int k = 0; k < 4; ++k) {
|
||
result->data[j][i] += a->data[j][k] * b->data[k][i];
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
#if defined(__cplusplus) && defined(GUF_LINALG_IMPL_STATIC)
|
||
GUF_LINALG_KWRDS void guf_mat3x3_mul(const guf_mat3x3 *a, const guf_mat3x3 *b, guf_mat3x3 *result)
|
||
#else
|
||
GUF_LINALG_KWRDS void guf_mat3x3_mul(const guf_mat3x3 *a, const guf_mat3x3 *b, guf_mat3x3 * restrict result)
|
||
#endif
|
||
{
|
||
GUF_ASSERT(a && b && result);
|
||
GUF_ASSERT(a != result && b != result);
|
||
for (int i = 0; i < 3; ++i) {
|
||
for (int j = 0; j < 3; ++j) {
|
||
result->data[j][i] = 0;
|
||
for (int k = 0; k < 3; ++k) {
|
||
result->data[j][i] += a->data[j][k] * b->data[k][i];
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
#if defined(__cplusplus) && defined(GUF_LINALG_IMPL_STATIC)
|
||
GUF_LINALG_KWRDS void guf_mat4x4_transposed(const guf_mat4x4 *m, guf_mat4x4 *transposed)
|
||
#else
|
||
GUF_LINALG_KWRDS void guf_mat4x4_transposed(const guf_mat4x4 *m, guf_mat4x4 * restrict transposed)
|
||
#endif
|
||
{
|
||
GUF_ASSERT(m && transposed);
|
||
GUF_ASSERT(m != transposed);
|
||
for (int i = 0; i < 4; ++i) {
|
||
for (int j = 0; j < 4; ++j) {
|
||
transposed->data[i][j] = m->data[j][i];
|
||
}
|
||
}
|
||
}
|
||
|
||
#if defined(__cplusplus) && defined(GUF_LINALG_IMPL_STATIC)
|
||
GUF_LINALG_KWRDS void guf_mat3x3_transposed(const guf_mat3x3 *m, guf_mat3x3 *transposed)
|
||
#else
|
||
GUF_LINALG_KWRDS void guf_mat3x3_transposed(const guf_mat3x3 *m, guf_mat3x3 * restrict transposed)
|
||
#endif
|
||
{
|
||
GUF_ASSERT(m && transposed);
|
||
GUF_ASSERT(m != transposed);
|
||
for (int i = 0; i < 3; ++i) {
|
||
for (int j = 0; j < 3; ++j) {
|
||
transposed->data[i][j] = m->data[j][i];
|
||
}
|
||
}
|
||
}
|
||
|
||
GUF_LINALG_KWRDS void guf_mat4x4_set_rotmat(guf_mat4x4 *m, const guf_mat3x3 *rotmat_3x3)
|
||
{
|
||
GUF_ASSERT(m && rotmat_3x3)
|
||
for (int row = 0; row < 3; ++row) {
|
||
for (int col = 0; col < 3; ++col) {
|
||
m->data[row][col] = rotmat_3x3->data[row][col];
|
||
}
|
||
}
|
||
}
|
||
|
||
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 (!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_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);
|
||
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_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;
|
||
|
||
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_f32(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_f32(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_f32(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_f32(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);
|
||
}
|
||
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];
|
||
}
|
||
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;
|
||
}
|
||
|
||
// Assumes the last row of mat is [0, 0, 0, 1] and v->w implicitly is 1 (affine transformation)
|
||
GUF_LINALG_KWRDS guf_vec3 guf_vec3_transformed(const guf_mat4x4 *mat, const guf_vec3 *v)
|
||
{
|
||
GUF_ASSERT(mat && v);
|
||
guf_vec3 v_transformed = {
|
||
.x = mat->data[0][0] * v->x + mat->data[0][1] * v->y + mat->data[0][2] * v->z + mat->data[0][3],
|
||
.y = mat->data[1][0] * v->x + mat->data[1][1] * v->y + mat->data[1][2] * v->z + mat->data[1][3],
|
||
.z = mat->data[2][0] * v->x + mat->data[2][1] * v->y + mat->data[2][2] * v->z + mat->data[2][3]
|
||
};
|
||
return v_transformed;
|
||
}
|
||
|
||
GUF_LINALG_KWRDS guf_vec4 guf_vec4_transformed(const guf_mat4x4 *mat, const guf_vec4 *v)
|
||
{
|
||
GUF_ASSERT(mat && v);
|
||
guf_vec4 v_transformed = {
|
||
.x = mat->data[0][0] * v->x + mat->data[0][1] * v->y + mat->data[0][2] * v->z + v->w * mat->data[0][3],
|
||
.y = mat->data[1][0] * v->x + mat->data[1][1] * v->y + mat->data[1][2] * v->z + v->w * mat->data[1][3],
|
||
.z = mat->data[2][0] * v->x + mat->data[2][1] * v->y + mat->data[2][2] * v->z + v->w * mat->data[2][3],
|
||
.w = mat->data[3][0] * v->x + mat->data[3][1] * v->y + mat->data[3][2] * v->z + v->w * mat->data[3][3],
|
||
};
|
||
return v_transformed;
|
||
}
|
||
|
||
GUF_LINALG_KWRDS guf_vec2 *guf_vec2_normalise(guf_vec2 *v)
|
||
{
|
||
GUF_ASSERT(v);
|
||
const float mag = guf_vec2_mag(*v);
|
||
if (mag == 0 || isnan(mag)) {
|
||
v->x = v->y = (float)INFINITY;
|
||
} else {
|
||
v->x /= mag;
|
||
v->y /= mag;
|
||
}
|
||
return v;
|
||
}
|
||
|
||
GUF_LINALG_KWRDS guf_vec3 *guf_vec3_normalise(guf_vec3 *v)
|
||
{
|
||
GUF_ASSERT(v);
|
||
const float mag = guf_vec3_mag(*v);
|
||
if (mag == 0 || isnan(mag)) {
|
||
v->x = v->y = v->z = (float)INFINITY;
|
||
} else {
|
||
v->x /= mag;
|
||
v->y /= mag;
|
||
v->z /= mag;
|
||
}
|
||
return v;
|
||
}
|
||
|
||
GUF_LINALG_KWRDS guf_vec3 guf_vec4_to_vec3(guf_vec4 v)
|
||
{
|
||
if (guf_isclose_abstol_f32(v.w, 0, 1e-21f)) { // If w is extremely close 0, we treat it as zero.
|
||
guf_vec3 inf_vec = {(float)INFINITY, (float)INFINITY, (float)INFINITY};
|
||
return inf_vec;
|
||
} else if (guf_isclose_reltol_f32(v.w, 1, 1e-9f)) { // If w is almost 1, we treat it as exactly 1.
|
||
guf_vec3 vec = {.x = v.x, .y = v.y, .z = v.z};
|
||
return vec;
|
||
} else {
|
||
guf_vec3 vec = {.x = v.x / v.w, .y = v.y / v.w, .z = v.z / v.w};
|
||
return vec;
|
||
}
|
||
}
|
||
|
||
/*
|
||
(Unit)quaternion operations:
|
||
cf. https://danceswithcode.net/engineeringnotes/quaternions/quaternions.html (last-retrieved 2025-03-06)
|
||
*/
|
||
|
||
GUF_LINALG_KWRDS guf_quaternion guf_quaternion_from_axis_angle(float angle, guf_vec3 unit_axis)
|
||
{
|
||
const float sin_halfangle = sinf(angle / 2.f);
|
||
guf_quaternion q = {
|
||
.w = cosf(angle / 2.f),
|
||
.x = unit_axis.x * sin_halfangle,
|
||
.y = unit_axis.y * sin_halfangle,
|
||
.z = unit_axis.z * sin_halfangle,
|
||
};
|
||
return q;
|
||
}
|
||
|
||
GUF_LINALG_KWRDS guf_quaternion guf_quaternion_from_euler(guf_euler_angles euler)
|
||
{
|
||
const float r = euler.roll / 2;
|
||
const float p = euler.pitch / 2;
|
||
const float y = euler.yaw / 2;
|
||
guf_quaternion q = {
|
||
.w = cosf(r) * cosf(p) * cosf(y) + sinf(r) * sinf(p) * sinf(y),
|
||
.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_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));
|
||
|
||
const float half_pi = GUF_PI_F32 / 2;
|
||
if (fabsf(euler.pitch - half_pi) <= GUF_QUAT_TO_EULER_EPS) { // Gimbal lock: pitch == pi/2
|
||
euler.roll = 0;
|
||
euler.yaw = -2 * atan2f(q.x, q.w);
|
||
} else if (fabsf(euler.pitch - (-half_pi)) <= GUF_QUAT_TO_EULER_EPS) { // Gimbal lock: pitch == -pi/2
|
||
euler.roll = 0;
|
||
euler.yaw = 2 * atan2f(q.x, q.w);
|
||
} else { // No gimbal-lock
|
||
euler.roll = atan2f(2 * (q.w * q.x + q.y * q.z), q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z);
|
||
euler.yaw = atan2f(2 * (q.w * q.z + q.x * q.y), q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z);
|
||
}
|
||
return euler;
|
||
}
|
||
|
||
GUF_LINALG_KWRDS float guf_quaternion_to_axis_angle(const guf_quaternion *q, guf_vec3 *axis_result)
|
||
{
|
||
GUF_ASSERT(q);
|
||
if (fabsf(q->w - 1) <= GUF_QUAT_TO_AXIS_ANGLE_EPS) { // Singularity: q->w is very close to 1 (identity-quaternions produce no rotation).
|
||
const float angle = 0;
|
||
if (axis_result) {
|
||
axis_result->x = axis_result->y = axis_result->z = 0;
|
||
}
|
||
return angle;
|
||
}
|
||
|
||
const float angle = 2 * acosf(q->w);
|
||
if (axis_result) {
|
||
const float sin_halfangle = sinf(angle / 2.f);
|
||
axis_result->x = q->x / sin_halfangle;
|
||
axis_result->y = q->y / sin_halfangle;
|
||
axis_result->z = q->z / sin_halfangle;
|
||
}
|
||
return angle;
|
||
}
|
||
|
||
GUF_LINALG_KWRDS void guf_mat4x4_init_from_quaternion(guf_mat4x4 *rotmat, guf_quaternion q)
|
||
{
|
||
GUF_ASSERT(rotmat);
|
||
|
||
rotmat->data[0][0] = 1 - 2 * q.y * q.y - 2 * q.z * q.z;
|
||
rotmat->data[0][1] = 2 * q.x * q.y - 2 * q.w * q.z;
|
||
rotmat->data[0][2] = 2 * q.x * q.z - 2 * q.w * q.y;
|
||
rotmat->data[0][3] = 0;
|
||
|
||
rotmat->data[1][0] = 2 * q.x * q.y + 2 * q.w * q.z;
|
||
rotmat->data[1][1] = 1 - 2 * q.x * q.x - 2 * q.z * q.z;
|
||
rotmat->data[1][2] = 2 * q.y * q.z - 2 * q.w * q.x;
|
||
rotmat->data[1][3] = 0;
|
||
|
||
rotmat->data[2][0] = 2 * q.x * q.z - 2 * q.w * q.y;
|
||
rotmat->data[2][1] = 2 * q.y * q.z + 2 * q.w * q.x;
|
||
rotmat->data[2][2] = 1 - 2 * q.x * q.x - 2 * q.y * q.y;
|
||
rotmat->data[2][3] = 0;
|
||
|
||
rotmat->data[3][0] = rotmat->data[3][1] = rotmat->data[3][2] = 0;
|
||
rotmat->data[3][3] = 1;
|
||
}
|
||
|
||
GUF_LINALG_KWRDS void guf_mat3x3_init_from_quaternion(guf_mat3x3 *rotmat, guf_quaternion q)
|
||
{
|
||
GUF_ASSERT(rotmat);
|
||
|
||
rotmat->data[0][0] = 1 - 2 * q.y * q.y - 2 * q.z * q.z;
|
||
rotmat->data[0][1] = 2 * q.x * q.y - 2 * q.w * q.z;
|
||
rotmat->data[0][2] = 2 * q.x * q.z - 2 * q.w * q.y;
|
||
|
||
rotmat->data[1][0] = 2 * q.x * q.y + 2 * q.w * q.z;
|
||
rotmat->data[1][1] = 1 - 2 * q.x * q.x - 2 * q.z * q.z;
|
||
rotmat->data[1][2] = 2 * q.y * q.z - 2 * q.w * q.x;
|
||
|
||
rotmat->data[2][0] = 2 * q.x * q.z - 2 * q.w * q.y;
|
||
rotmat->data[2][1] = 2 * q.y * q.z + 2 * q.w * q.x;
|
||
rotmat->data[2][2] = 1 - 2 * q.x * q.x - 2 * q.y * q.y;
|
||
}
|
||
|
||
GUF_LINALG_KWRDS guf_quaternion *guf_quaternion_normalise(guf_quaternion *q)
|
||
{
|
||
GUF_ASSERT(q);
|
||
const float sq_mag = guf_quaternion_mag_squared(*q);
|
||
if (sq_mag == 0 || sq_mag == 1) { // Prevent div by zero and don't normalise if already normalised.
|
||
return q;
|
||
}
|
||
|
||
const float mag = sqrtf(sq_mag);
|
||
if (mag == 0) { // Just in case.
|
||
return q;
|
||
}
|
||
q->w /= mag;
|
||
q->x /= mag;
|
||
q->y /= mag;
|
||
q->z /= mag;
|
||
return q;
|
||
}
|
||
|
||
#if defined(__cplusplus) && defined(GUF_LINALG_IMPL_STATIC)
|
||
GUF_LINALG_KWRDS void guf_quaternion_mul(const guf_quaternion *a, const guf_quaternion *b, guf_quaternion *result)
|
||
#else
|
||
GUF_LINALG_KWRDS void guf_quaternion_mul(const guf_quaternion *a, const guf_quaternion *b, guf_quaternion * restrict result)
|
||
#endif
|
||
{
|
||
GUF_ASSERT(a && b && result);
|
||
GUF_ASSERT(a != result && b != result);
|
||
result->w = a->w * b->w - a->x * b->x - a->y * b->y - a->z * b->z; // (r0s0 − r1s1 − r2s2 − r3s3)
|
||
result->x = a->w * b->x + a->x * b->w - a->y * b->z + a->z * b->y; // (r0s1 + r1s0 − r2s3 + r3s2)
|
||
result->y = a->w * b->y + a->x * b->z + a->y * b->w - a->z * b->x; // (r0s2 + r1s3 + r2s0 − r3s1)
|
||
result->z = a->w * b->z - a->x * b->y + a->y * b->x + a->z * b->w; // (r0s3 − r1s2 + r2s1 + r3s0)
|
||
}
|
||
|
||
GUF_LINALG_KWRDS guf_vec3 *guf_vec3_rotate_by_quat(guf_vec3 *p, const guf_quaternion *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};
|
||
|
||
// (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)
|
||
guf_quaternion tmp;
|
||
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;
|
||
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
|
||
|
||
#endif /* end impl */
|
||
|
||
#undef GUF_LINALG_KWRDS
|