Havok: Implement quat and vector normalisation

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,

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<HK_QUADREAL_EPS_SQRD>());
+}
+
+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<const hkQuaternionf&>(g_vectorfConstants[HK_QUADREAL_0001]);
 }

lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h

@@ -135,9 +135,28 @@ public:
     template <int N>
     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 <int N>
     HK_FORCE_INLINE hkSimdFloat32 lengthSquared() const;
 
+    /// Get the inverse length (1/|v|) of this vector as if it had N components.
+    template <int N>
+    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 <int N>
+    HK_FORCE_INLINE hkSimdFloat32 lengthInverseUnsafe() const;
+
+    /// Normalize this vector as if it had N components.
+    template <int N>
+    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 <int N>
+    HK_FORCE_INLINE void normalizeUnsafe();
+
     // ========== Misc
 
     // ========== Component access

lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.inl

@@ -432,6 +432,28 @@ inline hkSimdFloat32 hkVector4f::lengthSquared() const {
     return dot<N>(*this);
 }
 
+template <int N>
+inline hkSimdFloat32 hkVector4f::lengthInverse() const {
+    const hkSimdFloat32 len = lengthSquared<N>();
+    return len.sqrtInverse();
+}
+
+template <int N>
+inline hkSimdFloat32 hkVector4f::lengthInverseUnsafe() const {
+    const hkSimdFloat32 len = lengthSquared<N>();
+    return len.sqrtInverseUnsafe();
+}
+
+template <int N>
+inline void hkVector4f::normalize() {
+    mul(lengthInverse<N>());
+}
+
+template <int N>
+inline void hkVector4f::normalizeUnsafe() {
+    mul(lengthInverseUnsafe<N>());
+}
+
 template <int Constant>
 inline const hkVector4f& hkVector4f::getConstant() {
     return reinterpret_cast<const hkVector4f&>(g_vectorfConstants[Constant]);