From 1c105db5bc0dcd1f750a7381616005c82614b1fc Mon Sep 17 00:00:00 2001
From: Jan <jan@laupetin.net>
Date: Fri, 10 May 2024 22:55:22 +0200
Subject: [PATCH] fix: quaternion multiplication and division

i hope its correct at least, quaternions are not my strength
---
 src/ObjWriting/XModel/Gltf/GltfWriter.cpp |  2 +-
 src/Utils/Math/Quaternion.h               | 65 ++++++++++++++++++-----
 2 files changed, 53 insertions(+), 14 deletions(-)

diff --git a/src/ObjWriting/XModel/Gltf/GltfWriter.cpp b/src/ObjWriting/XModel/Gltf/GltfWriter.cpp
index 2f148aad..4a63bc43 100644
--- a/src/ObjWriting/XModel/Gltf/GltfWriter.cpp
+++ b/src/ObjWriting/XModel/Gltf/GltfWriter.cpp
@@ -204,7 +204,7 @@ namespace
                 {
                     const auto& parentBone = xmodel.m_bones[bone.parentIndex];
                     translation -= Vector3f(parentBone.globalOffset[0], parentBone.globalOffset[2], -parentBone.globalOffset[1]);
-                    rotation -= Quaternion32(
+                    rotation /= Quaternion32(
                         parentBone.globalRotation.m_x, parentBone.globalRotation.m_z, -parentBone.globalRotation.m_y, parentBone.globalRotation.m_w);
                 }
                 rotation.Normalize();
diff --git a/src/Utils/Math/Quaternion.h b/src/Utils/Math/Quaternion.h
index f2a88e55..b69cc414 100644
--- a/src/Utils/Math/Quaternion.h
+++ b/src/Utils/Math/Quaternion.h
@@ -57,12 +57,41 @@ public:
         return static_cast<T>((q1.m_x * q2.m_x) + (q1.m_y * q2.m_y) + (q1.m_z * q2.m_z) + (q1.m_w * q2.m_w));
     }
 
-    T lengthSquared()
+    static Quaternion& conj(Quaternion& result)
+    {
+        result.m_x = -result.m_x;
+        result.m_y = -result.m_y;
+        result.m_z = -result.m_z;
+        return result;
+    }
+
+    static Quaternion& invert(Quaternion& result)
+    {
+        // from game programming gems p198
+        // do result = conj( q ) / norm( q )
+        Quaternion::conj(result);
+
+        // return if norm() is near 0 (divide by 0 would result in NaN)
+        T l = result.lengthSquared();
+        if (l < static_cast<T>(0.0001))
+        {
+            return result;
+        }
+
+        T l_inv = static_cast<T>(1.0) / l;
+        result.m_x *= l_inv;
+        result.m_y *= l_inv;
+        result.m_z *= l_inv;
+        result.m_w *= l_inv;
+        return result;
+    }
+
+    T lengthSquared() const
     {
         return Quaternion::dot(*this, *this);
     }
 
-    T length()
+    T length() const
     {
         return sqrt(lengthSquared());
     }
@@ -114,31 +143,41 @@ public:
 
     friend Quaternion operator*(const Quaternion& lhs, const Quaternion& rhs)
     {
-        return Quaternion(lhs.m_x + rhs.m_x, lhs.m_y + rhs.m_y, lhs.m_z + rhs.m_z, lhs.m_w + rhs.m_w);
+        const T x2 = lhs.m_w * rhs.m_x + lhs.m_x * rhs.m_w + lhs.m_y * rhs.m_z - lhs.m_z * rhs.m_y;
+        const T y2 = lhs.m_w * rhs.m_y + lhs.m_y * rhs.m_w + lhs.m_z * rhs.m_x - lhs.m_x * rhs.m_z;
+        const T z2 = lhs.m_w * rhs.m_z + lhs.m_z * rhs.m_w + lhs.m_x * rhs.m_y - lhs.m_y * rhs.m_x;
+        const T w2 = lhs.m_w * rhs.m_w - lhs.m_x * rhs.m_x - lhs.m_y * rhs.m_y - lhs.m_z * rhs.m_z;
+
+        return Quaternion(x2, y2, z2, w2);
     }
 
     friend Quaternion operator/(const Quaternion& lhs, const Quaternion& rhs)
     {
-        return Quaternion(lhs.m_x - rhs.m_x, lhs.m_y - rhs.m_y, lhs.m_z - rhs.m_z, lhs.m_w - rhs.m_w);
+        Quaternion rhsInv = rhs;
+        Quaternion::invert(rhsInv);
+        return lhs * rhsInv;
     }
 
     friend Quaternion& operator*=(Quaternion& lhs, const Quaternion& rhs)
     {
-        lhs.m_x += rhs.m_x;
-        lhs.m_y += rhs.m_y;
-        lhs.m_z += rhs.m_z;
-        lhs.m_w += rhs.m_w;
+        const T x2 = lhs.m_w * rhs.m_x + lhs.m_x * rhs.m_w + lhs.m_y * rhs.m_z - lhs.m_z * rhs.m_y;
+        const T y2 = lhs.m_w * rhs.m_y + lhs.m_y * rhs.m_w + lhs.m_z * rhs.m_x - lhs.m_x * rhs.m_z;
+        const T z2 = lhs.m_w * rhs.m_z + lhs.m_z * rhs.m_w + lhs.m_x * rhs.m_y - lhs.m_y * rhs.m_x;
+        const T w2 = lhs.m_w * rhs.m_w - lhs.m_x * rhs.m_x - lhs.m_y * rhs.m_y - lhs.m_z * rhs.m_z;
+
+        lhs.m_x = x2;
+        lhs.m_y = y2;
+        lhs.m_z = z2;
+        lhs.m_w = w2;
 
         return lhs;
     }
 
     friend Quaternion& operator/=(Quaternion& lhs, const Quaternion& rhs)
     {
-        lhs.m_x -= rhs.m_x;
-        lhs.m_y -= rhs.m_y;
-        lhs.m_z -= rhs.m_z;
-        lhs.m_w -= rhs.m_w;
-
+        Quaternion rhsInv = rhs;
+        Quaternion::invert(rhsInv);
+        lhs *= rhsInv;
         return lhs;
     }
 };