diff --git a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h b/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h index bc1014e0..c6b8390f 100644 --- a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h +++ b/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h @@ -156,6 +156,9 @@ public: // ========== Misc + /// Dot product of this vector and (a.x, a.y, a.z, 1). + hkSimdFloat32 dot4xyz1(hkVector4fParameter a) const; + // ========== Component access hkFloat32& operator()(int i) { return reinterpret_cast(&v)[i]; } diff --git a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.inl b/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.inl index e094d5be..853c8ae7 100644 --- a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.inl +++ b/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.inl @@ -399,7 +399,8 @@ inline void hkVector4f::_setRotatedDir(hkQuaternionfParameter quat, hkVector4fPa setAdd(result, result); } -inline void hkVector4f::_setRotatedInverseDir(hkQuaternionfParameter quat, hkVector4fParameter vec) { +inline void hkVector4f::_setRotatedInverseDir(hkQuaternionfParameter quat, + hkVector4fParameter vec) { const auto& u = quat.getImag(); const hkSimdFloat32 s = quat.getRealPart(); @@ -484,6 +485,20 @@ inline void hkVector4f::normalizeUnsafe() { mul(lengthInverseUnsafe()); } +inline hkSimdFloat32 hkVector4f::dot4xyz1(hkVector4fParameter a) const { +#ifdef HK_VECTOR4F_AARCH64_NEON + float32x4_t x2 = v * a.v; + float32x2_t low = vget_low_f32(x2); + float32x2_t high = vget_high_f32(x2); + high = vset_lane_f32(vgetq_lane_f32(v, 3), high, 1); + float32x2_t xy_zw = vpadd_f32(low, high); + float32x2_t xyzw = vpadd_f32(xy_zw, xy_zw); + return xyzw; +#else + return (v[0] * a.v[0]) + (v[1] * a.v[1]) + (v[2] * a.v[2]) + v[3]; +#endif +} + template inline const hkVector4f& hkVector4f::getConstant() { return reinterpret_cast(g_vectorfConstants[Constant]);