1 files changed, 141 insertions, 0 deletions
diff --git a/mons_math/src/quat.c b/mons_math/src/quat.c
new file mode 100644
index 0000000..7c6a1cd
--- /dev/null
+++ b/mons_math/src/quat.c
@@ -0,0 +1,141 @@
+#include "mons_math/quat.h"
+#include "mons_math/mat4.h"
+#include "mons_math/vec3.h"
+#include "mons_math/vec4.h"
+#include <math.h>
+#include <stdlib.h>
+#include <stdio.h>
+
+mons_quat mons_quat_from_axis_angle(mons_vec3 axis, float angle) {
+    float half_angle = 0.5f * angle;
+    float s = sinf(half_angle);
+    return (mons_quat){
+        axis.x * s,
+        axis.y * s,
+        axis.z * s,
+        cosf(half_angle),
+    };
+}
+
+mons_quat mons_quat_mul(mons_quat a, mons_quat b) {
+    return (mons_quat){
+        (a.w * b.x) + (a.x * b.w) + (a.y * b.z) - (a.z * b.y),
+        (a.w * b.y) + (a.y * b.w) + (a.z * b.x) - (a.x * b.z),
+        (a.w * b.z) + (a.z * b.w) + (a.x * b.y) - (a.y * b.x),
+        (a.w * b.w) - (a.x * b.x) - (a.y * b.y) - (a.z * b.z),
+    };
+}
+
+void mons_quat_mul_inplace(mons_quat *a, mons_quat b) {
+    *a = mons_quat_mul(*a, b);
+}
+
+mons_quat mons_quat_mul_f(mons_quat a, float b) {
+    return (mons_quat){
+        a.x * b,
+        a.y * b,
+        a.z * b,
+        a.w * b,
+    };
+}
+
+void mons_quat_mul_f_inplace(mons_quat *a, float b) {
+    a->x *= b;
+    a->y *= b;
+    a->z *= b;
+    a->w *= b;
+}
+
+mons_quat mons_quat_mul_i(mons_quat a, int b) {
+    return (mons_quat){
+        a.x * b,
+        a.y * b,
+        a.z * b,
+        a.w * b,
+    };
+}
+
+void mons_quat_mul_i_inplace(mons_quat *a, int b) {
+    a->x *= b;
+    a->y *= b;
+    a->z *= b;
+    a->w *= b;
+}
+
+mons_mat4 mons_quat_to_mat4(mons_quat a) {
+    return (mons_mat4){
+        {
+            1.0 - (2.0 * a.y * a.y) - (2.0 * a.z * a.z),
+            (2.0 * a.x * a.y) - (2.0 * a.w * a.z),
+            (2.0 * a.x * a.z) + (2.0 * a.w * a.y),
+            0.0,
+        },
+        {
+            (2.0 * a.x * a.y) + (2.0 * a.w * a.z),
+            1.0 - (2.0 * a.x * a.x) - (2.0 * a.z * a.z),
+            (2.0 * a.y * a.z) - (2.0 * a.w * a.x),
+            0.0,
+        },
+        {
+            (2.0 * a.x * a.z) - (2.0 * a.w * a.y),
+            (2.0 * a.y * a.z) + (2.0 * a.w * a.x),
+            1.0 - (2.0 * a.x * a.x) - (2.0 * a.y * a.y),
+            0.0,
+        },
+        {
+            0.0,
+            0.0,
+            0.0,
+            1.0,
+        },
+    };
+}
+
+mons_quat mons_quat_normalize(mons_quat a) {
+    mons_vec4 normalized = mons_vec4_normalize(*((mons_vec4 *)&a));
+    return *((mons_quat *)&normalized);
+}
+
+void mons_quat_normalize_inplace(mons_quat *a) {
+    mons_vec4_normalize_inplace((mons_vec4 *)a);
+}
+
+mons_vec3 mons_quat_transform_vec3(mons_quat quat, mons_vec3 vec) {
+    mons_vec3 quat_vec = (mons_vec3){quat.x, quat.y, quat.z};
+    float s = quat.w;
+
+    return mons_vec3_add(
+        mons_vec3_add(
+            mons_vec3_mul_f(quat_vec, mons_vec3_dot(quat_vec, vec) * 2.0f),
+            mons_vec3_mul_f(vec, s * s - mons_vec3_dot(quat_vec, quat_vec))),
+        mons_vec3_mul_f(mons_vec3_cross(quat_vec, vec), 2.0f * s));
+}
+
+mons_quat mons_quat_conjugate(mons_quat quat) {
+    return (mons_quat){
+        .x = -quat.x,
+        .y = -quat.y,
+        .z = -quat.z,
+        .w = quat.w,
+    };
+}
+
+mons_quat mons_quat_looking_at(mons_vec3 eye, mons_vec3 target, mons_vec3 up) {
+    mons_vec3 f = mons_vec3_normalize(mons_vec3_sub(target, eye));
+    mons_vec3 side = mons_vec3_normalize(mons_vec3_cross(up, f));
+    mons_vec3 b = mons_vec3_normalize(mons_vec3_add(f, MONS_VEC3_NEG_Z));
+    mons_quat p = mons_quat_from_axis_angle(mons_vec3_cross(b, f), mons_vec3_dot(b,f));
+    mons_vec3 r = (mons_vec3) {
+        p.w*p.w + p.x*p.x - p.y*p.y - p.z*p.z,
+        (2.0f * p.x * p.y) - (2.0f * p.w * p.z),
+        (2.0f * p.x * p.z) + (2.0f * p.w * p.y),
+    };
+    b = mons_vec3_normalize(mons_vec3_add(side, r));
+    mons_quat q = mons_quat_from_axis_angle(mons_vec3_cross(b,side), mons_vec3_dot(b, side));
+
+    return mons_quat_mul(p, q);
+}
+
+void mons_quat_print(mons_quat quat) {
+    mons_vec4_print(*(mons_vec4*)&quat);
+}