From 91dbd90f856e0db41ce1872fd9222e0bae642d4b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?L=C3=A9o=20Lam?= <leo@leolam.fr>
Date: Sat, 15 Jan 2022 21:40:41 +0100
Subject: [PATCH] Havok: Add quaternion multiplication

---
 .../Base/Math/Quaternion/hkQuaternionf.h      | 35 +++++++++++++++++++
 .../Common/Base/Math/Vector/hkVector4f.h      |  2 ++
 .../Common/Base/Math/Vector/hkVector4f.inl    | 20 +++++++++++
 3 files changed, 57 insertions(+)

diff --git a/lib/hkStubs/Havok/Common/Base/Math/Quaternion/hkQuaternionf.h b/lib/hkStubs/Havok/Common/Base/Math/Quaternion/hkQuaternionf.h
index 93ff87d1..3619fc13 100644
--- a/lib/hkStubs/Havok/Common/Base/Math/Quaternion/hkQuaternionf.h
+++ b/lib/hkStubs/Havok/Common/Base/Math/Quaternion/hkQuaternionf.h
@@ -16,6 +16,12 @@ public:
     HK_FORCE_INLINE const hkFloat32& operator()(int i) const;
     template <int I>
     HK_FORCE_INLINE hkSimdFloat32 getComponent() const;
+    HK_FORCE_INLINE hkFloat32 getReal() const;
+    HK_FORCE_INLINE hkSimdFloat32 getRealPart() const;
+    HK_FORCE_INLINE const hkVector4f& getImag() const;
+
+    HK_FORCE_INLINE void mul(hkQuaternionfParameter q);
+    HK_FORCE_INLINE void setMul(hkQuaternionfParameter q0, hkQuaternionfParameter q1);
 
     hkVector4f m_vec;
 };
@@ -41,3 +47,32 @@ template <int I>
 inline hkSimdFloat32 hkQuaternionf::getComponent() const {
     return m_vec.getComponent<I>();
 }
+
+inline hkFloat32 hkQuaternionf::getReal() const {
+    return m_vec(3);
+}
+
+inline hkSimdFloat32 hkQuaternionf::getRealPart() const {
+    return m_vec.getW();
+}
+
+inline const hkVector4f& hkQuaternionf::getImag() const {
+    return m_vec;
+}
+
+inline void hkQuaternionf::mul(hkQuaternionfParameter q) {
+    setMul(*this, q);
+}
+
+inline void hkQuaternionf::setMul(hkQuaternionfParameter r, hkQuaternionfParameter q) {
+    const auto rImag = r.getImag();
+    const auto qImag = q.getImag();
+    const auto rReal = r.getRealPart();
+    const auto qReal = q.getRealPart();
+
+    hkVector4f vec;
+    vec.setCross(rImag, qImag);
+    vec.addMul(rReal, qImag);
+    vec.addMul(qReal, rImag);
+    m_vec.setXYZ_W(vec, (rReal * qReal) - rImag.dot<3>(qImag));
+}
diff --git a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h b/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h
index 15cde5af..cc95443d 100644
--- a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h
+++ b/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h
@@ -28,6 +28,8 @@ public:
     // ========== Vector initialization
 
     HK_FORCE_INLINE void set(hkFloat32 x, hkFloat32 y, hkFloat32 z, hkFloat32 w = 0);
+    HK_FORCE_INLINE void setXYZ(hkVector4fParameter xyz);
+    HK_FORCE_INLINE void setXYZ_W(hkVector4fParameter xyz, hkSimdFloat32Parameter w);
     HK_FORCE_INLINE void setAll(hkFloat32 x);
     HK_FORCE_INLINE void setZero();
 
diff --git a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.inl b/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.inl
index f4bbe66e..3b3bcec2 100644
--- a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.inl
+++ b/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.inl
@@ -31,6 +31,26 @@ inline void hkVector4f::set(hkFloat32 x, hkFloat32 y, hkFloat32 z, hkFloat32 w)
 #endif
 }
 
+inline void hkVector4f::setXYZ(hkVector4fParameter xyz) {
+#ifdef HK_VECTOR4F_AARCH64_NEON
+    v = vsetq_lane_f32(vgetq_lane_f32(v, 3), xyz.v, 3);
+#else
+    auto newValue = xyz.v;
+    newValue[3] = v[3];
+    v = newValue;
+#endif
+}
+
+inline void hkVector4f::setXYZ_W(hkVector4fParameter xyz, hkSimdFloat32Parameter w) {
+#ifdef HK_VECTOR4F_AARCH64_NEON
+    v = vsetq_lane_f32(w, xyz.v, 3);
+#else
+    auto newValue = xyz.v;
+    newValue[3] = w;
+    v = newValue;
+#endif
+}
+
 inline void hkVector4f::setAll(hkReal x) {
     v = {x, x, x, x};
 }