Add linear algebra functions

src/guf_math.h

@@ -1,36 +1,17 @@
+#if defined(GUF_MATH_IMPL_STATIC)
+    #define GUF_MATH_KWRDS static
+#else
+    #define GUF_MATH_KWRDS
+#endif
+
 #ifndef GUF_MATH_H
 #define GUF_MATH_H
 #include "guf_assert.h"
+#include <math.h>
 
 #define GUF_PI 3.14159265358979323846264338327950288
 #define GUF_PI_F32 3.14159265358979323846264338327950288f
 
-
-typedef struct guf_vec2_i32 {
-    int32_t x, y;
-} guf_vec2_i32;
-
-typedef struct guf_vec2_f32 {
-    float x, y;
-} guf_vec2_32;
-
-typedef struct guf_vec2_f64 {
-    double x, y;
-} guf_vec2;
-
-
-typedef struct guf_vec3_i32 {
-    int32_t x, y, z;
-} guf_vec3_i32;
-
-typedef struct guf_vec3_f32 {
-    float x, y, z;
-} guf_vec3_32;
-
-typedef struct guf_vec3_f64 {
-    double x, y, z;
-} guf_vec3;
-
 // Rotate left.
 static inline uint64_t guf_rotl_u64(uint64_t x, int k) {return (x << k) | (x >> (64 - k));}
 static inline uint32_t guf_rotl_u32(uint32_t x, int k) {return (x << k) | (x >> (32 - k));}
@@ -86,14 +67,71 @@ static inline uint16_t  guf_absdiff_i16(int16_t a, int16_t b) {return a > b ? (u
 static inline uint32_t  guf_absdiff_i32(int32_t a, int32_t b) {return a > b ? (uint32_t)a - (uint32_t)b : (uint32_t)b - (uint32_t)a;}
 static inline uint64_t  guf_absdiff_i64(int64_t a, int64_t b) {return a > b ? (uint64_t)a - (uint64_t)b : (uint64_t)b - (uint64_t)a;}
 
+/*
+    Floating-point comparison:
+
+    cf. https://peps.python.org/pep-0485/ (last-retrieved 2025-03-04)
+        https://github.com/PythonCHB/close_pep/blob/master/is_close.py (last-retrieved 2025-03-04)
+        
+    rel_tol: "The relative tolerance -- the amount of error allowed, relative to the magnitude of the input values."
+                    (Example: To set a tolerance of 5%, pass rel_tol=0.05)
+                    (Example: rel_tol=1e-9 assures that the two values are the same within about 9 decimal digits)
+    abs_tol: "The minimum absolute tolerance level -- useful for comparisons to zero." (or close to zero, I think).
+*/
+static bool guf_isclose_f32(float a, float b, float rel_tol, float abs_tol)
+{
+    if (a == b) {
+        return true;
+    }
+
+    if (isinf(a) || isinf(b)) { // Two infinities of opposite sign, or one infinity and one finite number. 
+        return false;
+    }
+    if (rel_tol < 0) {
+        rel_tol = 0;
+    }
+    if (abs_tol < 0) {
+        abs_tol = 0;
+    }
+    // The relative tolerance is scaled by the larger of the two values.
+    const float diff = fabsf(b - a);
+    return ((diff <= fabsf(rel_tol * b)) || (diff <= fabsf(rel_tol * a))) || (diff <= abs_tol);
+}
+
+static bool guf_isclose_reltol_f32(float a, float b, float rel_tol)
+{
+    return guf_isclose_f32(a, b, rel_tol, 0);
+}
+static bool guf_isclose_abstol_f32(float a, float b, float abs_tol)
+{
+    return guf_isclose_f32(a, b, 0, abs_tol);
+}
+
+static bool guf_isclose_f64(double a, double b, double rel_tol, double abs_tol)
+{
+    if (a == b) {
+        return true;
+    }
+
+    if (isinf(a) || isinf(b)) { // Two infinities of opposite sign, or one infinity and one finite number. 
+        return false;
+    }
+    if (rel_tol < 0) {
+        rel_tol = 0;
+    }
+    if (abs_tol < 0) {
+        abs_tol = 0;
+    }
+    // The relative tolerance is scaled by the larger of the two values.
+    const double diff = fabs(b - a);
+    return ((diff <= fabs(rel_tol * b)) || (diff <= fabs(rel_tol * a))) || (diff <= abs_tol);
+}
+
 // An alternative lerp would be a + alpha * (b - a) (advantage: would be weakly monotonic, disadvantage: would not guarantee a for alpha = 0 and b for alpha = 1)
 static inline float guf_lerp_f32(float a, float b, float alpha)     {return (1 - alpha) * a + alpha * b;}
 static inline double guf_lerp_f64(double a, double b, double alpha) {return (1 - alpha) * a + alpha * b;}
 
-/*
+// smoothstep interpolation, cf. https://en.wikipedia.org/wiki/Smoothstep (last-retrieved 2025-02-18)
-    smoothstep interpolation, cf. https://en.wikipedia.org/wiki/Smoothstep (last-retrieved 2025-02-18)
-*/ 
-
 static inline float guf_smoothstep_f32(float edge0, float edge1, float x) 
 {
     if (edge0 == edge1) { // Prevent division by zero.

src/guf_math_linalg.h

@@ -0,0 +1,153 @@
+#if defined(GUF_MATH_LINALG_IMPL_STATIC)
+    #define GUF_MATH_LINALG_KWRDS static
+#else
+    #define GUF_MATH_LINALG_KWRDS
+#endif
+
+#ifndef GUF_MATH_LINALG_H
+    #define GUF_MATH_LINALG_H
+    #include "guf_assert.h"
+    #include "guf_math.h"
+
+    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;
+
+    #ifdef __cplusplus
+        // C++ has no restrict keyword like C99...
+        GUF_MATH_LINALG_KWRDS void guf_mat4x4_mul(const guf_mat4x4 *a, const guf_mat4x4 *b, guf_mat4x4 *result);
+    #else
+        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 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);
+
+    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;};
+
+    GUF_MATH_LINALG_KWRDS guf_vec2 *guf_vec2_normalise(guf_vec2 *v);    
+    GUF_MATH_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_MATH_LINALG_KWRDS guf_vec3 guf_vec4_to_vec3(guf_vec4 v);
+#endif 
+
+
+#if defined(GUF_MATH_LINALG_IMPL) || defined(GUF_MATH_LINALG_IMPL_STATIC)
+
+#if defined(__cplusplus) && defined(GUF_MATH_LINALG_IMPL_STATIC)
+GUF_MATH_LINALG_KWRDS void guf_mat4x4_mul(const guf_mat4x4 *a, const guf_mat4x4 *b, guf_mat4x4 *result)
+#else
+GUF_MATH_LINALG_KWRDS void guf_mat4x4_mul(const guf_mat4x4f *a, const guf_mat4x4f *b, guf_mat4x4f * 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];
+            }
+        }
+    }
+}
+
+// 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) 
+{
+    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_MATH_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_MATH_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_MATH_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_MATH_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;
+    }
+};
+
+#undef GUF_MATH_LINALG_IMPL
+#undef GUF_MATH_LINALG_IMPL_STATIC
+
+#endif /* end impl */
+
+#undef GUF_MATH_LINALG_KWRDS

src/test/test.cpp

@@ -5,6 +5,7 @@
 
 extern "C" {
     #include "guf_init.h"
+    #include "guf_math.h"
 }
 
 #include "test_dbuf.hpp"