Havok: Add hkVector4f::store

Fixes a matching issue in physCapsuleShape

1 files changed, 36 insertions, 0 deletions

diff --git a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h b/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h
index a6fbc982..e8a6039b 100644
--- a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h
+++ b/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h
@@ -4,6 +4,11 @@
 #include <Havok/Common/Base/Types/hkBaseDefs.h>
 #include <Havok/Common/Base/Types/hkBaseTypes.h>
 
+#ifdef __aarch64__
+#include <arm_neon.h>
+#define HK_VECTOR4F_AARCH64_NEON
+#endif
+
 class hkVector4f {
 public:
     HK_DECLARE_CLASS_ALLOCATOR(hkVector4f)
@@ -20,6 +25,10 @@ public:
 
     void sub_7100FABE80(const hkVector4f&, const hkVector4f&);
 
+    /// Store N floats to out.
+    template <int N>
+    void store(hkFloat32* out) const;
+
     m128 v;
 };
 
@@ -40,3 +49,30 @@ inline void hkVector4f::set(hkFloat32 x, hkFloat32 y, hkFloat32 z, hkFloat32 w)
 inline void hkVector4f::setAll(hkReal x) {
     v = {x, x, x, x};
 }
+
+template <int N>
+inline void hkVector4f::store(hkFloat32* out) const {
+    static_assert(1 <= N && N <= 4, "invalid N");
+#ifdef HK_VECTOR4F_AARCH64_NEON
+    switch (N) {
+    case 1:
+        vst1q_lane_f32(out, v, 0);
+        break;
+    case 2:
+        vst1_f32(out, vget_low_f32(v));
+        break;
+    case 3:
+        vst1_f32(out, vget_low_f32(v));
+        vst1q_lane_f32(out + 2, v, 2);
+        break;
+    case 4:
+        vst1q_f32(out, v);
+        break;
+    default:
+        break;
+    }
+#else
+    for (int i = 0; i < N; ++i)
+        p[i] = v[i];
+#endif
+}