ksys/phys: Add RigidBodyAccessor

lib/hkStubs/Havok/Common/Base/Math/Matrix/hkMatrix3f.h

@@ -6,7 +6,44 @@ class hkMatrix3f {
 public:
     hkMatrix3f() {}  // NOLINT(modernize-use-equals-default)
 
+    HK_FORCE_INLINE hkFloat32& operator()(int row, int col);
+    HK_FORCE_INLINE const hkFloat32& operator()(int row, int col) const;
+
+    HK_FORCE_INLINE hkVector4f& getColumn(int i);
+    HK_FORCE_INLINE const hkVector4f& getColumn(int i) const;
+
+    HK_FORCE_INLINE void getRows(hkVector4f& r0, hkVector4f& r1, hkVector4f& r2) const;
+
     hkVector4f m_col0;
     hkVector4f m_col1;
     hkVector4f m_col2;
 };
+
+hkFloat32& hkMatrix3f::operator()(int row, int col) {
+    return getColumn(col)(row);
+}
+
+const hkFloat32& hkMatrix3f::operator()(int row, int col) const {
+    return getColumn(col)(row);
+}
+
+hkVector4f& hkMatrix3f::getColumn(int i) {
+    return (&m_col0)[i];
+}
+
+const hkVector4f& hkMatrix3f::getColumn(int i) const {
+    return (&m_col0)[i];
+}
+
+inline void hkMatrix3f::getRows(hkVector4f& r0, hkVector4f& r1, hkVector4f& r2) const {
+    hkVector4f c0;
+    c0.set(m_col0(0), m_col1(0), m_col2(0));
+    hkVector4f c1;
+    c1.set(m_col0(1), m_col1(1), m_col2(1));
+    hkVector4f c2;
+    c2.set(m_col0(2), m_col1(2), m_col2(2));
+
+    r0 = c0;
+    r1 = c1;
+    r2 = c2;
+}

lib/hkStubs/Havok/Common/Base/Math/Quaternion/hkQuaternionf.h

@@ -5,6 +5,16 @@
 class hkQuaternionf {
 public:
     hkQuaternionf() {}  // NOLINT(modernize-use-equals-default)
+    HK_FORCE_INLINE hkQuaternionf(hkFloat32 ix, hkFloat32 iy, hkFloat32 iz, hkFloat32 r);
+    HK_FORCE_INLINE void set(hkFloat32 ix, hkFloat32 iy, hkFloat32 iz, hkFloat32 r);
 
     hkVector4f m_vec;
 };
+
+inline hkQuaternionf::hkQuaternionf(hkFloat32 ix, hkFloat32 iy, hkFloat32 iz, hkFloat32 r) {
+    set(ix, iy, iz, r);
+}
+
+inline void hkQuaternionf::set(hkFloat32 ix, hkFloat32 iy, hkFloat32 iz, hkFloat32 r) {
+    m_vec.set(ix, iy, iz, r);
+}

lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h

@@ -23,6 +23,7 @@ public:
     HK_FORCE_INLINE hkVector4f() {}
     HK_FORCE_INLINE hkVector4f(hkFloat32 x, hkFloat32 y, hkFloat32 z, hkFloat32 w = 0);
     HK_FORCE_INLINE hkVector4f(const hkVector4f& other);
+    HK_FORCE_INLINE hkVector4f& operator=(hkVector4fParameter) = default;
 
     // ========== Vector initialization
 
@@ -75,8 +76,23 @@ public:
 
     // ========== Comparisons
 
+    HK_FORCE_INLINE hkVector4fComparison less(hkVector4fParameter a) const;
+    HK_FORCE_INLINE hkVector4fComparison lessEqual(hkVector4fParameter a) const;
+    HK_FORCE_INLINE hkVector4fComparison greater(hkVector4fParameter a) const;
+    HK_FORCE_INLINE hkVector4fComparison greaterEqual(hkVector4fParameter a) const;
+    HK_FORCE_INLINE hkVector4fComparison equal(hkVector4fParameter a) const;
+    HK_FORCE_INLINE hkVector4fComparison notEqual(hkVector4fParameter a) const;
+    HK_FORCE_INLINE hkVector4fComparison lessZero() const;
+    HK_FORCE_INLINE hkVector4fComparison lessEqualZero() const;
+    HK_FORCE_INLINE hkVector4fComparison greaterZero() const;
+    HK_FORCE_INLINE hkVector4fComparison greaterEqualZero() const;
+    HK_FORCE_INLINE hkVector4fComparison equalZero() const;
+    HK_FORCE_INLINE hkVector4fComparison notEqualZero() const;
+
     // ========== Sign, comparisons, clamping
 
+    void setAbs(hkVector4fParameter a);
+
     // ========== Matrix operations (out-of-line)
 
     void setRotatedDir(const hkMatrix3f& a, hkVector4fParameter b);
@@ -93,8 +109,21 @@ public:
 
     // ========== Component access
 
-    hkSimdFloat32 operator()(int i) const { return v[i]; }
-    hkSimdFloat32 operator[](int i) const { return v[i]; }
+    hkFloat32& operator()(int i) { return reinterpret_cast<float*>(&v)[i]; }
+    hkFloat32& operator[](int i) { return reinterpret_cast<float*>(&v)[i]; }
+    const hkFloat32& operator()(int i) const { return reinterpret_cast<const float*>(&v)[i]; }
+    const hkFloat32& operator[](int i) const { return reinterpret_cast<const float*>(&v)[i]; }
+
+    hkSimdFloat32 getComponent(int i) const { return v[i]; }
+    hkSimdFloat32 getX() const { return getComponent(0); }
+    hkSimdFloat32 getY() const { return getComponent(1); }
+    hkSimdFloat32 getZ() const { return getComponent(2); }
+    hkSimdFloat32 getW() const { return getComponent(3); }
+    void setComponent(int i, hkSimdFloat32Parameter val) { v[i] = val; }
+    void setX(hkSimdFloat32Parameter val) { setComponent(0, val); }
+    void setY(hkSimdFloat32Parameter val) { setComponent(1, val); }
+    void setZ(hkSimdFloat32Parameter val) { setComponent(2, val); }
+    void setW(hkSimdFloat32Parameter val) { setComponent(3, val); }
 
     // ========== Load/store
 

lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.inl

@@ -63,7 +63,7 @@ inline void hkVector4f::mul(hkSimdFloat32Parameter a) {
 
 inline void hkVector4f::setMul(hkVector4fParameter a, hkSimdFloat32Parameter r) {
 #ifdef HK_VECTOR4F_AARCH64_NEON
-    v = vmulq_n_f32(v, r);
+    v = vmulq_n_f32(a.v, r);
 #else
     v *= r.val();
 #endif
@@ -75,7 +75,7 @@ inline void hkVector4f::setMul(hkSimdFloat32Parameter r, hkVector4fParameter a)
 
 inline void hkVector4f::setAdd(hkVector4fParameter a, hkSimdFloat32Parameter b) {
 #ifdef HK_VECTOR4F_AARCH64_NEON
-    v = vaddq_f32(v, vdupq_n_f32(b));
+    v = vaddq_f32(a.v, vdupq_n_f32(b));
 #else
     v += b.val();
 #endif
@@ -83,7 +83,7 @@ inline void hkVector4f::setAdd(hkVector4fParameter a, hkSimdFloat32Parameter b)
 
 inline void hkVector4f::setSub(hkVector4fParameter a, hkSimdFloat32Parameter b) {
 #ifdef HK_VECTOR4F_AARCH64_NEON
-    v = vsubq_f32(v, vdupq_n_f32(b));
+    v = vsubq_f32(a.v, vdupq_n_f32(b));
 #else
     v -= b.val();
 #endif
@@ -167,12 +167,31 @@ inline void hkVector4f::setCross(hkVector4fParameter a, hkVector4fParameter b) {
     //	x = a[1] * b[2] - b[1] * a[2]
     //	y = a[2] * b[0] - b[2] * a[0]
     //      ----   ----   ----   ----
-    //        a     bb     b      aa
-
+    //        1      2      3      4
+#ifdef HK_VECTOR4F_AARCH64_NEON
+    // Shuffle a and b to get the column vectors 2 and 4
+    const auto a_yzwx = vextq_f32(a.v, a.v, 1);
+    const auto b_yzwx = vextq_f32(b.v, b.v, 1);
+    const auto a_yz = vget_low_f32(a_yzwx);
+    const auto b_yz = vget_low_f32(b_yzwx);
+    const auto a_wx = vget_high_f32(a_yzwx);
+    const auto b_wx = vget_high_f32(b_yzwx);
+    const auto a_xw = vrev64_f32(a_wx);
+    const auto b_xw = vrev64_f32(b_wx);
+    const auto a_yzxw = vcombine_f32(a_yz, a_xw);
+    const auto b_yzxw = vcombine_f32(b_yz, b_xw);
+    const auto cross = (a.v * b_yzxw) - (a_yzxw * b.v);
+    // Shuffle `cross` back to the correct order (zxyw -> xyzw)
+    const auto cross_yzwx = vextq_f32(cross, cross, 1);
+    const auto c_xy = vget_low_f32(cross);
+    const auto c_yz = vget_low_f32(cross_yzwx);
+    v = vcombine_f32(c_yz, c_xy);
+#else
     auto cross0 = a.v * __builtin_shufflevector(b.v, b.v, 1, 2, 0, 3);
     auto cross1 = b.v * __builtin_shufflevector(a.v, a.v, 1, 2, 0, 3);
     auto diff = cross0 - cross1;
     v = __builtin_shufflevector(diff, diff, 1, 2, 0, 3);
+#endif
 }
 
 inline void hkVector4f::setInterpolate(hkVector4fParameter v0, hkVector4fParameter v1,
@@ -183,6 +202,63 @@ inline void hkVector4f::setInterpolate(hkVector4fParameter v0, hkVector4fParamet
     setAddMul(v0, diff, t);
 }
 
+inline hkVector4fComparison hkVector4f::less(hkVector4fParameter a) const {
+    return hkVector4fComparison::convert(v < a.v);
+}
+
+inline hkVector4fComparison hkVector4f::lessEqual(hkVector4fParameter a) const {
+    return hkVector4fComparison::convert(v <= a.v);
+}
+
+inline hkVector4fComparison hkVector4f::greater(hkVector4fParameter a) const {
+    return hkVector4fComparison::convert(v > a.v);
+}
+
+inline hkVector4fComparison hkVector4f::greaterEqual(hkVector4fParameter a) const {
+    return hkVector4fComparison::convert(v >= a.v);
+}
+
+inline hkVector4fComparison hkVector4f::equal(hkVector4fParameter a) const {
+    return hkVector4fComparison::convert(v == a.v);
+}
+
+inline hkVector4fComparison hkVector4f::notEqual(hkVector4fParameter a) const {
+    return hkVector4fComparison::convert(v != a.v);
+}
+
+inline hkVector4fComparison hkVector4f::lessZero() const {
+    return hkVector4fComparison::convert(v < m128());
+}
+
+inline hkVector4fComparison hkVector4f::lessEqualZero() const {
+    return hkVector4fComparison::convert(v <= m128());
+}
+
+inline hkVector4fComparison hkVector4f::greaterZero() const {
+    return hkVector4fComparison::convert(v > m128());
+}
+
+inline hkVector4fComparison hkVector4f::greaterEqualZero() const {
+    return hkVector4fComparison::convert(v >= m128());
+}
+
+inline hkVector4fComparison hkVector4f::equalZero() const {
+    return hkVector4fComparison::convert(v == m128());
+}
+
+inline hkVector4fComparison hkVector4f::notEqualZero() const {
+    return hkVector4fComparison::convert(v != m128());
+}
+
+inline void hkVector4f::setAbs(hkVector4fParameter a) {
+#ifdef HK_VECTOR4F_AARCH64_NEON
+    v = vabsq_f32(a.v);
+#else
+    for (int i = 0; i < 4; ++i)
+        v[i] = std::abs(a[i]);
+#endif
+}
+
 inline void hkVector4f::_setRotatedDir(const hkMatrix3f& a, hkVector4fParameter b) {
 #ifdef HK_VECTOR4F_AARCH64_NEON
     auto col0 = vmulq_laneq_f32(a.m_col0.v, v, 0);

lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4fComparison.h

@@ -15,6 +15,33 @@ class hkVector4fComparison {
 public:
     HK_DECLARE_CLASS_ALLOCATOR(hkVector4fComparison)
 
+    enum Mask {
+        INDEX_W = 3,
+        INDEX_Z = 2,
+        INDEX_Y = 1,
+        INDEX_X = 0,
+
+        MASK_NONE = 0x0,              // 0000
+        MASK_W = (1 << INDEX_W),      // 0001
+        MASK_Z = (1 << INDEX_Z),      // 0010
+        MASK_ZW = (MASK_Z | MASK_W),  // 0011
+
+        MASK_Y = (1 << INDEX_Y),        // 0100
+        MASK_YW = (MASK_Y | MASK_W),    // 0101
+        MASK_YZ = (MASK_Y | MASK_Z),    // 0110
+        MASK_YZW = (MASK_YZ | MASK_W),  // 0111
+
+        MASK_X = (1 << INDEX_X),        // 1000
+        MASK_XW = (MASK_X | MASK_W),    // 1001
+        MASK_XZ = (MASK_X | MASK_Z),    // 1010
+        MASK_XZW = (MASK_XZ | MASK_W),  // 1011
+
+        MASK_XY = (MASK_X | MASK_Y),     // 1100
+        MASK_XYW = (MASK_XY | MASK_W),   // 1101
+        MASK_XYZ = (MASK_XY | MASK_Z),   // 1110
+        MASK_XYZW = (MASK_XY | MASK_ZW)  // 1111
+    };
+
     HK_FORCE_INLINE static hkVector4fComparison convert(const m128& x) { return {x}; }
 
     HK_FORCE_INLINE void setAnd(hkVector4fComparisonParameter a, hkVector4fComparisonParameter b);
@@ -28,6 +55,14 @@ public:
                                    hkVector4fComparisonParameter trueValue,
                                    hkVector4fComparisonParameter falseValue);
 
+    template <Mask m = Mask::MASK_XYZW>
+    HK_FORCE_INLINE hkBool32 allAreSet() const;
+    HK_FORCE_INLINE hkBool32 allAreSet(Mask m) const;
+
+    template <Mask M = Mask::MASK_XYZW>
+    HK_FORCE_INLINE hkBool32 anyIsSet() const;
+    HK_FORCE_INLINE hkBool32 anyIsSet(Mask m) const;
+
     m128u m_mask;
 };
 
@@ -64,3 +99,25 @@ inline void hkVector4fComparison::setSelect(hkVector4fComparisonParameter comp,
     m_mask = (comp.m_mask & trueValue.m_mask) | (comp.m_mask & ~falseValue.m_mask);
 #endif
 }
+
+template <hkVector4fComparison::Mask m>
+inline hkBool32 hkVector4fComparison::allAreSet() const {
+    return ((m & MASK_X) == 0 || m_mask[0] != 0) && ((m & MASK_Y) == 0 || m_mask[1] != 0) &&
+           ((m & MASK_Z) == 0 || m_mask[2] != 0) && ((m & MASK_W) == 0 || m_mask[3] != 0);
+}
+
+inline hkBool32 hkVector4fComparison::allAreSet(hkVector4fComparison::Mask m) const {
+    return ((m & MASK_X) == 0 || m_mask[0] != 0) && ((m & MASK_Y) == 0 || m_mask[1] != 0) &&
+           ((m & MASK_Z) == 0 || m_mask[2] != 0) && ((m & MASK_W) == 0 || m_mask[3] != 0);
+}
+
+template <hkVector4fComparison::Mask m>
+inline hkBool32 hkVector4fComparison::anyIsSet() const {
+    return ((m & MASK_X) != 0 && m_mask[0] != 0) || ((m & MASK_Y) != 0 && m_mask[1] != 0) ||
+           ((m & MASK_Z) != 0 && m_mask[2] != 0) || ((m & MASK_W) != 0 && m_mask[3] != 0);
+}
+
+inline hkBool32 hkVector4fComparison::anyIsSet(Mask m) const {
+    return ((m & MASK_X) != 0 && m_mask[0] != 0) || ((m & MASK_Y) != 0 && m_mask[1] != 0) ||
+           ((m & MASK_Z) != 0 && m_mask[2] != 0) || ((m & MASK_W) != 0 && m_mask[3] != 0);
+}

lib/hkStubs/Havok/Physics2012/Dynamics/Motion/hkpMotion.h

@@ -177,7 +177,7 @@ inline const hkVector4& hkpMotion::getLinearVelocity() const {
 }
 
 inline const hkVector4& hkpMotion::getAngularVelocity() const {
-    return m_linearVelocity;
+    return m_angularVelocity;
 }
 
 inline void hkpMotion::getPointVelocity(const hkVector4& p, hkVector4& vecOut) const {