ksys/phys: Add more RigidBody functions

And fix a bunch of hkVector4f / hkSimdFloat32 interop matching issues.

5 files changed, 37 insertions, 15 deletions

diff --git a/lib/hkStubs/Havok/Common/Base/Math/Matrix/hkTransformf.h b/lib/hkStubs/Havok/Common/Base/Math/Matrix/hkTransformf.h
index 2c27cd33..175b3a0c 100644
--- a/lib/hkStubs/Havok/Common/Base/Math/Matrix/hkTransformf.h
+++ b/lib/hkStubs/Havok/Common/Base/Math/Matrix/hkTransformf.h
@@ -21,6 +21,8 @@ public:
 
     HK_FORCE_INLINE void set(const hkRotationf& r, hkVector4fParameter t);
     HK_FORCE_INLINE void set(hkQuaternionfParameter q, hkVector4fParameter t);
+
+    HK_FORCE_INLINE static const hkTransformf& getIdentity();
     HK_FORCE_INLINE void setIdentity();
 
     hkRotationf m_rotation;
@@ -56,6 +58,10 @@ inline void hkTransformf::set(const hkQuaternionf& q, const hkVector4f& t) {
     m_translation = t;
 }
 
+inline const hkTransformf& hkTransformf::getIdentity() {
+    return reinterpret_cast<const hkTransformf&>(g_vectorfConstants[HK_QUADREAL_1000]);
+}
+
 inline void hkTransformf::setIdentity() {
     m_rotation.setIdentity();
     m_translation.setZero();
diff --git a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkSimdFloat32.h b/lib/hkStubs/Havok/Common/Base/Math/Vector/hkSimdFloat32.h
index fc4633e9..ebb3a3eb 100644
--- a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkSimdFloat32.h
+++ b/lib/hkStubs/Havok/Common/Base/Math/Vector/hkSimdFloat32.h
@@ -60,6 +60,8 @@ public:
 
     void setAbs(hkSimdFloat32Parameter x);
 
+    HK_FORCE_INLINE m128 toQuad() const;
+
     Storage m_real;
 };
 
@@ -150,3 +152,11 @@ inline void hkSimdFloat32::setAbs(hkSimdFloat32Parameter x) {
         m_real[i] = std::abs(x.m_real[i]);
 #endif
 }
+
+inline m128 hkSimdFloat32::toQuad() const {
+#ifdef HK_SIMD_FLOAT32_AARCH64_NEON
+    return vcombine_f32(m_real, m_real);
+#else
+    return m_real;
+#endif
+}
diff --git a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h b/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h
index 62a1ae12..598ca3a7 100644
--- a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h
+++ b/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h
@@ -31,6 +31,7 @@ public:
     HK_FORCE_INLINE void setXYZ(hkVector4fParameter xyz);
     HK_FORCE_INLINE void setXYZ_W(hkVector4fParameter xyz, hkSimdFloat32Parameter w);
     HK_FORCE_INLINE void setAll(hkFloat32 x);
+    HK_FORCE_INLINE void setAll(hkSimdFloat32Parameter x);
     HK_FORCE_INLINE void setZero();
 
     // ========== Vector operations
diff --git a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.inl b/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.inl
index ed42e7fe..fb00d055 100644
--- a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.inl
+++ b/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.inl
@@ -55,6 +55,10 @@ inline void hkVector4f::setAll(hkReal x) {
     v = {x, x, x, x};
 }
 
+inline void hkVector4f::setAll(hkSimdFloat32Parameter x) {
+    v = x.toQuad();
+}
+
 inline void hkVector4f::setZero() {
     setAll(0);
 }
@@ -96,11 +100,7 @@ inline void hkVector4f::mul(hkSimdFloat32Parameter a) {
 }
 
 inline void hkVector4f::setMul(hkVector4fParameter a, hkSimdFloat32Parameter r) {
-#ifdef HK_VECTOR4F_AARCH64_NEON
-    v = vmulq_n_f32(a.v, r);
-#else
-    v *= r.val();
-#endif
+    v = a.v * r.toQuad();
 }
 
 inline void hkVector4f::setMul(hkSimdFloat32Parameter r, hkVector4fParameter a) {
@@ -108,19 +108,11 @@ inline void hkVector4f::setMul(hkSimdFloat32Parameter r, hkVector4fParameter a)
 }
 
 inline void hkVector4f::setAdd(hkVector4fParameter a, hkSimdFloat32Parameter b) {
-#ifdef HK_VECTOR4F_AARCH64_NEON
-    v = vaddq_f32(a.v, vdupq_n_f32(b));
-#else
-    v += b.val();
-#endif
+    v = a.v + b.toQuad();
 }
 
 inline void hkVector4f::setSub(hkVector4fParameter a, hkSimdFloat32Parameter b) {
-#ifdef HK_VECTOR4F_AARCH64_NEON
-    v = vsubq_f32(a.v, vdupq_n_f32(b));
-#else
-    v -= b.val();
-#endif
+    v = a.v - b.toQuad();
 }
 
 inline void hkVector4f::setReciprocal(hkVector4fParameter a) {
diff --git a/lib/hkStubs/Havok/Common/Base/Types/Geometry/Aabb/hkAabb.h b/lib/hkStubs/Havok/Common/Base/Types/Geometry/Aabb/hkAabb.h
index 07084ded..2a9a79a1 100644
--- a/lib/hkStubs/Havok/Common/Base/Types/Geometry/Aabb/hkAabb.h
+++ b/lib/hkStubs/Havok/Common/Base/Types/Geometry/Aabb/hkAabb.h
@@ -11,6 +11,19 @@ public:
     hkAabb() {}
     HK_FORCE_INLINE hkAabb(const hkVector4& min, const hkVector4& max) : m_min(min), m_max(max) {}
 
+    HK_FORCE_INLINE void getExtents(hkVector4& e) const;
+    HK_FORCE_INLINE void getCenter(hkVector4& center) const;
+
     hkVector4 m_min;
     hkVector4 m_max;
 };
+
+inline void hkAabb::getExtents(hkVector4& e) const {
+    e.setSub(m_max, m_min);
+}
+
+inline void hkAabb::getCenter(hkVector4& center) const {
+    hkVector4 s;
+    s.setAdd(m_min, m_max);
+    center.setMul(s, hkSimdReal::getConstant<HK_QUADREAL_INV_2>());
+}