diff --git a/lib/hkStubs/Havok/Common/Base/Math/Header/hkMathHeaderConstants.h b/lib/hkStubs/Havok/Common/Base/Math/Header/hkMathHeaderConstants.h index 37689d33..18c7b2f5 100644 --- a/lib/hkStubs/Havok/Common/Base/Math/Header/hkMathHeaderConstants.h +++ b/lib/hkStubs/Havok/Common/Base/Math/Header/hkMathHeaderConstants.h @@ -38,7 +38,9 @@ enum hkVectorConstant { HK_QUADREAL_0001, HK_QUADREAL_MAX, HK_QUADREAL_HIGH, + /// Epsilon. HK_QUADREAL_EPS, + /// Epsilon squared. HK_QUADREAL_EPS_SQRD, HK_QUADREAL_MIN, HK_QUADREAL_MINUS_MAX, diff --git a/lib/hkStubs/Havok/Common/Base/Math/Quaternion/hkQuaternionf.h b/lib/hkStubs/Havok/Common/Base/Math/Quaternion/hkQuaternionf.h index 157c8b93..53414d17 100644 --- a/lib/hkStubs/Havok/Common/Base/Math/Quaternion/hkQuaternionf.h +++ b/lib/hkStubs/Havok/Common/Base/Math/Quaternion/hkQuaternionf.h @@ -20,6 +20,9 @@ public: HK_FORCE_INLINE hkSimdFloat32 getRealPart() const; HK_FORCE_INLINE const hkVector4f& getImag() const; + HK_FORCE_INLINE hkBool32 hasValidAxis() const; + /// @warning This should only be called if hasValidAxis() is true. + HK_FORCE_INLINE void getAxis(hkVector4f& axis) const; HK_FORCE_INLINE hkFloat32 getAngle() const; hkSimdFloat32 getAngleSr() const; @@ -28,6 +31,8 @@ public: HK_FORCE_INLINE void setInverse(hkQuaternionfParameter q); + HK_FORCE_INLINE void normalize(); + HK_FORCE_INLINE static const hkQuaternionf& getIdentity(); hkVector4f m_vec; @@ -67,6 +72,16 @@ inline const hkVector4f& hkQuaternionf::getImag() const { return m_vec; } +inline hkBool32 hkQuaternionf::hasValidAxis() const { + return m_vec.lengthSquared<3>().isGreater(hkSimdFloat32::getConstant()); +} + +void hkQuaternionf::getAxis(hkVector4f& axis) const { + hkVector4f result = getImag(); + result.normalize<3>(); + axis.setFlipSign(result, getRealPart().lessZero()); +} + inline hkFloat32 hkQuaternionf::getAngle() const { return getAngleSr().val(); } @@ -92,6 +107,10 @@ inline void hkQuaternionf::setInverse(const hkQuaternionf& q) { m_vec.setNeg<3>(q.getImag()); } +inline void hkQuaternionf::normalize() { + m_vec.normalizeUnsafe<4>(); +} + inline const hkQuaternionf& hkQuaternionf::getIdentity() { return reinterpret_cast(g_vectorfConstants[HK_QUADREAL_0001]); } diff --git a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h b/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h index b4504c79..54556ae6 100644 --- a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h +++ b/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h @@ -135,9 +135,28 @@ public: template HK_FORCE_INLINE void setDot(hkVector4fParameter a, hkVector4fParameter b); + /// Get the squared length (|v|^2) of this vector as if it had N components. template HK_FORCE_INLINE hkSimdFloat32 lengthSquared() const; + /// Get the inverse length (1/|v|) of this vector as if it had N components. + template + HK_FORCE_INLINE hkSimdFloat32 lengthInverse() const; + + /// Get the inverse length (1/|v|) of this vector as if it had N components. + /// Does not check for negative sqrt values or divide-by-zero. + template + HK_FORCE_INLINE hkSimdFloat32 lengthInverseUnsafe() const; + + /// Normalize this vector as if it had N components. + template + HK_FORCE_INLINE void normalize(); + + /// Normalize this vector as if it had N components. + /// Does not check for negative sqrt values or divide-by-zero. + template + HK_FORCE_INLINE void normalizeUnsafe(); + // ========== Misc // ========== Component access diff --git a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.inl b/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.inl index b11ce178..fd137b5f 100644 --- a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.inl +++ b/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.inl @@ -432,6 +432,28 @@ inline hkSimdFloat32 hkVector4f::lengthSquared() const { return dot(*this); } +template +inline hkSimdFloat32 hkVector4f::lengthInverse() const { + const hkSimdFloat32 len = lengthSquared(); + return len.sqrtInverse(); +} + +template +inline hkSimdFloat32 hkVector4f::lengthInverseUnsafe() const { + const hkSimdFloat32 len = lengthSquared(); + return len.sqrtInverseUnsafe(); +} + +template +inline void hkVector4f::normalize() { + mul(lengthInverse()); +} + +template +inline void hkVector4f::normalizeUnsafe() { + mul(lengthInverseUnsafe()); +} + template inline const hkVector4f& hkVector4f::getConstant() { return reinterpret_cast(g_vectorfConstants[Constant]);