diff --git a/Makefile b/Makefile index 8244791d40..24394a58db 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 \ @@ -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 @@ -120,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 diff --git a/include/fgeom.h b/include/fgeom.h new file mode 100644 index 0000000000..72f2c1901a --- /dev/null +++ b/include/fgeom.h @@ -0,0 +1,407 @@ +/** + * @file fgeom.h + * @brief Structs and functions for common 3D geometry calculations. + * @ingroup fastmath + */ + +#ifndef LIBDRAGON_FGEOM_H +#define LIBDRAGON_FGEOM_H + +#include +#include +#include +#include "fmath.h" + +#ifdef __cplusplus +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; ///< Named fields to access individual vector components directly. + }; + 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; ///< Named fields to access individual vector components directly. + }; + 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; ///< Named fields to access individual quaternion components directly. + }; + 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]; ///< 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; + fm_vec3_sub(&diff, a, 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); + if (len < FM_EPSILON) { + *out = (fm_vec3_t){}; + return; + } + *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, + a->z * b->x - a->x * b->z, + 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]); + +/** + * @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->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); + +/** + * @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)); + if (len < FM_EPSILON) { + fm_quat_identity(out); + return; + } + *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){}; + out->m[0][0] = 1; + out->m[1][1] = 1; + out->m[2][2] = 1; + out->m[3][3] = 1; +} + +/** + * @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] *= 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; +} + +/** + * @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] += translate->v[i]; +} + +/** + * @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; + 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; +} + +/** + * @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++) + { + for (int j = 0; j < 4; j++) + { + out->m[i][j] = m->m[j][i]; + } + } +} + +/** + * @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++) + { + 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]; + } +} + +#ifdef __cplusplus +} +#endif + +#endif \ No newline at end of file diff --git a/include/fmath.h b/include/fmath.h index a597bc6d81..fd8193e241 100644 --- a/include/fmath.h +++ b/include/fmath.h @@ -66,6 +66,21 @@ extern "C" { #endif +/** + * @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. * * This version is type-punning safe and produces optimal code when optimizing. @@ -199,6 +214,24 @@ 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 #define truncf(x) fm_truncf(x) #define floorf(x) fm_floorf(x) 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..8c913573df --- /dev/null +++ b/src/math/fgeom.c @@ -0,0 +1,231 @@ +#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_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); + 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 fm_vec3_t *axis, float angle) +{ + float s, c; + fm_sincosf(angle, &s, &c); + + float t = 1.0f - c; + 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 }, + { 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 fm_vec3_t *scale, const fm_quat_t *quat, const fm_vec3_t *translate) +{ + 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->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->x, translate->y, translate->z, 1 } + }}; +} + +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); + fm_sincosf(rot[1], &s1, &c1); + fm_sincosf(rot[2], &s2, &c2); + + *out = (fm_mat4_t){{ + { 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 } + }}; +} + +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; + } + } +} diff --git a/src/fmath.c b/src/math/fmath.c similarity index 94% rename from src/fmath.c rename to src/math/fmath.c index d4291e09f2..d85bdc3b1a 100644 --- a/src/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; +}