Havok: Add hkVector4f::allEqual

3 files changed, 40 insertions, 0 deletions

diff --git a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkSimdFloat32.h b/lib/hkStubs/Havok/Common/Base/Math/Vector/hkSimdFloat32.h
index 87f2c901..2a481f46 100644
--- a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkSimdFloat32.h
+++ b/lib/hkStubs/Havok/Common/Base/Math/Vector/hkSimdFloat32.h
@@ -1,6 +1,7 @@
 #pragma once
 
 #include <Havok/Common/Base/Types/hkBaseTypes.h>
+#include <cmath>
 
 using hkSimdFloat32Parameter = class hkSimdFloat32;
 
@@ -11,5 +12,11 @@ public:
     operator float() const { return val(); }  // NOLINT(google-explicit-constructor)
     hkFloat32 val() const { return m_real; }
 
+    void setAbs(hkSimdFloat32Parameter x);
+
     hkFloat32 m_real;
 };
+
+inline void hkSimdFloat32::setAbs(hkSimdFloat32Parameter x) {
+    m_real = std::abs(x.m_real);
+}
diff --git a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h b/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h
index dbba2bc8..4241a235 100644
--- a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h
+++ b/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h
@@ -89,6 +89,12 @@ public:
     HK_FORCE_INLINE hkVector4fComparison equalZero() const;
     HK_FORCE_INLINE hkVector4fComparison notEqualZero() const;
 
+    /// Whether the first N components of this vector are within `epsilon`
+    /// of the corresponding components in `v`.
+    template <int N>
+    HK_FORCE_INLINE hkBool32 allEqual(hkVector4fParameter rhs,
+                                      hkSimdFloat32Parameter epsilon) const;
+
     // ========== Sign, comparisons, clamping
 
     void setAbs(hkVector4fParameter a);
diff --git a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.inl b/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.inl
index 640b5f47..ae0fc39e 100644
--- a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.inl
+++ b/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.inl
@@ -250,6 +250,33 @@ inline hkVector4fComparison hkVector4f::notEqualZero() const {
     return hkVector4fComparison::convert(v != m128());
 }
 
+template <int N>
+inline hkBool32 hkVector4f::allEqual(hkVector4fParameter rhs,
+                                     hkSimdFloat32Parameter epsilon) const {
+    static_assert(1 <= N && N <= 4, "invalid N");
+
+    if constexpr (N == 1) {
+        hkSimdFloat32 t = getX() - rhs.getX();
+        t.setAbs(t);
+        return t <= epsilon;
+
+    } else {
+        hkVector4f diff;
+        diff.setSub(*this, rhs);
+        diff.setAbs(diff);
+        hkVector4f epsilon_v;
+        epsilon_v.setAll(epsilon);
+
+        constexpr auto mask = static_cast<hkVector4fComparison::Mask>([] {
+            int mask = 0;
+            for (int i = 0; i < N; ++i)
+                mask |= 1 << i;
+            return mask;
+        }());
+        return diff.lessEqual(epsilon_v).allAreSet<mask>();
+    }
+}
+
 inline void hkVector4f::setAbs(hkVector4fParameter a) {
 #ifdef HK_VECTOR4F_AARCH64_NEON
     v = vabsq_f32(a.v);