Add quaternion operations
This commit is contained in:
jun
parent
fc8118c182
commit
e489dd694b
@ -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)
|
||||
|
||||
@ -6,7 +6,17 @@
|
||||
#include <inttypes.h>
|
||||
#include <stddef.h>
|
||||
|
||||
// #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:
|
||||
|
||||
@ -8,10 +8,15 @@
|
||||
#define GUF_MATH_H
|
||||
#include "guf_assert.h"
|
||||
#include <math.h>
|
||||
#include <float.h>
|
||||
|
||||
#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;}
|
||||
|
||||
@ -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
|
||||
|
||||
|
||||
@ -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);
|
||||
|
||||
@ -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"
|
||||
};
|
||||
|
||||
1
src/test/guf_init_impl.c
Normal file
1
src/test/guf_init_impl.c
Normal file
@ -0,0 +1 @@
|
||||
#include "guf_init.h"
|
||||
@ -4,7 +4,7 @@
|
||||
#include <iostream>
|
||||
|
||||
extern "C" {
|
||||
#include "guf_init.h"
|
||||
#include "guf_assert.h"
|
||||
#include "guf_math.h"
|
||||
}
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user