From a3bf47bd9d533ecac2726b6d8b76a38b0ff8221a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?L=C3=A9o=20Lam?= Date: Fri, 28 Jan 2022 15:39:50 +0100 Subject: [PATCH] Havok: Add hkQuaternionf::setMulInverse/setInverseMul --- .../Base/Math/Quaternion/hkQuaternionf.h | 30 ++++++++++++++++++- 1 file changed, 29 insertions(+), 1 deletion(-) diff --git a/lib/hkStubs/Havok/Common/Base/Math/Quaternion/hkQuaternionf.h b/lib/hkStubs/Havok/Common/Base/Math/Quaternion/hkQuaternionf.h index 53414d17..32c6c901 100644 --- a/lib/hkStubs/Havok/Common/Base/Math/Quaternion/hkQuaternionf.h +++ b/lib/hkStubs/Havok/Common/Base/Math/Quaternion/hkQuaternionf.h @@ -26,8 +26,14 @@ public: HK_FORCE_INLINE hkFloat32 getAngle() const; hkSimdFloat32 getAngleSr() const; + /// self = self * q HK_FORCE_INLINE void mul(hkQuaternionfParameter q); - HK_FORCE_INLINE void setMul(hkQuaternionfParameter q0, hkQuaternionfParameter q1); + /// self = r * q + HK_FORCE_INLINE void setMul(hkQuaternionfParameter r, hkQuaternionfParameter q); + /// self = r * q^-1 + HK_FORCE_INLINE void setMulInverse(hkQuaternionfParameter r, hkQuaternionfParameter q); + /// self = r^-1 * q + HK_FORCE_INLINE void setInverseMul(hkQuaternionfParameter r, hkQuaternionfParameter q); HK_FORCE_INLINE void setInverse(hkQuaternionfParameter q); @@ -103,6 +109,28 @@ inline void hkQuaternionf::setMul(hkQuaternionfParameter r, hkQuaternionfParamet m_vec.setXYZ_W(vec, (rReal * qReal) - rImag.dot<3>(qImag)); } +inline void hkQuaternionf::setMulInverse(hkQuaternionfParameter r, hkQuaternionfParameter q) { + const auto rImag = r.getImag(); + const auto qImag = q.getImag(); + + hkVector4f vec; + vec.setCross(qImag, rImag); + vec.subMul(r.getRealPart(), qImag); + vec.addMul(q.getRealPart(), rImag); + m_vec.setXYZ_W(vec, rImag.dot<4>(qImag)); +} + +inline void hkQuaternionf::setInverseMul(hkQuaternionfParameter r, hkQuaternionfParameter q) { + const auto rImag = r.getImag(); + const auto qImag = q.getImag(); + + hkVector4f vec; + vec.setCross(qImag, rImag); + vec.addMul(r.getRealPart(), qImag); + vec.subMul(q.getRealPart(), rImag); + m_vec.setXYZ_W(vec, rImag.dot<4>(qImag)); +} + inline void hkQuaternionf::setInverse(const hkQuaternionf& q) { m_vec.setNeg<3>(q.getImag()); }