From 82286fa560b8e1e556e4184b69d98179c70f6c4c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?L=C3=A9o=20Lam?= Date: Fri, 28 Jan 2022 20:07:34 +0100 Subject: [PATCH] Havok: Add hkVector4f::_setRotatedDir --- .../Common/Base/Math/Vector/hkVector4f.h | 3 ++ .../Common/Base/Math/Vector/hkVector4f.inl | 32 +++++++++++++++++++ 2 files changed, 35 insertions(+) diff --git a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h b/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h index e276a66b..bc1014e0 100644 --- a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h +++ b/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h @@ -119,6 +119,9 @@ public: // ========== Matrix operations (inline) HK_FORCE_INLINE void _setRotatedDir(const hkMatrix3f& a, hkVector4fParameter b); + HK_FORCE_INLINE void _setRotatedDir(hkQuaternionfParameter quat, hkVector4fParameter vec); + HK_FORCE_INLINE void _setRotatedInverseDir(hkQuaternionfParameter quat, + hkVector4fParameter vec); HK_FORCE_INLINE void _setTransformedPos(const hkTransformf& a, hkVector4fParameter b); // ========== Length and normalization diff --git a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.inl b/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.inl index fa4e6ce0..e094d5be 100644 --- a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.inl +++ b/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.inl @@ -382,6 +382,38 @@ inline void hkVector4f::_setRotatedDir(const hkMatrix3f& a, hkVector4fParameter #endif } +inline void hkVector4f::_setRotatedDir(hkQuaternionfParameter quat, hkVector4fParameter vec) { + // Rodrigues' rotation formula + + const auto& u = quat.getImag(); + const hkSimdFloat32 s = quat.getRealPart(); + + hkVector4f result; + result.setMul(vec, s * s - hkSimdFloat32::getConstant()); + result.addMul(u, u.dot<3>(vec)); + + hkVector4f cross; + cross.setCross(u, vec); + result.addMul(cross, s); + + setAdd(result, result); +} + +inline void hkVector4f::_setRotatedInverseDir(hkQuaternionfParameter quat, hkVector4fParameter vec) { + const auto& u = quat.getImag(); + const hkSimdFloat32 s = quat.getRealPart(); + + hkVector4f result; + result.setMul(vec, s * s - hkSimdFloat32::getConstant()); + result.addMul(u, u.dot<3>(vec)); + + hkVector4f cross; + cross.setCross(vec, u); + result.addMul(cross, s); + + setAdd(result, result); +} + inline void hkVector4f::_setTransformedPos(const hkTransformf& a, hkVector4fParameter b) { hkVector4f t; t._setRotatedDir(a.getRotation(), b);