From 58a5a63a38ef8a489635a362b081581d5a8dbc67 Mon Sep 17 00:00:00 2001
From: dbalatoni13 <40299962+dbalatoni13@users.noreply.github.com>
Date: Mon, 14 Apr 2025 16:18:48 +0200
Subject: [PATCH] Implement C_Quat functions

---
 src/dolphin/mtx/quat.c | 32 ++++++++++++++++++++++++++++----
 1 file changed, 28 insertions(+), 4 deletions(-)

diff --git a/src/dolphin/mtx/quat.c b/src/dolphin/mtx/quat.c
index 166400b9..610b640a 100644
--- a/src/dolphin/mtx/quat.c
+++ b/src/dolphin/mtx/quat.c
@@ -33,7 +33,10 @@ void PSQUATAdd(register const Quaternion *p, register const Quaternion *q, regis
 #ifdef TARGET_PC
 void C_QUATMultiply(const Quaternion *a, const Quaternion *b, Quaternion *ab)
 {
-    // TODO PC
+    ab->x = a->w * b->x + a->x * b->w + a->y * b->z - a->z * b->y;
+    ab->y = a->w * b->y - a->x * b->z + a->y * b->w + a->z * b->x;
+    ab->z = a->w * b->z + a->x * b->y - a->y * b->x + a->z * b->w;
+    ab->w = a->w * b->w - a->x * b->x - a->y * b->y - a->z * b->z;
 }
 #endif
 
@@ -68,9 +71,19 @@ void PSQUATMultiply(register const Quaternion *a, register const Quaternion *b,
 #endif
 
 #ifdef TARGET_PC
-void C_QUATNormalize(register Quaternion *src, Quaternion *unit)
+void C_QUATNormalize(const Quaternion *src, Quaternion *unit)
 {
-    // TODO PC
+    float len = sqrtf(src->x * src->x + src->y * src->y + src->z * src->z + src->w * src->w);
+    if (len > 0.0f) {
+        float inv_len = 1.0f / len;
+        unit->x = src->x * inv_len;
+        unit->y = src->y * inv_len;
+        unit->z = src->z * inv_len;
+        unit->w = src->w * inv_len;
+    } else {
+        unit->x = unit->y = unit->z = 0.0f;
+        unit->w = 1.0f;
+    }
 }
 #endif
 
@@ -119,7 +132,18 @@ void PSQUATNormalize(const register Quaternion *src, register Quaternion *unit)
 #ifdef TARGET_PC
 void C_QUATInverse(const Quaternion *src, Quaternion *inv)
 {
-    // TODO PC
+    float len_squared = src->x * src->x + src->y * src->y + src->z * src->z + src->w * src->w;
+
+    if (len_squared > 0.0f) {
+        float inv_len_squared = 1.0f / len_squared;
+        inv->x = -src->x * inv_len_squared;
+        inv->y = -src->y * inv_len_squared;
+        inv->z = -src->z * inv_len_squared;
+        inv->w = src->w * inv_len_squared;
+    } else {
+        inv->x = inv->y = inv->z = 0.0f;
+        inv->w = 1.0f;
+    }
 }
 #endif