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