From d97c8d458c6840e6b907b7070931d11bbce18b06 Mon Sep 17 00:00:00 2001 From: Dennis Heinze Date: Mon, 11 Nov 2024 19:51:44 +0100 Subject: [PATCH 1/6] move fmath.c to subdir --- Makefile | 2 +- src/{ => math}/fmath.c | 0 2 files changed, 1 insertion(+), 1 deletion(-) rename src/{ => math}/fmath.c (100%) diff --git a/Makefile b/Makefile index 8244791d40..9952d08e2b 100755 --- a/Makefile +++ b/Makefile @@ -34,7 +34,7 @@ libdragonsys.a: $(BUILD_DIR)/system.o LIBDRAGON_OBJS += \ $(BUILD_DIR)/n64sys.o $(BUILD_DIR)/interrupt.o $(BUILD_DIR)/backtrace.o \ - $(BUILD_DIR)/fmath.o $(BUILD_DIR)/inthandler.o $(BUILD_DIR)/entrypoint.o \ + $(BUILD_DIR)/math/fmath.o $(BUILD_DIR)/inthandler.o $(BUILD_DIR)/entrypoint.o \ $(BUILD_DIR)/debug.o $(BUILD_DIR)/debugcpp.o $(BUILD_DIR)/usb.o $(BUILD_DIR)/libcart/cart.o $(BUILD_DIR)/fatfs/ff.o \ $(BUILD_DIR)/fatfs/ffunicode.o $(BUILD_DIR)/fat.o $(BUILD_DIR)/rompak.o $(BUILD_DIR)/dragonfs.o \ $(BUILD_DIR)/audio.o $(BUILD_DIR)/display.o $(BUILD_DIR)/surface.o \ diff --git a/src/fmath.c b/src/math/fmath.c similarity index 100% rename from src/fmath.c rename to src/math/fmath.c From e532824e4a33f805f280abe54f8965a048f21129 Mon Sep 17 00:00:00 2001 From: Dennis Heinze Date: Wed, 20 Nov 2024 21:37:21 +0100 Subject: [PATCH 2/6] add some macros and functions to fmath.h --- include/fmath.h | 10 ++++++++++ src/math/fmath.c | 6 ++++++ 2 files changed, 16 insertions(+) diff --git a/include/fmath.h b/include/fmath.h index a597bc6d81..0da6b880a9 100644 --- a/include/fmath.h +++ b/include/fmath.h @@ -66,6 +66,9 @@ extern "C" { #endif +#define FM_EPSILON 1e-4f +#define FM_PI 3.14159265358979f + /** @brief Reinterpret the bits composing a float as a int32. * * This version is type-punning safe and produces optimal code when optimizing. @@ -199,6 +202,13 @@ void fm_sincosf(float x, float *sin, float *cos); */ float fm_atan2f(float y, float x); +inline float fm_lerp(float a, float b, float t) +{ + return a + (b - a) * t; +} + +float fm_lerp_angle(float a, float b, float t); + #ifdef LIBDRAGON_FAST_MATH #define truncf(x) fm_truncf(x) #define floorf(x) fm_floorf(x) diff --git a/src/math/fmath.c b/src/math/fmath.c index d4291e09f2..d85bdc3b1a 100644 --- a/src/math/fmath.c +++ b/src/math/fmath.c @@ -80,3 +80,9 @@ float fm_atan2f(float y, float x) { return copysignf(r, y); } +float fm_lerp_angle(float a, float b, float t) +{ + float diff = fm_fmodf((b - a), FM_PI*2); + float dist = fm_fmodf(diff*2, FM_PI*2) - diff; + return a + dist * t; +} From 5617c3827d62b474baccb72bf7a53492f1c04485 Mon Sep 17 00:00:00 2001 From: Dennis Heinze Date: Wed, 20 Nov 2024 21:37:33 +0100 Subject: [PATCH 3/6] add fgeom.h, fgemo.c --- Makefile | 3 +- include/fgeom.h | 237 +++++++++++++++++++++++++++++++++++++++++++ include/libdragon.h | 1 + src/math/fgeom.c | 239 ++++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 479 insertions(+), 1 deletion(-) create mode 100644 include/fgeom.h create mode 100644 src/math/fgeom.c diff --git a/Makefile b/Makefile index 9952d08e2b..2f9e899202 100755 --- a/Makefile +++ b/Makefile @@ -72,7 +72,8 @@ LIBDRAGON_OBJS += \ $(BUILD_DIR)/GL/rsp_gl_pipeline.o $(BUILD_DIR)/GL/glu.o \ $(BUILD_DIR)/GL/cpu_pipeline.o $(BUILD_DIR)/GL/rsp_pipeline.o \ $(BUILD_DIR)/dlfcn.o $(BUILD_DIR)/model64.o \ - $(BUILD_DIR)/bb/skc.o $(BUILD_DIR)/bb/nand.o $(BUILD_DIR)/bb/bbfs.o + $(BUILD_DIR)/bb/skc.o $(BUILD_DIR)/bb/nand.o $(BUILD_DIR)/bb/bbfs.o \ + $(BUILD_DIR)/math/fgeom.o include $(SOURCE_DIR)/kernel/libdragon.mk include $(SOURCE_DIR)/audio/libdragon.mk diff --git a/include/fgeom.h b/include/fgeom.h new file mode 100644 index 0000000000..f5fb66244b --- /dev/null +++ b/include/fgeom.h @@ -0,0 +1,237 @@ +#ifndef LIBDRAGON_FGEOM_H +#define LIBDRAGON_FGEOM_H + +#include +#include +#include +#include "fmath.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef union { + struct { + float x, y, z; + }; + float v[3]; +} fm_vec3_t; + +typedef union { + struct { + float x, y, z, w; + }; + float v[4]; +} fm_vec4_t; + +typedef union { + struct { + float x, y, z, w; + }; + float v[4]; +} fm_quat_t; + +typedef struct { + float m[4][4]; +} fm_mat4_t; + +/******** VEC3 **********/ + +inline void fm_vec3_negate(fm_vec3_t *out, const fm_vec3_t *v) +{ + *out = (fm_vec3_t){{ -v->v[0], -v->v[1], -v->v[2] }}; +} + +inline void fm_vec3_add(fm_vec3_t *out, const fm_vec3_t *a, const fm_vec3_t *b) +{ + *out = (fm_vec3_t){{ a->v[0] + b->v[0], a->v[1] + b->v[1], a->v[2] + b->v[2] }}; +} + +inline void fm_vec3_sub(fm_vec3_t *out, const fm_vec3_t *a, const fm_vec3_t *b) +{ + *out = (fm_vec3_t){{ a->v[0] - b->v[0], a->v[1] - b->v[1], a->v[2] - b->v[2] }}; +} + +inline void fm_vec3_mul(fm_vec3_t *out, const fm_vec3_t *a, const fm_vec3_t *b) +{ + *out = (fm_vec3_t){{ a->v[0] * b->v[0], a->v[1] * b->v[1], a->v[2] * b->v[2] }}; +} + +inline void fm_vec3_div(fm_vec3_t *out, const fm_vec3_t *a, const fm_vec3_t *b) +{ + *out = (fm_vec3_t){{ a->v[0] / b->v[0], a->v[1] / b->v[1], a->v[2] / b->v[2] }}; +} + +inline void fm_vec3_scale(fm_vec3_t *out, const fm_vec3_t *a, float s) +{ + *out = (fm_vec3_t){{ a->v[0] * s, a->v[1] * s, a->v[2] * s }}; +} + +inline float fm_vec3_dot(const fm_vec3_t *a, const fm_vec3_t *b) +{ + return a->v[0] * b->v[0] + a->v[1] * b->v[1] + a->v[2] * b->v[2]; +} + +inline float fm_vec3_len2(const fm_vec3_t *a) +{ + return fm_vec3_dot(a, a); +} + +inline float fm_vec3_len(const fm_vec3_t *a) +{ + return sqrtf(fm_vec3_len2(a)); +} + +inline float fm_vec3_distance2(const fm_vec3_t *a, const fm_vec3_t *b) +{ + fm_vec3_t diff; + fm_vec3_sub(&diff, a, b); + return fm_vec3_len2(&diff); +} + +inline float fm_vec3_distance(const fm_vec3_t *a, const fm_vec3_t *b) +{ + return sqrtf(fm_vec3_distance2(a, b)); +} + +inline void fm_vec3_norm(fm_vec3_t *out, const fm_vec3_t *a) +{ + float len = fm_vec3_len(a); + if (len < FM_EPSILON) { + *out = (fm_vec3_t){}; + return; + } + *out = (fm_vec3_t){{ a->v[0] / len, a->v[1] / len, a->v[2] / len }}; +} + +inline void fm_vec3_cross(fm_vec3_t *out, const fm_vec3_t *a, const fm_vec3_t *b) +{ + *out = (fm_vec3_t){{ a->v[1] * b->v[2] - a->v[2] * b->v[1], + a->v[2] * b->v[0] - a->v[0] * b->v[2], + a->v[0] * b->v[1] - a->v[1] * b->v[0] }}; +} + +inline void fm_vec3_lerp(fm_vec3_t *out, const fm_vec3_t *a, const fm_vec3_t *b, float t) +{ + *out = (fm_vec3_t){{ a->v[0] + (b->v[0] - a->v[0]) * t, + a->v[1] + (b->v[1] - a->v[1]) * t, + a->v[2] + (b->v[2] - a->v[2]) * t }}; +} + +void fm_vec3_reflect(fm_vec3_t *out, const fm_vec3_t *i, const fm_vec3_t *n); +bool fm_vec3_refract(fm_vec3_t *out, const fm_vec3_t *i, const fm_vec3_t *n, float eta); + +/******** QUAT **********/ + +inline void fm_quat_identity(fm_quat_t *out) +{ + *out = (fm_quat_t){{ 0, 0, 0, 1 }}; +} + +void fm_quat_from_euler(fm_quat_t *out, const float rot[3]); + +inline void fm_quat_from_rotation(fm_quat_t *out, const float axis[3], float angle) +{ + float s, c; + fm_sincosf(angle * 0.5f, &s, &c); + + *out = (fm_quat_t){{ axis[0] * s, axis[1] * s, axis[2] * s, c }}; +} + +void fm_quat_mul(fm_quat_t *out, const fm_quat_t *a, const fm_quat_t *b); +void fm_quat_rotate(fm_quat_t *out, const fm_quat_t *q, const float axis[3], float angle); + +inline float fm_quat_dot(const fm_quat_t *a, const fm_quat_t *b) +{ + return a->v[0] * b->v[0] + a->v[1] * b->v[1] + a->v[2] * b->v[2] + a->v[3] * b->v[3]; +} + +inline void fm_quat_norm(fm_quat_t *out, const fm_quat_t *q) +{ + float len = sqrtf(fm_quat_dot(q, q)); + if (len < FM_EPSILON) { + fm_quat_identity(out); + return; + } + *out = (fm_quat_t){{ q->v[0] / len, q->v[1] / len, q->v[2] / len, q->v[3] / len }}; +} + +void fm_quat_nlerp(fm_quat_t *out, const fm_quat_t *a, const fm_quat_t *b, float t); +void fm_quat_slerp(fm_quat_t *out, const fm_quat_t *a, const fm_quat_t *b, float t); + + +/******** MAT4 **********/ + +inline void fm_mat4_identity(fm_mat4_t *out) +{ + *out = (fm_mat4_t){}; + out->m[0][0] = 1; + out->m[1][1] = 1; + out->m[2][2] = 1; + out->m[3][3] = 1; +} + +inline void fm_mat4_scale(fm_mat4_t *out, const float v[3]) +{ + for (int i=0; i<4; i++) out->m[0][i] *= v[0]; + for (int i=0; i<4; i++) out->m[1][i] *= v[1]; + for (int i=0; i<4; i++) out->m[2][i] *= v[2]; +} + +inline void fm_mat4_translate(fm_mat4_t *out, const float v[3]) +{ + for (int i=0; i<3; i++) out->m[3][i] += v[i]; +} + +void fm_mat4_rotate(fm_mat4_t *out, const float axis[3], float angle); +void fm_mat4_from_srt(fm_mat4_t *out, const float scale[3], const float quat[4], const float translate[3]); +void fm_mat4_from_srt_euler(fm_mat4_t *out, const float scale[3], const float rot[3], const float translate[3]); + + +inline void fm_mat4_mul(fm_mat4_t *out, const fm_mat4_t *a, const fm_mat4_t *b) +{ + fm_mat4_t tmp; + for (int i = 0; i < 4; i++) + { + for (int j = 0; j < 4; j++) + { + tmp.m[i][j] = a->m[i][0] * b->m[0][j] + + a->m[i][1] * b->m[1][j] + + a->m[i][2] * b->m[2][j] + + a->m[i][3] * b->m[3][j]; + } + } + *out = tmp; +} + +inline void fm_mat4_transpose(fm_mat4_t *out, const fm_mat4_t *m) +{ + for (int i = 0; i < 4; i++) + { + for (int j = 0; j < 4; j++) + { + out->m[i][j] = m->m[j][i]; + } + } +} + +float fm_mat4_det(const fm_mat4_t *m); + +void fm_mat4_inverse(fm_mat4_t *out, const fm_mat4_t *m); + +inline void fm_mat4_mul_vec3(fm_vec4_t *out, const fm_mat4_t *m, const fm_vec3_t *v) +{ + for (int i = 0; i < 4; i++) + { + out->v[i] = m->m[0][i] * v->v[0] + + m->m[1][i] * v->v[1] + + m->m[2][i] * v->v[2] + + m->m[3][i]; + } +} + +#ifdef __cplusplus +} +#endif + +#endif \ No newline at end of file diff --git a/include/libdragon.h b/include/libdragon.h index aa806641c0..5c13dfc4b6 100644 --- a/include/libdragon.h +++ b/include/libdragon.h @@ -22,6 +22,7 @@ /* Easy include wrapper */ #include "n64types.h" #include "fmath.h" +#include "fgeom.h" #include "audio.h" #include "console.h" #include "debug.h" diff --git a/src/math/fgeom.c b/src/math/fgeom.c new file mode 100644 index 0000000000..6f93d3e3b7 --- /dev/null +++ b/src/math/fgeom.c @@ -0,0 +1,239 @@ +#include "fgeom.h" + +void fm_vec3_reflect(fm_vec3_t *out, const fm_vec3_t *i, const fm_vec3_t *n) +{ + fm_vec3_t tmp; + fm_vec3_scale(&tmp, n, 2.0f * fm_vec3_dot(i, n)); + fm_vec3_sub(out, i, &tmp); +} + +bool fm_vec3_refract(fm_vec3_t *out, const fm_vec3_t *i, const fm_vec3_t *n, float eta) +{ + float ndi = fm_vec3_dot(n, i); + float eni = eta * ndi; + float k = 1.0f - eta*eta + eni*eni; + + if (k < 0.0f) { + *out = (fm_vec3_t){}; + return false; + } + + fm_vec3_t tmp; + fm_vec3_scale(&tmp, n, eni + sqrtf(k)); + fm_vec3_scale(out, i, eta); + fm_vec3_sub(out, out, &tmp); + return true; +} + +// Create a quaternion +void fm_quat_from_euler(fm_quat_t *out, const float rot[3]) +{ + float c1, c2, c3, s1, s2, s3; + fm_sincosf(rot[0] * 0.5f, &s1, &c1); + fm_sincosf(rot[1] * 0.5f, &s2, &c2); + fm_sincosf(rot[2] * 0.5f, &s3, &c3); + + *out = (fm_quat_t){{ c1 * c2 * c3 + s1 * s2 * s3, + s1 * c2 * c3 - c1 * s2 * s3, + c1 * s2 * c3 + s1 * c2 * s3, + c1 * c2 * s3 - s1 * s2 * c3 }}; +} + +void fm_quat_from_euler_xyz(fm_quat_t *out, const float rot[3]) +{ + float c1, c2, c3, s1, s2, s3; + fm_sincosf(rot[0] * 0.5f, &s1, &c1); + fm_sincosf(rot[1] * 0.5f, &s2, &c2); + fm_sincosf(rot[2] * 0.5f, &s3, &c3); + + *out = (fm_quat_t){{ s1 * c2 * c3 + c1 * s2 * s3, + c1 * s2 * c3 - s1 * c2 * s3, + c1 * c2 * s3 - s1 * s2 * c3, + c1 * c2 * c3 - s1 * s2 * s3 }}; +} + +inline void fm_quat_mul(fm_quat_t *out, const fm_quat_t *a, const fm_quat_t *b) +{ + *out = (fm_quat_t){{ a->v[3] * b->v[0] + a->v[0] * b->v[3] + a->v[1] * b->v[2] - a->v[2] * b->v[1], + a->v[3] * b->v[1] - a->v[0] * b->v[2] + a->v[1] * b->v[3] + a->v[2] * b->v[0], + a->v[3] * b->v[2] + a->v[0] * b->v[1] - a->v[1] * b->v[0] + a->v[2] * b->v[3], + a->v[3] * b->v[3] - a->v[0] * b->v[0] - a->v[1] * b->v[1] - a->v[2] * b->v[2] }}; +} + +void fm_quat_from_euler_zxy(fm_quat_t *out, const float rot[3]) +{ + fm_quat_identity(out); + fm_quat_rotate(out, out, (float[]){0,0,1}, rot[0]); + fm_quat_rotate(out, out, (float[]){1,0,0}, rot[1]); + fm_quat_rotate(out, out, (float[]){0,1,0}, rot[2]); +} + +void fm_quat_rotate(fm_quat_t *out, const fm_quat_t *q, const float axis[3], float angle) +{ + fm_quat_t r; + fm_quat_from_rotation(&r, axis, angle); + fm_quat_mul(out, q, &r); +} + +void fm_quat_nlerp(fm_quat_t *out, const fm_quat_t *a, const fm_quat_t *b, float t) +{ + float blend = 1.0f - t; + if (fm_quat_dot(a, b) < 0.0f) blend = -blend; + + *out = (fm_quat_t){{ a->v[0] * blend + b->v[0] * t, + a->v[1] * blend + b->v[1] * t, + a->v[2] * blend + b->v[2] * t, + a->v[3] * blend + b->v[3] * t }}; + fm_quat_norm(out, out); +} + +void fm_quat_slerp(fm_quat_t *out, const fm_quat_t *a, const fm_quat_t *b, float t) +{ + float dot = fm_quat_dot(a, b); + float negated = 1.0f; + if (dot < 0.0f) { + dot = -dot; + negated = -1.0f; + } + + float theta = acosf(dot); + float sin_theta = sinf(theta); + float inv_sin_theta = 1.0f / sin_theta; + float ws = sinf((1.0f - t) * theta) * inv_sin_theta; + float we = sinf(t * theta) * inv_sin_theta * negated; + *out = (fm_quat_t){{ a->v[0] * ws + b->v[0] * we, + a->v[1] * ws + b->v[1] * we, + a->v[2] * ws + b->v[2] * we, + a->v[3] * ws + b->v[3] * we }}; +} + + +void fm_mat4_rotate(fm_mat4_t *out, const float axis[3], float angle) +{ + float s, c; + fm_sincosf(angle, &s, &c); + + float t = 1.0f - c; + float x = axis[0], y = axis[1], z = axis[2]; + + *out = (fm_mat4_t){{ + { t * x * x + c, t * x * y - s * z, t * x * z + s * y, 0 }, + { t * x * y + s * z, t * y * y + c, t * y * z - s * x, 0 }, + { t * x * z - s * y, t * y * z + s * x, t * z * z + c, 0 }, + { 0, 0, 0, 1 } + }}; +} + +void fm_mat4_from_srt(fm_mat4_t *out, const float scale[3], const float quat[4], const float translate[3]) +{ + float x = quat[0], y = quat[1], z = quat[2], w = quat[3]; + float x2 = x + x, y2 = y + y, z2 = z + z; + float xx = x * x2, xy = x * y2, xz = x * z2; + float yy = y * y2, yz = y * z2, zz = z * z2; + float wx = w * x2, wy = w * y2, wz = w * z2; + float sx = scale[0], sy = scale[1], sz = scale[2]; + + *out = (fm_mat4_t){{ + { (1.0f - (yy + zz)) * sx, (xy + wz) * sx, (xz - wy) * sx, 0 }, + { (xy - wz) * sy, (1.0f - (xx + zz)) * sy, (yz + wx) * sy, 0 }, + { (xz + wy) * sz, (yz - wx) * sz, (1.0f - (xx + yy)) * sz, 0 }, + { translate[0], translate[1], translate[2], 1 } + }}; +} + +void fm_mat4_from_srt_euler(fm_mat4_t *out, const float scale[3], const float rot[3], const float translate[3]) +{ + float s0, c0, s1, c1, s2, c2; + fm_sincosf(rot[0], &s0, &c0); + fm_sincosf(rot[1], &s1, &c1); + fm_sincosf(rot[2], &s2, &c2); + + *out = (fm_mat4_t){{ + { scale[0] * c2 * c1, scale[0] * (c2 * s1 * s0 - s2 * c0), scale[0] * (c2 * s1 * c0 + s2 * s0), 0 }, + { scale[1] * s2 * c1, scale[1] * (s2 * s1 * s0 + c2 * c0), scale[1] * (s2 * s1 * c0 - c2 * s0), 0 }, + {-scale[2] * s1, scale[2] * c1 * s0, scale[2] * c1 * c0, 0 }, + { translate[0], translate[1], translate[2], 1 } + }}; +} + +float fm_mat4_det(const fm_mat4_t *m) +{ + float m00 = m->m[0][0], m01 = m->m[0][1], m02 = m->m[0][2], m03 = m->m[0][3], + m10 = m->m[1][0], m11 = m->m[1][1], m12 = m->m[1][2], m13 = m->m[1][3], + m20 = m->m[2][0], m21 = m->m[2][1], m22 = m->m[2][2], m23 = m->m[2][3], + m30 = m->m[3][0], m31 = m->m[3][1], m32 = m->m[3][2], m33 = m->m[3][3]; + + float t0 = m22 * m33 - m32 * m23; + float t1 = m21 * m33 - m31 * m23; + float t2 = m21 * m32 - m31 * m22; + float t3 = m20 * m33 - m30 * m23; + float t4 = m20 * m32 - m30 * m22; + float t5 = m20 * m31 - m30 * m21; + + return m00 * (m11 * t0 - m12 * t1 + m13 * t2) + - m01 * (m10 * t0 - m12 * t3 + m13 * t4) + + m02 * (m10 * t1 - m11 * t3 + m13 * t5) + - m03 * (m10 * t2 - m11 * t4 + m12 * t5); +} + +void fm_mat4_inverse(fm_mat4_t *out, const fm_mat4_t *m) +{ + fm_mat4_t tmp; + + float m00 = m->m[0][0], m01 = m->m[0][1], m02 = m->m[0][2], m03 = m->m[0][3], + m10 = m->m[1][0], m11 = m->m[1][1], m12 = m->m[1][2], m13 = m->m[1][3], + m20 = m->m[2][0], m21 = m->m[2][1], m22 = m->m[2][2], m23 = m->m[2][3], + m30 = m->m[3][0], m31 = m->m[3][1], m32 = m->m[3][2], m33 = m->m[3][3]; + + float t0 = m22 * m33 - m32 * m23; + float t1 = m21 * m33 - m31 * m23; + float t2 = m21 * m32 - m31 * m22; + float t3 = m20 * m33 - m30 * m23; + float t4 = m20 * m32 - m30 * m22; + float t5 = m20 * m31 - m30 * m21; + + tmp.m[0][0] = m11 * t0 - m12 * t1 + m13 * t2; + tmp.m[1][0] =-(m10 * t0 - m12 * t3 + m13 * t4); + tmp.m[2][0] = m10 * t1 - m11 * t3 + m13 * t5; + tmp.m[3][0] =-(m10 * t2 - m11 * t4 + m12 * t5); + + tmp.m[0][1] =-(m01 * t0 - m02 * t1 + m03 * t2); + tmp.m[1][1] = m00 * t0 - m02 * t3 + m03 * t4; + tmp.m[2][1] =-(m00 * t1 - m01 * t3 + m03 * t5); + tmp.m[3][1] = m00 * t2 - m01 * t4 + m02 * t5; + + t0 = m12 * m33 - m32 * m13; + t1 = m11 * m33 - m31 * m13; + t2 = m11 * m32 - m31 * m12; + t3 = m10 * m33 - m30 * m13; + t4 = m10 * m32 - m30 * m12; + t5 = m10 * m31 - m30 * m11; + + tmp.m[0][2] = m01 * t0 - m02 * t1 + m03 * t2; + tmp.m[1][2] =-(m00 * t0 - m02 * t3 + m03 * t4); + tmp.m[2][2] = m00 * t1 - m01 * t3 + m03 * t5; + tmp.m[3][2] =-(m00 * t2 - m01 * t4 + m02 * t5); + + t0 = m12 * m23 - m22 * m13; + t1 = m11 * m23 - m21 * m13; + t2 = m11 * m22 - m21 * m12; + t3 = m10 * m23 - m20 * m13; + t4 = m10 * m22 - m20 * m12; + t5 = m10 * m21 - m20 * m11; + + tmp.m[0][3] =-(m01 * t0 - m02 * t1 + m03 * t2); + tmp.m[1][3] = m00 * t0 - m02 * t3 + m03 * t4; + tmp.m[2][3] =-(m00 * t1 - m01 * t3 + m03 * t5); + tmp.m[3][3] = m00 * t2 - m01 * t4 + m02 * t5; + + float det = 1.0f / (m00 * tmp.m[0][0] + m01 * tmp.m[1][0] + + m02 * tmp.m[2][0] + m03 * tmp.m[3][0]); + + for (int i = 0; i < 4; i++) + { + for (int j = 0; j < 4; j++) + { + out->m[i][j] = tmp.m[i][j] * det; + } + } +} From b3e090bec12f1d9d987c6e8027a23ddb830107a9 Mon Sep 17 00:00:00 2001 From: Dennis Heinze Date: Wed, 20 Nov 2024 22:06:49 +0100 Subject: [PATCH 4/6] fix missing install --- Makefile | 1 + 1 file changed, 1 insertion(+) diff --git a/Makefile b/Makefile index 2f9e899202..24394a58db 100755 --- a/Makefile +++ b/Makefile @@ -121,6 +121,7 @@ install: install-mk libdragon install -Cv -m 0644 include/pputils.h $(INSTALLDIR)/mips64-elf/include/pputils.h install -Cv -m 0644 include/n64sys.h $(INSTALLDIR)/mips64-elf/include/n64sys.h install -Cv -m 0644 include/fmath.h $(INSTALLDIR)/mips64-elf/include/fmath.h + install -Cv -m 0644 include/fgeom.h $(INSTALLDIR)/mips64-elf/include/fgeom.h install -Cv -m 0644 include/backtrace.h $(INSTALLDIR)/mips64-elf/include/backtrace.h install -Cv -m 0644 include/cop0.h $(INSTALLDIR)/mips64-elf/include/cop0.h install -Cv -m 0644 include/cop1.h $(INSTALLDIR)/mips64-elf/include/cop1.h From 2ea27d12d3b686f44923e69f0ead43884f488b5e Mon Sep 17 00:00:00 2001 From: Dennis Heinze Date: Thu, 21 Nov 2024 22:22:47 +0100 Subject: [PATCH 5/6] use loops where possible + use named fields --- include/fgeom.h | 36 +++++++++++++++++------------------- 1 file changed, 17 insertions(+), 19 deletions(-) diff --git a/include/fgeom.h b/include/fgeom.h index f5fb66244b..fabf01d007 100644 --- a/include/fgeom.h +++ b/include/fgeom.h @@ -39,37 +39,37 @@ typedef struct { inline void fm_vec3_negate(fm_vec3_t *out, const fm_vec3_t *v) { - *out = (fm_vec3_t){{ -v->v[0], -v->v[1], -v->v[2] }}; + for (int i = 0; i < 3; i++) out->v[i] = -v->v[i]; } inline void fm_vec3_add(fm_vec3_t *out, const fm_vec3_t *a, const fm_vec3_t *b) { - *out = (fm_vec3_t){{ a->v[0] + b->v[0], a->v[1] + b->v[1], a->v[2] + b->v[2] }}; + for (int i = 0; i < 3; i++) out->v[i] = a->v[i] + b->v[i]; } inline void fm_vec3_sub(fm_vec3_t *out, const fm_vec3_t *a, const fm_vec3_t *b) { - *out = (fm_vec3_t){{ a->v[0] - b->v[0], a->v[1] - b->v[1], a->v[2] - b->v[2] }}; + for (int i = 0; i < 3; i++) out->v[i] = a->v[i] - b->v[i]; } inline void fm_vec3_mul(fm_vec3_t *out, const fm_vec3_t *a, const fm_vec3_t *b) { - *out = (fm_vec3_t){{ a->v[0] * b->v[0], a->v[1] * b->v[1], a->v[2] * b->v[2] }}; + for (int i = 0; i < 3; i++) out->v[i] = a->v[i] * b->v[i]; } inline void fm_vec3_div(fm_vec3_t *out, const fm_vec3_t *a, const fm_vec3_t *b) { - *out = (fm_vec3_t){{ a->v[0] / b->v[0], a->v[1] / b->v[1], a->v[2] / b->v[2] }}; + for (int i = 0; i < 3; i++) out->v[i] = a->v[i] / b->v[i]; } inline void fm_vec3_scale(fm_vec3_t *out, const fm_vec3_t *a, float s) { - *out = (fm_vec3_t){{ a->v[0] * s, a->v[1] * s, a->v[2] * s }}; + for (int i = 0; i < 3; i++) out->v[i] = a->v[i] * s; } inline float fm_vec3_dot(const fm_vec3_t *a, const fm_vec3_t *b) { - return a->v[0] * b->v[0] + a->v[1] * b->v[1] + a->v[2] * b->v[2]; + return a->x * b->x + a->y * b->y + a->z * b->z; } inline float fm_vec3_len2(const fm_vec3_t *a) @@ -101,21 +101,19 @@ inline void fm_vec3_norm(fm_vec3_t *out, const fm_vec3_t *a) *out = (fm_vec3_t){}; return; } - *out = (fm_vec3_t){{ a->v[0] / len, a->v[1] / len, a->v[2] / len }}; + *out = (fm_vec3_t){{ a->x / len, a->y / len, a->z / len }}; } inline void fm_vec3_cross(fm_vec3_t *out, const fm_vec3_t *a, const fm_vec3_t *b) { - *out = (fm_vec3_t){{ a->v[1] * b->v[2] - a->v[2] * b->v[1], - a->v[2] * b->v[0] - a->v[0] * b->v[2], - a->v[0] * b->v[1] - a->v[1] * b->v[0] }}; + *out = (fm_vec3_t){{ a->y * b->z - a->z * b->y, + a->z * b->x - a->x * b->z, + a->x * b->y - a->y * b->x }}; } inline void fm_vec3_lerp(fm_vec3_t *out, const fm_vec3_t *a, const fm_vec3_t *b, float t) { - *out = (fm_vec3_t){{ a->v[0] + (b->v[0] - a->v[0]) * t, - a->v[1] + (b->v[1] - a->v[1]) * t, - a->v[2] + (b->v[2] - a->v[2]) * t }}; + for (int i = 0; i < 3; i++) out->v[i] = a->v[i] + (b->v[i] - a->v[i]) * t; } void fm_vec3_reflect(fm_vec3_t *out, const fm_vec3_t *i, const fm_vec3_t *n); @@ -143,7 +141,7 @@ void fm_quat_rotate(fm_quat_t *out, const fm_quat_t *q, const float axis[3], flo inline float fm_quat_dot(const fm_quat_t *a, const fm_quat_t *b) { - return a->v[0] * b->v[0] + a->v[1] * b->v[1] + a->v[2] * b->v[2] + a->v[3] * b->v[3]; + return a->x * b->x + a->y * b->y + a->z * b->z + a->w * b->w; } inline void fm_quat_norm(fm_quat_t *out, const fm_quat_t *q) @@ -153,7 +151,7 @@ inline void fm_quat_norm(fm_quat_t *out, const fm_quat_t *q) fm_quat_identity(out); return; } - *out = (fm_quat_t){{ q->v[0] / len, q->v[1] / len, q->v[2] / len, q->v[3] / len }}; + *out = (fm_quat_t){{ q->x / len, q->y / len, q->z / len, q->w / len }}; } void fm_quat_nlerp(fm_quat_t *out, const fm_quat_t *a, const fm_quat_t *b, float t); @@ -223,9 +221,9 @@ inline void fm_mat4_mul_vec3(fm_vec4_t *out, const fm_mat4_t *m, const fm_vec3_t { for (int i = 0; i < 4; i++) { - out->v[i] = m->m[0][i] * v->v[0] + - m->m[1][i] * v->v[1] + - m->m[2][i] * v->v[2] + + out->v[i] = m->m[0][i] * v->x + + m->m[1][i] * v->y + + m->m[2][i] * v->z + m->m[3][i]; } } From bda149d0ce7b76ecbb947bcaa1217c563fdd09f8 Mon Sep 17 00:00:00 2001 From: Dennis Heinze Date: Sat, 23 Nov 2024 21:37:52 +0100 Subject: [PATCH 6/6] add documentation --- include/fgeom.h | 214 ++++++++++++++++++++++++++++++++++++++++++----- include/fmath.h | 27 +++++- src/math/fgeom.c | 32 +++---- 3 files changed, 230 insertions(+), 43 deletions(-) diff --git a/include/fgeom.h b/include/fgeom.h index fabf01d007..72f2c1901a 100644 --- a/include/fgeom.h +++ b/include/fgeom.h @@ -1,3 +1,9 @@ +/** + * @file fgeom.h + * @brief Structs and functions for common 3D geometry calculations. + * @ingroup fastmath + */ + #ifndef LIBDRAGON_FGEOM_H #define LIBDRAGON_FGEOM_H @@ -10,78 +16,137 @@ extern "C" { #endif +/** + * @brief Three dimensional vector with single precision floating point components. + * + * The components can either be accessed via named fields (x, y, z), + * or by index using the v field. + */ typedef union { struct { - float x, y, z; + float x, y, z; ///< Named fields to access individual vector components directly. }; - float v[3]; + float v[3]; ///< Array field to access the vector components via index. } fm_vec3_t; +/** + * @brief Four dimensional vector with single precision floating point components. + * + * The components can either be accessed via named fields (x, y, z, w), + * or by index using the v field. + */ typedef union { struct { - float x, y, z, w; + float x, y, z, w; ///< Named fields to access individual vector components directly. }; - float v[4]; + float v[4]; ///< Array field to access the vector components via index. } fm_vec4_t; +/** + * @brief A quaternion with single precision floating point components. + * + * The components can either be accessed via named fields (x, y, z, w), + * or by index using the v field. + */ typedef union { struct { - float x, y, z, w; + float x, y, z, w; ///< Named fields to access individual quaternion components directly. }; - float v[4]; + float v[4]; ///< Array field to access the quaternion components via index. } fm_quat_t; +/** + * @brief 4x4 Matrix with single precision floating point components. + */ typedef struct { - float m[4][4]; + float m[4][4]; ///< Two-dimensional array that contains the matrix coefficients in column-major order. } fm_mat4_t; /******** VEC3 **********/ +/** + * @brief Negate a 3D vector. + * @note This function still works if input and output are in the same memory location. + */ inline void fm_vec3_negate(fm_vec3_t *out, const fm_vec3_t *v) { for (int i = 0; i < 3; i++) out->v[i] = -v->v[i]; } +/** + * @brief Add two 3D vectors component-wise. + * @note This function still works if input and output are in the same memory location. + */ inline void fm_vec3_add(fm_vec3_t *out, const fm_vec3_t *a, const fm_vec3_t *b) { for (int i = 0; i < 3; i++) out->v[i] = a->v[i] + b->v[i]; } +/** + * @brief Subtract two 3D vectors component-wise. + * @note This function still works if input and output are in the same memory location. + */ inline void fm_vec3_sub(fm_vec3_t *out, const fm_vec3_t *a, const fm_vec3_t *b) { for (int i = 0; i < 3; i++) out->v[i] = a->v[i] - b->v[i]; } +/** + * @brief Multiply two 3D vectors component-wise. + * @note If you need the dot product, use #fm_vec3_dot instead. + * If you need the cross product, use #fm_vec3_cross instead. + * @note This function still works if input and output are in the same memory location. + */ inline void fm_vec3_mul(fm_vec3_t *out, const fm_vec3_t *a, const fm_vec3_t *b) { for (int i = 0; i < 3; i++) out->v[i] = a->v[i] * b->v[i]; } +/** + * @brief Divide two 3D vectors component-wise. + * @note This function still works if input and output are in the same memory location. + */ inline void fm_vec3_div(fm_vec3_t *out, const fm_vec3_t *a, const fm_vec3_t *b) { for (int i = 0; i < 3; i++) out->v[i] = a->v[i] / b->v[i]; } +/** + * @brief Scale a 3D vector by a factor. + * @note This function still works if input and output are in the same memory location. + */ inline void fm_vec3_scale(fm_vec3_t *out, const fm_vec3_t *a, float s) { for (int i = 0; i < 3; i++) out->v[i] = a->v[i] * s; } +/** + * @brief Compute the dot product of two 3D vectors. + */ inline float fm_vec3_dot(const fm_vec3_t *a, const fm_vec3_t *b) { return a->x * b->x + a->y * b->y + a->z * b->z; } +/** + * @brief Compute the square magnitude of a 3D vector. + */ inline float fm_vec3_len2(const fm_vec3_t *a) { return fm_vec3_dot(a, a); } +/** + * @brief Compute the magnitude of a 3D vector. + */ inline float fm_vec3_len(const fm_vec3_t *a) { return sqrtf(fm_vec3_len2(a)); } +/** + * @brief Compute the square distance between two 3D vectors. + */ inline float fm_vec3_distance2(const fm_vec3_t *a, const fm_vec3_t *b) { fm_vec3_t diff; @@ -89,11 +154,18 @@ inline float fm_vec3_distance2(const fm_vec3_t *a, const fm_vec3_t *b) return fm_vec3_len2(&diff); } +/** + * @brief Compute the distance between two 3D vectors. + */ inline float fm_vec3_distance(const fm_vec3_t *a, const fm_vec3_t *b) { return sqrtf(fm_vec3_distance2(a, b)); } +/** + * @brief Normalize a 3D vector. + * @note This function still works if input and output are in the same memory location. + */ inline void fm_vec3_norm(fm_vec3_t *out, const fm_vec3_t *a) { float len = fm_vec3_len(a); @@ -104,6 +176,9 @@ inline void fm_vec3_norm(fm_vec3_t *out, const fm_vec3_t *a) *out = (fm_vec3_t){{ a->x / len, a->y / len, a->z / len }}; } +/** + * @brief Compute the cross product of two 3D vectors. + */ inline void fm_vec3_cross(fm_vec3_t *out, const fm_vec3_t *a, const fm_vec3_t *b) { *out = (fm_vec3_t){{ a->y * b->z - a->z * b->y, @@ -111,39 +186,88 @@ inline void fm_vec3_cross(fm_vec3_t *out, const fm_vec3_t *a, const fm_vec3_t *b a->x * b->y - a->y * b->x }}; } +/** + * @brief Linearly interpolate between two 3D vectors. + */ inline void fm_vec3_lerp(fm_vec3_t *out, const fm_vec3_t *a, const fm_vec3_t *b, float t) { for (int i = 0; i < 3; i++) out->v[i] = a->v[i] + (b->v[i] - a->v[i]) * t; } +/** + * @brief Compute the reflection of an incident vector off a surface. + * + * @param[out] out Will contain the reflected vector. + * @param[in] i The incident vector. + * @param[in] n The surface's normal vector. Must be normalized. + */ void fm_vec3_reflect(fm_vec3_t *out, const fm_vec3_t *i, const fm_vec3_t *n); + +/** + * @brief Compute the refraction of an incident vector through a surface. + * + * @param[out] out Will contain the refracted vector. + * @param[in] i The incident vector. Must be normalized. + * @param[in] n The surface's normal vector. Must be normalized. + * @param[in] eta The ratio of indices of refraction (indicent/transmitted) + * @return True if refraction occurs; false if total internal reflection occurs. + */ bool fm_vec3_refract(fm_vec3_t *out, const fm_vec3_t *i, const fm_vec3_t *n, float eta); /******** QUAT **********/ +/** + * @brief Create an identity quaternion. + */ inline void fm_quat_identity(fm_quat_t *out) { *out = (fm_quat_t){{ 0, 0, 0, 1 }}; } +/** + * @brief Create a quaternion from euler angles. + * + * @param[out] out Will contain the created quaternion. + * @param[in] rot Array containing euler angles in radians. + */ void fm_quat_from_euler(fm_quat_t *out, const float rot[3]); -inline void fm_quat_from_rotation(fm_quat_t *out, const float axis[3], float angle) +/** + * @brief Create a quaternion from an axis of rotation and an angle. + * @param[out] out Will contain the created quaternion. + * @param[in] axis The axis of rotation. Must be normalized. + * @param[in] angle The angle in radians. + */ +inline void fm_quat_from_rotation(fm_quat_t *out, const fm_vec3_t *axis, float angle) { float s, c; fm_sincosf(angle * 0.5f, &s, &c); - *out = (fm_quat_t){{ axis[0] * s, axis[1] * s, axis[2] * s, c }}; + *out = (fm_quat_t){{ axis->x * s, axis->y * s, axis->z * s, c }}; } +/** + * @brief Multiply two quaternions. + */ void fm_quat_mul(fm_quat_t *out, const fm_quat_t *a, const fm_quat_t *b); -void fm_quat_rotate(fm_quat_t *out, const fm_quat_t *q, const float axis[3], float angle); +/** + * @brief Rotate a quaternion around an axis by some angle. + * @note This is just a shorthand for #fm_quat_from_rotation and #fm_quat_mul. + */ +void fm_quat_rotate(fm_quat_t *out, const fm_quat_t *q, const fm_vec3_t *axis, float angle); + +/** + * @brief Compute the dot product of two quaternions. + */ inline float fm_quat_dot(const fm_quat_t *a, const fm_quat_t *b) { return a->x * b->x + a->y * b->y + a->z * b->z + a->w * b->w; } +/** + * @brief Normalize a quaternion. + */ inline void fm_quat_norm(fm_quat_t *out, const fm_quat_t *q) { float len = sqrtf(fm_quat_dot(q, q)); @@ -154,12 +278,25 @@ inline void fm_quat_norm(fm_quat_t *out, const fm_quat_t *q) *out = (fm_quat_t){{ q->x / len, q->y / len, q->z / len, q->w / len }}; } +/** + * @brief Compute normalized linear interpolation between two quaternions. + * @note This function is faster than #fm_quat_slerp, but produces a non-constant angular velocity when used for animation. + */ void fm_quat_nlerp(fm_quat_t *out, const fm_quat_t *a, const fm_quat_t *b, float t); + + +/** + * @brief Compute spherical linear interpolation between two quaternions. + * @note As opposed to #fm_quat_nlerp, this function produces constant angular velocity when used for animation, but is slower. + */ void fm_quat_slerp(fm_quat_t *out, const fm_quat_t *a, const fm_quat_t *b, float t); /******** MAT4 **********/ +/** + * @brief Create a 4x4 identity matrix. + */ inline void fm_mat4_identity(fm_mat4_t *out) { *out = (fm_mat4_t){}; @@ -169,23 +306,46 @@ inline void fm_mat4_identity(fm_mat4_t *out) out->m[3][3] = 1; } -inline void fm_mat4_scale(fm_mat4_t *out, const float v[3]) +/** + * @brief Apply scale to a 4x4 matrix. + */ +inline void fm_mat4_scale(fm_mat4_t *out, const fm_vec3_t *scale) { - for (int i=0; i<4; i++) out->m[0][i] *= v[0]; - for (int i=0; i<4; i++) out->m[1][i] *= v[1]; - for (int i=0; i<4; i++) out->m[2][i] *= v[2]; + for (int i=0; i<4; i++) out->m[0][i] *= scale->x; + for (int i=0; i<4; i++) out->m[1][i] *= scale->y; + for (int i=0; i<4; i++) out->m[2][i] *= scale->z; } -inline void fm_mat4_translate(fm_mat4_t *out, const float v[3]) +/** + * @brief Apply translation to a 4x4 matrix. + */ +inline void fm_mat4_translate(fm_mat4_t *out, const fm_vec3_t *translate) { - for (int i=0; i<3; i++) out->m[3][i] += v[i]; + for (int i=0; i<3; i++) out->m[3][i] += translate->v[i]; } -void fm_mat4_rotate(fm_mat4_t *out, const float axis[3], float angle); -void fm_mat4_from_srt(fm_mat4_t *out, const float scale[3], const float quat[4], const float translate[3]); -void fm_mat4_from_srt_euler(fm_mat4_t *out, const float scale[3], const float rot[3], const float translate[3]); - - +/** + * @brief Apply rotation to a 4x4 matrix, specified by an axis of rotation and an angle. + */ +void fm_mat4_rotate(fm_mat4_t *out, const fm_vec3_t *axis, float angle); + +/** + * @brief Create an affine transformation matrix from scale, rotation and translation. + * + * The rotation is accepted as a quaternion. + */ +void fm_mat4_from_srt(fm_mat4_t *out, const fm_vec3_t *scale, const fm_quat_t *quat, const fm_vec3_t *translate); + +/** + * @brief Create an affine transformation matrix from scale, rotation and translation. + * + * The rotation is accepted as euler angles. + */ +void fm_mat4_from_srt_euler(fm_mat4_t *out, const fm_vec3_t *scale, const float rot[3], const fm_vec3_t *translate); + +/** + * @brief Multiply two 4x4 matrices. + */ inline void fm_mat4_mul(fm_mat4_t *out, const fm_mat4_t *a, const fm_mat4_t *b) { fm_mat4_t tmp; @@ -202,6 +362,9 @@ inline void fm_mat4_mul(fm_mat4_t *out, const fm_mat4_t *a, const fm_mat4_t *b) *out = tmp; } +/** + * @brief Transpose a 4x4 matrix. + */ inline void fm_mat4_transpose(fm_mat4_t *out, const fm_mat4_t *m) { for (int i = 0; i < 4; i++) @@ -213,10 +376,19 @@ inline void fm_mat4_transpose(fm_mat4_t *out, const fm_mat4_t *m) } } +/** + * @brief Compute the determinant of a 4x4 matrix. + */ float fm_mat4_det(const fm_mat4_t *m); +/** + * @brief Compute the inverse of a 4x4 matrix. + */ void fm_mat4_inverse(fm_mat4_t *out, const fm_mat4_t *m); +/** + * @brief Multiply a 3D vector by a 4x4 matrix by assuming 1 as the hypothetical 4th component of the vector. + */ inline void fm_mat4_mul_vec3(fm_vec4_t *out, const fm_mat4_t *m, const fm_vec3_t *v) { for (int i = 0; i < 4; i++) diff --git a/include/fmath.h b/include/fmath.h index 0da6b880a9..fd8193e241 100644 --- a/include/fmath.h +++ b/include/fmath.h @@ -66,8 +66,20 @@ extern "C" { #endif -#define FM_EPSILON 1e-4f -#define FM_PI 3.14159265358979f +/** + * @brief Error constant for approximate comparisons. + */ +#define FM_EPSILON 1e-4f + +/** + * @brief Single precision pi constant. + */ +#define FM_PI 3.14159265358979f + +/** + * @brief Convert degrees to radians. + */ +#define FM_DEG2RAD(deg) ((deg) * 0.01745329252f) /** @brief Reinterpret the bits composing a float as a int32. * @@ -202,11 +214,22 @@ void fm_sincosf(float x, float *sin, float *cos); */ float fm_atan2f(float y, float x); +/** + * @brief Linearly interpolate between two scalar values. + */ inline float fm_lerp(float a, float b, float t) { return a + (b - a) * t; } +/** + * @brief Linearly interpolate between two angles, using the shortest path. + * + * @param a The start angle in radians + * @param b The end angle in radians + * @param t The interpolation factor + * @return The interpolated angle. + */ float fm_lerp_angle(float a, float b, float t); #ifdef LIBDRAGON_FAST_MATH diff --git a/src/math/fgeom.c b/src/math/fgeom.c index 6f93d3e3b7..8c913573df 100644 --- a/src/math/fgeom.c +++ b/src/math/fgeom.c @@ -60,15 +60,7 @@ inline void fm_quat_mul(fm_quat_t *out, const fm_quat_t *a, const fm_quat_t *b) a->v[3] * b->v[3] - a->v[0] * b->v[0] - a->v[1] * b->v[1] - a->v[2] * b->v[2] }}; } -void fm_quat_from_euler_zxy(fm_quat_t *out, const float rot[3]) -{ - fm_quat_identity(out); - fm_quat_rotate(out, out, (float[]){0,0,1}, rot[0]); - fm_quat_rotate(out, out, (float[]){1,0,0}, rot[1]); - fm_quat_rotate(out, out, (float[]){0,1,0}, rot[2]); -} - -void fm_quat_rotate(fm_quat_t *out, const fm_quat_t *q, const float axis[3], float angle) +void fm_quat_rotate(fm_quat_t *out, const fm_quat_t *q, const fm_vec3_t *axis, float angle) { fm_quat_t r; fm_quat_from_rotation(&r, axis, angle); @@ -108,13 +100,13 @@ void fm_quat_slerp(fm_quat_t *out, const fm_quat_t *a, const fm_quat_t *b, float } -void fm_mat4_rotate(fm_mat4_t *out, const float axis[3], float angle) +void fm_mat4_rotate(fm_mat4_t *out, const fm_vec3_t *axis, float angle) { float s, c; fm_sincosf(angle, &s, &c); float t = 1.0f - c; - float x = axis[0], y = axis[1], z = axis[2]; + float x = axis->x, y = axis->y, z = axis->z; *out = (fm_mat4_t){{ { t * x * x + c, t * x * y - s * z, t * x * z + s * y, 0 }, @@ -124,24 +116,24 @@ void fm_mat4_rotate(fm_mat4_t *out, const float axis[3], float angle) }}; } -void fm_mat4_from_srt(fm_mat4_t *out, const float scale[3], const float quat[4], const float translate[3]) +void fm_mat4_from_srt(fm_mat4_t *out, const fm_vec3_t *scale, const fm_quat_t *quat, const fm_vec3_t *translate) { - float x = quat[0], y = quat[1], z = quat[2], w = quat[3]; + float x = quat->x, y = quat->y, z = quat->z, w = quat->w; float x2 = x + x, y2 = y + y, z2 = z + z; float xx = x * x2, xy = x * y2, xz = x * z2; float yy = y * y2, yz = y * z2, zz = z * z2; float wx = w * x2, wy = w * y2, wz = w * z2; - float sx = scale[0], sy = scale[1], sz = scale[2]; + float sx = scale->x, sy = scale->y, sz = scale->z; *out = (fm_mat4_t){{ { (1.0f - (yy + zz)) * sx, (xy + wz) * sx, (xz - wy) * sx, 0 }, { (xy - wz) * sy, (1.0f - (xx + zz)) * sy, (yz + wx) * sy, 0 }, { (xz + wy) * sz, (yz - wx) * sz, (1.0f - (xx + yy)) * sz, 0 }, - { translate[0], translate[1], translate[2], 1 } + { translate->x, translate->y, translate->z, 1 } }}; } -void fm_mat4_from_srt_euler(fm_mat4_t *out, const float scale[3], const float rot[3], const float translate[3]) +void fm_mat4_from_srt_euler(fm_mat4_t *out, const fm_vec3_t *scale, const float rot[3], const fm_vec3_t *translate) { float s0, c0, s1, c1, s2, c2; fm_sincosf(rot[0], &s0, &c0); @@ -149,10 +141,10 @@ void fm_mat4_from_srt_euler(fm_mat4_t *out, const float scale[3], const float ro fm_sincosf(rot[2], &s2, &c2); *out = (fm_mat4_t){{ - { scale[0] * c2 * c1, scale[0] * (c2 * s1 * s0 - s2 * c0), scale[0] * (c2 * s1 * c0 + s2 * s0), 0 }, - { scale[1] * s2 * c1, scale[1] * (s2 * s1 * s0 + c2 * c0), scale[1] * (s2 * s1 * c0 - c2 * s0), 0 }, - {-scale[2] * s1, scale[2] * c1 * s0, scale[2] * c1 * c0, 0 }, - { translate[0], translate[1], translate[2], 1 } + { scale->x * c2 * c1, scale->x * (c2 * s1 * s0 - s2 * c0), scale->x * (c2 * s1 * c0 + s2 * s0), 0 }, + { scale->y * s2 * c1, scale->y * (s2 * s1 * s0 + c2 * c0), scale->y * (s2 * s1 * c0 - c2 * s0), 0 }, + {-scale->z * s1, scale->z * c1 * s0, scale->z * c1 * c0, 0 }, + { translate->x, translate->y, translate->z, 1 } }}; }