1 files changed, 112 insertions, 0 deletions
diff --git a/mons_3d/src/transform.c b/mons_3d/src/transform.c
new file mode 100644
index 0000000..c4435fc
--- /dev/null
+++ b/mons_3d/src/transform.c
@@ -0,0 +1,112 @@
+#include <math.h>
+#include "transform.h"
+#include "mons_math/vec3.h"
+#include "mons_math/quat.h"
+#include <stdio.h>
+
+mons_mat4 translation_to_mat4(mons_vec3 translation) {
+    mons_mat4 result = MONS_MAT4_IDENTITY;
+    result.m1.w = translation.x;
+    result.m2.w = translation.y;
+    result.m3.w = translation.z;
+    return result;
+}
+
+mons_mat4 scale_to_mat4(mons_vec3 scale) {
+    mons_mat4 result = MONS_MAT4_IDENTITY;
+    result.m1.x = scale.x;
+    result.m2.y = scale.y;
+    result.m3.z = scale.z;
+    return result;
+}
+
+void mons_transform_translate(mons_transform *transform,
+                              mons_vec3 translation) {
+    mons_vec3_add_inplace(&transform->translation, translation);
+}
+
+void mons_transform_rotate(mons_transform *transform, mons_quat rotation) {
+    mons_quat_mul_inplace(&transform->rotation, rotation);
+}
+
+void mons_transform_scale(mons_transform *transform, mons_vec3 scale) {
+    mons_vec3_mul_memberwise_inplace(&transform->scale, scale);
+}
+
+mons_mat4 mons_transform_matrix(mons_transform transform) {
+    mons_mat4 translation = translation_to_mat4(transform.translation);
+    mons_mat4 result = translation;
+    mons_mat4_mul_inplace(&result, mons_quat_to_mat4(transform.rotation));
+    mons_mat4_mul_inplace(&result, scale_to_mat4(transform.scale));
+    return result;
+}
+
+mons_vec3 mons_transform_forward(mons_transform transform) {
+    return mons_quat_transform_vec3(transform.rotation, MONS_VEC3_Z);
+}
+
+mons_vec3 mons_transform_up(mons_transform transform) {
+    return mons_quat_transform_vec3(transform.rotation, MONS_VEC3_Y);
+}
+
+mons_vec3 mons_transform_right(mons_transform transform) {
+    return mons_quat_transform_vec3(transform.rotation, MONS_VEC3_X);
+}
+
+void mons_transform_look_at(mons_transform *transform, mons_vec3 point, mons_vec3 up) {
+    mons_vec3 forward = mons_vec3_normalize(mons_vec3_sub(transform->translation, point));
+    mons_vec3 right = mons_vec3_normalize(mons_vec3_cross(up, forward));
+    mons_vec3 new_up = mons_vec3_cross(forward, right);
+    float m00 = right.x;
+    float m01 = right.y;
+    float m02 = right.z;
+    float m10 = new_up.x;
+    float m11 = new_up.y;
+    float m12 = new_up.z;
+    float m20 = forward.x;
+    float m21 = forward.y;
+    float m22 = forward.z;
+
+    float num8 = (m00 + m11) + m22;
+    mons_quat quaternion = {0};
+    if (num8 > 0.0f) {
+        float num = sqrt(num8 + 1.0f);
+        quaternion.w = num * 0.5f;
+        num = 0.5f / num;
+        quaternion.x = (m12 - m21) * num;
+        quaternion.y = (m20 - m02) * num;
+        quaternion.z = (m01 - m10) * num;
+        transform->rotation = quaternion;
+        return;
+    }
+    if ((m00 >= m11) && (m00 >= m22))
+    {
+        float num7 = (float)sqrt(((1.0f + m00) - m11) - m22);
+        float num4 = 0.5f / num7;
+        quaternion.x = 0.5f * num7;
+        quaternion.y = (m01 + m10) * num4;
+        quaternion.z = (m02 + m20) * num4;
+        quaternion.w = (m12 - m21) * num4;
+        transform->rotation = quaternion;
+        return;
+    }
+    if (m11 > m22)
+    {
+        float num6 = (float)sqrt(((1.0f + m11) - m00) - m22);
+        float num3 = 0.5f / num6;
+        quaternion.x = (m10 + m01) * num3;
+        quaternion.y = 0.5f * num6;
+        quaternion.z = (m21 + m12) * num3;
+        quaternion.w = (m20 - m02) * num3;
+        transform->rotation = quaternion;
+        return;
+    }
+    float num5 = (float)sqrt(((1.0f + m22) - m00) - m11);
+    float num2 = 0.5f / num5;
+    quaternion.x = (m20 + m02) * num2;
+    quaternion.y = (m21 + m12) * num2;
+    quaternion.z = 0.5f * num5;
+    quaternion.w = (m01 - m10) * num2;
+    transform->rotation = quaternion;
+    return;
+}