Havok: Add quaternion multiplication

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));
+}

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();
 

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};
 }