diff --git a/CMakeLists.txt b/CMakeLists.txt index ff76fcf..c09bfd9 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -21,7 +21,7 @@ endif () add_executable(libguf_example src/test/example.c src/test/guf_str_impl.c src/test/guf_dict_impl.c) target_include_directories(libguf_example PRIVATE src src/test) -add_executable(libguf_test src/test/test.cpp src/test/guf_dbuf_impl.c src/test/guf_str_impl.c src/test/guf_dict_impl.c src/test/guf_rand_impl.c src/test/guf_sort_impl.c) +add_executable(libguf_test src/test/test.cpp src/test/guf_init_impl.c src/test/guf_dbuf_impl.c src/test/guf_str_impl.c src/test/guf_dict_impl.c src/test/guf_rand_impl.c src/test/guf_sort_impl.c) target_include_directories(libguf_test PRIVATE src src/test) if (NOT DEFINED MSVC) diff --git a/src/guf_common.h b/src/guf_common.h index f6ca47d..a7dd745 100644 --- a/src/guf_common.h +++ b/src/guf_common.h @@ -6,7 +6,17 @@ #include #include -// #define GUF_HASH_32_BIT +#if SIZE_MAX == UINT64_MAX + #define GUF_PLATFORM_BITS 64 +#elif SIZE_MAX == UINT32_MAX + #define GUF_PLATFORM_BITS 32 +#elif SIZE_MAX == UINT16_MAX + #define GUF_PLATFORM_BITS 16 +#elif SIZE_MAX == UINT8_MAX + #define GUF_PLATFORM_BITS 8 +#else + #define "Could not detect GUF_PLATFORM_BITS" +#endif /* // Copy- and move constructors: diff --git a/src/guf_math.h b/src/guf_math.h index 7671a1a..d80b43a 100644 --- a/src/guf_math.h +++ b/src/guf_math.h @@ -8,10 +8,15 @@ #define GUF_MATH_H #include "guf_assert.h" #include +#include #define GUF_PI 3.14159265358979323846264338327950288 #define GUF_PI_F32 3.14159265358979323846264338327950288f +// Biggest float/double less than one, cf. https://stackoverflow.com/a/33832753 (last-retrieved 2025-03-05) +#define GUF_MAX_F32_LT_ONE (1.f - FLT_EPSILON/FLT_RADIX) +#define GUF_MAX_F64_LT_ONE (1.0 - DBL_EPSILON/FLT_RADIX) + // 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));} @@ -72,6 +77,8 @@ static inline uint64_t guf_absdiff_i64(int64_t a, int64_t b) {return a > b ? (u 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) + + Based on is_close.py (Copyright: Christopher H. Barker, License: Apache License 2.0 http://opensource.org/licenses/apache2.0.php) 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) @@ -84,25 +91,23 @@ static bool guf_isclose_f32(float a, float b, float rel_tol, float abs_tol) return true; } - if (isinf(a) || isinf(b)) { // Two infinities of opposite sign, or one infinity and one finite number. + 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. + + rel_tol = (rel_tol < 0) ? 0 : ((rel_tol > 1) ? 1 : rel_tol); // [0, 1] + abs_tol = (abs_tol < 0) ? 0 : abs_tol; // [0, inf] + + // "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) +static inline 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) +static inline bool guf_isclose_abstol_f32(float a, float b, float abs_tol) { return guf_isclose_f32(a, b, 0, abs_tol); } @@ -113,20 +118,27 @@ static bool guf_isclose_f64(double a, double b, double rel_tol, double abs_tol) return true; } - if (isinf(a) || isinf(b)) { // Two infinities of opposite sign, or one infinity and one finite number. + 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. + + rel_tol = (rel_tol < 0) ? 0 : ((rel_tol > 1) ? 1 : rel_tol); // [0, 1] + abs_tol = (abs_tol < 0) ? 0 : abs_tol; // [0, inf] + + // "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); } +static inline bool guf_isclose_reltol_f64(double a, double b, double rel_tol) +{ + return guf_isclose_f64(a, b, rel_tol, 0); +} +static inline bool guf_isclose_abstol_f64(double a, double b, double abs_tol) +{ + return guf_isclose_f64(a, b, 0, 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;} diff --git a/src/guf_math_linalg.h b/src/guf_math_linalg.h index 36950a1..ca6a31d 100644 --- a/src/guf_math_linalg.h +++ b/src/guf_math_linalg.h @@ -9,6 +9,10 @@ #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; @@ -36,6 +40,41 @@ float w; // Scalar (real) part. } guf_quaternion; + // Convention: Tait-Bryan angles, i.e. rotation sequence z - y' - x'' + // (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_MATH_LINALG_KWRDS void guf_mat4x4_init_zero(guf_mat4x4 *m); + GUF_MATH_LINALG_KWRDS void guf_mat4x4_init_identity(guf_mat4x4 *m); + GUF_MATH_LINALG_KWRDS void guf_mat4x4_init_with_basis(guf_mat4x4 *m, guf_vec3 x_basis, guf_vec3 y_basis, guf_vec3 z_basis); + GUF_MATH_LINALG_KWRDS void guf_mat4x4_init_from_quaternion(guf_mat4x4 *rotmat, guf_quaternion q); + + GUF_MATH_LINALG_KWRDS void guf_mat3x3_init_zero(guf_mat3x3 *m); + GUF_MATH_LINALG_KWRDS void guf_mat3x3_init_identity(guf_mat3x3 *m); + GUF_MATH_LINALG_KWRDS void guf_mat3x3_init_with_basis(guf_mat3x3 *m, guf_vec3 x_basis, guf_vec3 y_basis, guf_vec3 z_basis); + GUF_MATH_LINALG_KWRDS void guf_mat3x3_init_from_quaternion(guf_mat3x3 *rotmat, guf_quaternion q); + + #ifdef __cplusplus + // C++ has no restrict keyword like C99... + GUF_MATH_LINALG_KWRDS void guf_mat4x4_transposed(const guf_mat4x4 *m, guf_mat4x4 *transposed); + #else + GUF_MATH_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_MATH_LINALG_KWRDS void guf_mat3x3_transposed(const guf_mat3x3 *m, guf_mat3x3 *transposed); + #else + GUF_MATH_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_MATH_LINALG_KWRDS void guf_mat4x4_mul(const guf_mat4x4 *a, const guf_mat4x4 *b, guf_mat4x4 *result); @@ -43,29 +82,237 @@ GUF_MATH_LINALG_KWRDS void guf_mat4x4_mul(const guf_mat4x4f *a, const guf_mat4x4f *b, guf_mat4x4f * restrict result); #endif + #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); + #else + GUF_MATH_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_MATH_LINALG_KWRDS void guf_mat4x4_set_rotmat(guf_mat4x4 *m, const guf_mat3x3 *rotmat_3x3); + 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;}; + 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_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); + + 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_MATH_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_MATH_LINALG_KWRDS void guf_quaternion_mul(const guf_quaternion *a, const guf_quaternion *b, guf_quaternion *result); + #else + GUF_MATH_LINALG_KWRDS void guf_quaternion_mul(const guf_quaternion *a, const guf_quaternion *b, guf_quaternion * restrict result); + #endif + + GUF_MATH_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_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); + #endif #if defined(GUF_MATH_LINALG_IMPL) || defined(GUF_MATH_LINALG_IMPL_STATIC) +GUF_MATH_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_MATH_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_MATH_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_MATH_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_MATH_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_MATH_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_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) +GUF_MATH_LINALG_KWRDS void guf_mat4x4_mul(const guf_mat4x4 *a, const guf_mat4x4 *b, guf_mat4x4 * restrict result) #endif { GUF_ASSERT(a && b && result); @@ -80,6 +327,64 @@ GUF_MATH_LINALG_KWRDS void guf_mat4x4_mul(const guf_mat4x4f *a, const guf_mat4x4 } } +#if defined(__cplusplus) && defined(GUF_MATH_LINALG_IMPL_STATIC) +GUF_MATH_LINALG_KWRDS void guf_mat3x3_mul(const guf_mat3x3 *a, const guf_mat3x3 *b, guf_mat3x3 *result) +#else +GUF_MATH_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_MATH_LINALG_IMPL_STATIC) +GUF_MATH_LINALG_KWRDS void guf_mat4x4_transposed(const guf_mat4x4 *m, guf_mat4x4 *transposed) +#else +GUF_MATH_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_MATH_LINALG_IMPL_STATIC) +GUF_MATH_LINALG_KWRDS void guf_mat3x3_transposed(const guf_mat3x3 *m, guf_mat3x3 *transposed) +#else +GUF_MATH_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_MATH_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]; + } + } +} + // 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) { @@ -145,6 +450,166 @@ 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) + +GUF_MATH_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_MATH_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_MATH_LINALG_KWRDS guf_euler_angles guf_quaternion_to_euler(const 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_MATH_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_MATH_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_MATH_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_MATH_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_MATH_LINALG_IMPL_STATIC) +GUF_MATH_LINALG_KWRDS void guf_quaternion_mul(const guf_quaternion *a, const guf_quaternion *b, guf_quaternion *result) +#else +GUF_MATH_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_MATH_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; +} + #undef GUF_MATH_LINALG_IMPL #undef GUF_MATH_LINALG_IMPL_STATIC diff --git a/src/guf_str.h b/src/guf_str.h index 1854ae2..7083c65 100644 --- a/src/guf_str.h +++ b/src/guf_str.h @@ -73,6 +73,10 @@ GUF_STR_KWRDS char guf_str_pop_front(guf_str *str); GUF_STR_KWRDS guf_str guf_str_substr_cpy(guf_str_view str, size_t pos, size_t count); // not necessary GUF_STR_KWRDS guf_str_view guf_substr_view(guf_str_view str, ptrdiff_t pos, ptrdiff_t count); +GUF_STR_KWRDS guf_str_view guf_str_view_trim_left(guf_str_view str); +GUF_STR_KWRDS guf_str_view guf_str_view_trim_right(guf_str_view str); + + // Tokenising/Iterating. GUF_STR_KWRDS guf_str_view guf_str_next_tok(guf_str_view *input, const guf_str_view *delims, ptrdiff_t num_delims, const guf_str_view *preserved_delims, ptrdiff_t num_preserved_delims); @@ -187,6 +191,49 @@ GUF_STR_KWRDS guf_str_view guf_str_next_tok(guf_str_view *input, const guf_str_v return tok; } +GUF_STR_KWRDS guf_str_view guf_str_view_trim_left_ascii(guf_str_view sv) +{ + if (sv.len <= 0 || sv.str == NULL) { + return sv; + } + + for (; sv.len > 0 && guf_char_isspace_ascii(*sv.str); --sv.len, ++sv.str); + + GUF_ASSERT(sv.len >= 0); + GUF_ASSERT(sv.len == 0 || !guf_char_isspace_ascii(*sv.str)); + + return sv; +} + +GUF_STR_KWRDS guf_str_view guf_str_view_trim_right_ascii(guf_str_view sv) +{ + if (sv.len <= 0 || sv.str == NULL) { + return sv; + } + + for (; sv.len > 0 && guf_char_isspace_ascii(sv.str[sv.len - 1]); --sv.len); + + GUF_ASSERT(sv.len >= 0); + GUF_ASSERT(sv.len == 0 || !guf_char_isspace_ascii(sv.str[sv.len - 1])); + + return sv; +} + +GUF_STR_KWRDS guf_str_view guf_str_view_trim_right(guf_str_view sv) +{ + if (sv.len <= 0 || sv.str == NULL) { + return sv; + } + char c = sv.str[sv.len - 1]; + while (sv.len > 0 && sv.str && c != ' ' && c != '\n' && c != '\t' && c != '\v' && c != '\f' && c != '\r') { + --sv.len; + ++sv.str; + c = sv.str[0]; + } + return sv; +} + + GUF_STR_KWRDS guf_str_view guf_substr_view(guf_str_view str, ptrdiff_t pos, ptrdiff_t count) { GUF_ASSERT(str.str); diff --git a/src/guf_utf8.h b/src/guf_utf8.h index d13ebd0..1dbbe26 100644 --- a/src/guf_utf8.h +++ b/src/guf_utf8.h @@ -23,6 +23,7 @@ typedef enum guf_utf8_stat { static inline bool guf_char_is_ascii(int c) {return c <= 0 && c <= 127;} static inline bool guf_uchar_is_ascii(unsigned char c) {return c <= 127;} +static inline bool guf_char_isspace_ascii(int c) {return c == ' ' || c == '\n' || c == '\t' || c == '\v' || c == '\f' || c == '\r';} GUF_UTF8_KWRDS int guf_utf8_num_bytes(unsigned char c); GUF_UTF8_KWRDS int guf_utf8_char_num_bytes(const guf_utf8_char *c); @@ -51,7 +52,7 @@ extern const guf_utf8_char GUF_UTF8_REPLACEMENT_CHAR; // Replacement character " // All utf-8 whitespace, cf. https://en.wikipedia.org/wiki/Whitespace_character#Unicode (last-retrieved 2025-02-27) const char* const GUF_UTF8_WHITESPACE[25] = { - " ", "\n", "\t", "\t", "\v", "\f", + " ", "\n", "\t", "\r", "\v", "\f", "\xC2\x85", "\xC2\xA0", "\xE1\x9A\x80", "\xE2\x80\x80", "\xE2\x80\x81", "\xE2\x80\x82", "\xE2\x80\x83", "\xE2\x80\x84", "\xE2\x80\x85", "\xE2\x80\x86", "\xE2\x80\x87", "\xE2\x80\x88", "\xE2\x80\x89", "\xE2\x80\x8A", "\xE2\x80\xA8", "\xE2\x80\xA9", "\xE2\x80\xAF", "\xE2\x81\x9F", "\xE3\x80\x80" }; diff --git a/src/test/guf_init_impl.c b/src/test/guf_init_impl.c new file mode 100644 index 0000000..7261d21 --- /dev/null +++ b/src/test/guf_init_impl.c @@ -0,0 +1 @@ +#include "guf_init.h" diff --git a/src/test/test.cpp b/src/test/test.cpp index 2d62bab..ce45b52 100644 --- a/src/test/test.cpp +++ b/src/test/test.cpp @@ -4,7 +4,7 @@ #include extern "C" { - #include "guf_init.h" + #include "guf_assert.h" #include "guf_math.h" } diff --git a/todo.txt b/todo.txt index c04c53b..6ee6102 100644 --- a/todo.txt +++ b/todo.txt @@ -1,5 +1,7 @@ - guf_stack, guf_queue, guf_ringbuf +- no guf_init.h + - unicode normalisation - track allocs for test (implement alloc tracker) - handle right-to-left text properly