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 +#include 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 + 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 +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([] { + int mask = 0; + for (int i = 0; i < N; ++i) + mask |= 1 << i; + return mask; + }()); + return diff.lessEqual(epsilon_v).allAreSet(); + } +} + inline void hkVector4f::setAbs(hkVector4fParameter a) { #ifdef HK_VECTOR4F_AARCH64_NEON v = vabsq_f32(a.v);