From 6ef3bb9327809cf82fc15ab8c06dfd28c34a065d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?L=C3=A9o=20Lam?= Date: Sun, 9 Jan 2022 13:04:40 +0100 Subject: [PATCH] ksys/phys: Add RigidBodyAccessor --- data/uking_functions.csv | 26 ++--- .../Common/Base/Math/Matrix/hkMatrix3f.h | 37 +++++++ .../Base/Math/Quaternion/hkQuaternionf.h | 10 ++ .../Common/Base/Math/Vector/hkVector4f.h | 33 ++++++- .../Common/Base/Math/Vector/hkVector4f.inl | 86 +++++++++++++++- .../Base/Math/Vector/hkVector4fComparison.h | 57 +++++++++++ .../Physics2012/Dynamics/Motion/hkpMotion.h | 2 +- lib/sead | 2 +- src/KingSystem/Physics/CMakeLists.txt | 4 + .../RigidBody/Shape/physCapsuleShape.cpp | 5 +- .../Physics/RigidBody/physRigidBody.cpp | 4 +- .../Physics/RigidBody/physRigidBody.h | 11 +-- .../RigidBody/physRigidBodyAccessor.cpp | 99 +++++++++++++++++++ .../Physics/RigidBody/physRigidBodyAccessor.h | 44 +++++++++ src/KingSystem/Physics/System/physDefines.h | 1 + src/KingSystem/Physics/physConversions.h | 86 ++++++++++++++++ 16 files changed, 472 insertions(+), 35 deletions(-) create mode 100644 src/KingSystem/Physics/RigidBody/physRigidBodyAccessor.cpp create mode 100644 src/KingSystem/Physics/RigidBody/physRigidBodyAccessor.h create mode 100644 src/KingSystem/Physics/physConversions.h diff --git a/data/uking_functions.csv b/data/uking_functions.csv index 5c8cf9f0..a67f9fba 100644 --- a/data/uking_functions.csv +++ b/data/uking_functions.csv @@ -83519,19 +83519,19 @@ Address,Quality,Size,Name 0x0000007100fa9c4c,U,000068, 0x0000007100fa9c90,U,000288, 0x0000007100fa9db0,U,000092, -0x0000007100fa9e0c,U,000024, -0x0000007100fa9e24,U,000004,nullsub_4237 -0x0000007100fa9e28,U,000004,j__ZdlPv_1001 -0x0000007100fa9e2c,U,000044, -0x0000007100fa9e58,U,000032, -0x0000007100fa9e78,U,000016, -0x0000007100fa9e88,U,000112,physicsXXXGetMtx -0x0000007100fa9ef8,U,000032,physicsXXXGetVelocity -0x0000007100fa9f18,U,000032, -0x0000007100fa9f38,U,000116, -0x0000007100fa9fac,U,000120, -0x0000007100faa024,U,000020, -0x0000007100faa038,U,000064, +0x0000007100fa9e0c,O,000024,_ZN4ksys4phys17RigidBodyAccessorC1EP12hkpRigidBody +0x0000007100fa9e24,O,000004,_ZN4ksys4phys17RigidBodyAccessorD1Ev +0x0000007100fa9e28,O,000004,_ZN4ksys4phys17RigidBodyAccessorD0Ev +0x0000007100fa9e2c,O,000044,_ZNK4ksys4phys17RigidBodyAccessor13getMotionTypeEv +0x0000007100fa9e58,O,000032,_ZNK4ksys4phys17RigidBodyAccessor11getPositionEPN4sead7Vector3IfEE +0x0000007100fa9e78,O,000016,_ZNK4ksys4phys17RigidBodyAccessor11getRotationEPN4sead4QuatIfEE +0x0000007100fa9e88,O,000112,_ZNK4ksys4phys17RigidBodyAccessor12getTransformEPN4sead8Matrix34IfEE +0x0000007100fa9ef8,O,000032,_ZNK4ksys4phys17RigidBodyAccessor17getLinearVelocityEPN4sead7Vector3IfEE +0x0000007100fa9f18,O,000032,_ZNK4ksys4phys17RigidBodyAccessor18getAngularVelocityEPN4sead7Vector3IfEE +0x0000007100fa9f38,O,000116,_ZNK4ksys4phys17RigidBodyAccessor22isVelocityGreaterEqualEf +0x0000007100fa9fac,O,000120,_ZNK4ksys4phys17RigidBodyAccessor16getPointVelocityEPN4sead7Vector3IfEERKS4_ +0x0000007100faa024,O,000020,_ZNK4ksys4phys17RigidBodyAccessor13getTimeFactorEv +0x0000007100faa038,O,000064,_ZNK4ksys4phys17RigidBodyAccessor20getDeltaCenterOfMassEPN4sead7Vector3IfEES5_ 0x0000007100faa078,U,000564, 0x0000007100faa2ac,U,000224, 0x0000007100faa3bc,U,000364, diff --git a/lib/hkStubs/Havok/Common/Base/Math/Matrix/hkMatrix3f.h b/lib/hkStubs/Havok/Common/Base/Math/Matrix/hkMatrix3f.h index 3046e638..2c758615 100644 --- a/lib/hkStubs/Havok/Common/Base/Math/Matrix/hkMatrix3f.h +++ b/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; +} diff --git a/lib/hkStubs/Havok/Common/Base/Math/Quaternion/hkQuaternionf.h b/lib/hkStubs/Havok/Common/Base/Math/Quaternion/hkQuaternionf.h index d1375727..c88e06c4 100644 --- a/lib/hkStubs/Havok/Common/Base/Math/Quaternion/hkQuaternionf.h +++ b/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); +} diff --git a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h b/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h index ded5078f..fbc674ba 100644 --- a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h +++ b/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(&v)[i]; } + hkFloat32& operator[](int i) { return reinterpret_cast(&v)[i]; } + const hkFloat32& operator()(int i) const { return reinterpret_cast(&v)[i]; } + const hkFloat32& operator[](int i) const { return reinterpret_cast(&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 diff --git a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.inl b/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.inl index 428314e3..e4544779 100644 --- a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.inl +++ b/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); diff --git a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4fComparison.h b/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4fComparison.h index 1d824c6a..c98fad4b 100644 --- a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4fComparison.h +++ b/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 + HK_FORCE_INLINE hkBool32 allAreSet() const; + HK_FORCE_INLINE hkBool32 allAreSet(Mask m) const; + + template + 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 +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 +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); +} diff --git a/lib/hkStubs/Havok/Physics2012/Dynamics/Motion/hkpMotion.h b/lib/hkStubs/Havok/Physics2012/Dynamics/Motion/hkpMotion.h index 830031aa..78446db9 100644 --- a/lib/hkStubs/Havok/Physics2012/Dynamics/Motion/hkpMotion.h +++ b/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 { diff --git a/lib/sead b/lib/sead index ec7fdc84..09be2c33 160000 --- a/lib/sead +++ b/lib/sead @@ -1 +1 @@ -Subproject commit ec7fdc84424b4dd54fe29642650eba0068f637b7 +Subproject commit 09be2c3398c6fb732505f20a55c7d4c22f6a26c1 diff --git a/src/KingSystem/Physics/CMakeLists.txt b/src/KingSystem/Physics/CMakeLists.txt index 194fb98b..09d0f8db 100644 --- a/src/KingSystem/Physics/CMakeLists.txt +++ b/src/KingSystem/Physics/CMakeLists.txt @@ -19,6 +19,8 @@ target_sources(uking PRIVATE RigidBody/physMotionAccessor.h RigidBody/physRigidBody.cpp RigidBody/physRigidBody.h + RigidBody/physRigidBodyAccessor.cpp + RigidBody/physRigidBodyAccessor.h RigidBody/physRigidBodyFactory.cpp RigidBody/physRigidBodyFactory.h RigidBody/physRigidBodyParam.cpp @@ -92,4 +94,6 @@ target_sources(uking PRIVATE System/physSystemData.h System/physUserTag.cpp System/physUserTag.h + + physConversions.h ) diff --git a/src/KingSystem/Physics/RigidBody/Shape/physCapsuleShape.cpp b/src/KingSystem/Physics/RigidBody/Shape/physCapsuleShape.cpp index 4eba6781..8db65e3a 100644 --- a/src/KingSystem/Physics/RigidBody/Shape/physCapsuleShape.cpp +++ b/src/KingSystem/Physics/RigidBody/Shape/physCapsuleShape.cpp @@ -2,6 +2,7 @@ #include #include #include +#include "KingSystem/Physics/physConversions.h" namespace ksys::phys { @@ -98,12 +99,12 @@ void CapsuleBody::sub_7100FABE80(sead::Vector3f* veca, sead::Vector3f* vecb, const hkTransformf& rb_vec) { if (veca != nullptr) { hkVector4 tmp; - tmp.setTransformedPos(rb_vec, hkVector4(vertex_a.x, vertex_a.y, vertex_a.z)); + tmp.setTransformedPos(rb_vec, toHkVec4(vertex_a)); tmp.store<3>(veca->e.data()); } if (vecb != nullptr) { hkVector4 tmp; - tmp.setTransformedPos(rb_vec, hkVector4(vertex_b.x, vertex_b.y, vertex_b.z)); + tmp.setTransformedPos(rb_vec, toHkVec4(vertex_b)); tmp.store<3>(vecb->e.data()); } } diff --git a/src/KingSystem/Physics/RigidBody/physRigidBody.cpp b/src/KingSystem/Physics/RigidBody/physRigidBody.cpp index 1e42fb83..04fb2e3c 100644 --- a/src/KingSystem/Physics/RigidBody/physRigidBody.cpp +++ b/src/KingSystem/Physics/RigidBody/physRigidBody.cpp @@ -7,7 +7,7 @@ namespace ksys::phys { RigidBody::RigidBody(u32 a, u32 mass_scaling, hkpRigidBody* hk_body, const sead::SafeString& name, sead::Heap* heap, bool a7) - : mCS(heap), mHkBody(hk_body), mHkBodyMgr(hk_body), _b4(a) { + : mCS(heap), mHkBody(hk_body), mRigidBodyAccessor(hk_body), _b4(a) { if (!name.isEmpty()) { mHkBody->setName(name.cstr()); } @@ -69,7 +69,7 @@ MotionType RigidBody::getMotionInfo() const { return MotionType::Keyframed; if (mMotionFlags.isOn(MotionFlag::Fixed)) return MotionType::Fixed; - return mHkBodyMgr.getMotionInfo(); + return mRigidBodyAccessor.getMotionType(); } void RigidBody::setContactMask(u32 value) { diff --git a/src/KingSystem/Physics/RigidBody/physRigidBody.h b/src/KingSystem/Physics/RigidBody/physRigidBody.h index 48e3736d..8c2de80d 100644 --- a/src/KingSystem/Physics/RigidBody/physRigidBody.h +++ b/src/KingSystem/Physics/RigidBody/physRigidBody.h @@ -6,6 +6,7 @@ #include #include #include +#include "KingSystem/Physics/RigidBody/physRigidBodyAccessor.h" #include "KingSystem/Physics/RigidBody/physRigidBodyParam.h" #include "KingSystem/Physics/System/physDefines.h" #include "KingSystem/Utils/Types.h" @@ -25,14 +26,6 @@ public: class RigidBody : public sead::IDisposer, public RigidBase { SEAD_RTTI_BASE(RigidBody) public: - struct HkBodyMgr { - explicit HkBodyMgr(hkpRigidBody* body); - virtual ~HkBodyMgr(); - MotionType getMotionInfo() const; - - void* p; - }; - enum class Flag1 { MassScaling = 1 << 0, _2 = 1 << 1, @@ -107,7 +100,7 @@ private: void* _88 = nullptr; void* _90 = nullptr; u16 _98 = 0; - HkBodyMgr mHkBodyMgr; + RigidBodyAccessor mRigidBodyAccessor; f32 _b0 = 1.0f; u32 _b4 = 0; MotionAccessor* mMotionAccessor = nullptr; diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodyAccessor.cpp b/src/KingSystem/Physics/RigidBody/physRigidBodyAccessor.cpp new file mode 100644 index 00000000..a68567b5 --- /dev/null +++ b/src/KingSystem/Physics/RigidBody/physRigidBodyAccessor.cpp @@ -0,0 +1,99 @@ +#include "KingSystem/Physics/RigidBody/physRigidBodyAccessor.h" +#include +#include "KingSystem/Physics/physConversions.h" + +namespace ksys::phys { + +RigidBodyAccessor::RigidBodyAccessor(hkpRigidBody* body) : mBody(body) {} + +RigidBodyAccessor::~RigidBodyAccessor() = default; + +MotionType RigidBodyAccessor::getMotionType() const { + switch (getBody()->getMotionType()) { + case hkpMotion::MOTION_INVALID: + break; + case hkpMotion::MOTION_DYNAMIC: + case hkpMotion::MOTION_SPHERE_INERTIA: + case hkpMotion::MOTION_BOX_INERTIA: + return MotionType::Dynamic; + case hkpMotion::MOTION_KEYFRAMED: + return MotionType::Keyframed; + case hkpMotion::MOTION_FIXED: + return MotionType::Fixed; + case hkpMotion::MOTION_THIN_BOX_INERTIA: + break; + case hkpMotion::MOTION_CHARACTER: + return MotionType::Dynamic; + case hkpMotion::MOTION_MAX_ID: + break; + } + return MotionType::Invalid; +} + +void RigidBodyAccessor::getPosition(sead::Vector3f* pos) const { + toVec3(pos, getBody()->getPosition()); +} + +void RigidBodyAccessor::getRotation(sead::Quatf* rot) const { + toQuat(rot, getBody()->getRotation()); +} + +void RigidBodyAccessor::getTransform(sead::Matrix34f* mtx) const { + toMtx34(mtx, getBody()->getTransform()); +} + +void RigidBodyAccessor::getLinearVelocity(sead::Vector3f* vel) const { + toVec3(vel, getBody()->getLinearVelocity()); +} + +void RigidBodyAccessor::getAngularVelocity(sead::Vector3f* vel) const { + toVec3(vel, getBody()->getAngularVelocity()); +} + +bool RigidBodyAccessor::isVelocityGreaterEqual(float vel) const { + hkVector4f v; + v.setAll(vel); + + hkVector4f linvel; + linvel.setAbs(getBody()->getLinearVelocity()); + + hkVector4f angvel; + angvel.setAbs(getBody()->getAngularVelocity()); + + return v.greaterEqual(linvel).allAreSet() && + v.greaterEqual(angvel).allAreSet(); +} + +void RigidBodyAccessor::getPointVelocity(sead::Vector3f* vel, const sead::Vector3f& point) const { + const auto p = toHkVec4(point); + hkVector4f out; + getBody()->getPointVelocity(p, out); + toVec3(vel, out); +} + +float RigidBodyAccessor::getTimeFactor() const { + return getBody()->getTimeFactor(); +} + +void RigidBodyAccessor::getDeltaCenterOfMass(sead::Vector3f* out_delta_pos, + sead::Vector3f* out_delta_angle) const { + const hkMotionState* state = getBody()->getMotion()->getMotionState(); + const auto center1 = state->getSweptTransform().m_centerOfMass1; + const auto center0 = state->getSweptTransform().m_centerOfMass0; + const auto delta_angle = state->m_deltaAngle; + + if (out_delta_pos != nullptr) { + hkVector4f value; + value.setSub(center1, center0); + value.mul(center1.getW()); // W is the time step + value.store<3>(out_delta_pos->e.data()); + } + + if (out_delta_angle != nullptr) { + hkVector4f value; + value.setMul(delta_angle, center1.getW()); + value.store<3>(out_delta_angle->e.data()); + } +} + +} // namespace ksys::phys diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodyAccessor.h b/src/KingSystem/Physics/RigidBody/physRigidBodyAccessor.h new file mode 100644 index 00000000..8e32ed40 --- /dev/null +++ b/src/KingSystem/Physics/RigidBody/physRigidBodyAccessor.h @@ -0,0 +1,44 @@ +#pragma once + +#include +#include +#include +#include "KingSystem/Physics/System/physDefines.h" + +class hkpRigidBody; + +namespace ksys::phys { + +class RigidBodyAccessor { +public: + explicit RigidBodyAccessor(hkpRigidBody* body); + virtual ~RigidBodyAccessor(); + + hkpRigidBody* getBody() const { return mBody; } + + MotionType getMotionType() const; + + void getPosition(sead::Vector3f* pos) const; + void getRotation(sead::Quatf* rot) const; + void getTransform(sead::Matrix34f* mtx) const; + + void getLinearVelocity(sead::Vector3f* vel) const; + void getAngularVelocity(sead::Vector3f* vel) const; + + /// Whether `vel` >= abs(c) for each component c of the linear velocity and angular velocity. + // XXX: does this function even make sense? We're comparing `vel` against two quantities + // that have different units! + bool isVelocityGreaterEqual(float vel) const; + + void getPointVelocity(sead::Vector3f* vel, const sead::Vector3f& point) const; + + float getTimeFactor() const; + + /// Both parameters are allowed to be null. + void getDeltaCenterOfMass(sead::Vector3f* out_delta_pos, sead::Vector3f* out_delta_angle) const; + +private: + hkpRigidBody* mBody; +}; + +} // namespace ksys::phys diff --git a/src/KingSystem/Physics/System/physDefines.h b/src/KingSystem/Physics/System/physDefines.h index 4c8d045b..96058c7c 100644 --- a/src/KingSystem/Physics/System/physDefines.h +++ b/src/KingSystem/Physics/System/physDefines.h @@ -171,6 +171,7 @@ enum class MotionType { Fixed = 1, Keyframed = 2, Unknown = 3, + Invalid = -1, }; union ReceiverMask { diff --git a/src/KingSystem/Physics/physConversions.h b/src/KingSystem/Physics/physConversions.h new file mode 100644 index 00000000..48db1dd9 --- /dev/null +++ b/src/KingSystem/Physics/physConversions.h @@ -0,0 +1,86 @@ +#pragma once + +#include +#include +#include +#include + +#ifdef __aarch64__ +#include +#endif + +namespace ksys::phys { + +inline void toVec3(sead::Vector3f* out, const hkVector4f& vec) { + out->x = vec.getX(); + out->y = vec.getY(); + out->z = vec.getZ(); +} + +[[nodiscard]] inline sead::Vector3f toVec3(const hkVector4f& vec) { + return {vec.getX(), vec.getY(), vec.getZ()}; +} + +inline void toHkVec4(hkVector4f* out, const sead::Vector3f& vec) { + out->set(vec.x, vec.y, vec.z); +} + +[[nodiscard]] inline hkVector4f toHkVec4(const sead::Vector3f& vec) { + return {vec.x, vec.y, vec.z}; +} + +inline void toQuat(sead::Quatf* out, const hkQuaternionf& quat) { + out->set(quat.m_vec.getW(), quat.m_vec.getX(), quat.m_vec.getY(), quat.m_vec.getZ()); +} + +[[nodiscard]] inline sead::Quatf toQuat(const hkQuaternionf& quat) { + return {quat.m_vec.getW(), quat.m_vec.getX(), quat.m_vec.getY(), quat.m_vec.getZ()}; +} + +inline void toHkQuat(hkQuaternionf* out, const sead::Quatf& quat) { + out->set(quat.x, quat.y, quat.x, quat.w); +} + +[[nodiscard]] inline hkQuaternionf toHkQuat(const sead::Quatf& quat) { + return {quat.x, quat.y, quat.x, quat.w}; +} + +inline void toMtx34(sead::Matrix34f* out, const hkTransformf& transform) { + const hkRotationf& rotate = transform.getRotation(); + const hkVector4f& translate = transform.getTranslation(); + + hkVector4f row0, row1, row2; + +#ifdef __aarch64__ + // XXX: this leads to really poor codegen (compared to using getRows, which + // is optimised into Neon zip/transpose instructions). Is Nintendo to blame + // for this bad usage of Neon intrinsics, or did Havok mess up their Neon getRows? + + row0.v = vld1q_lane_f32(&rotate(0, 0), row0.v, 0); + row1.v = vld1q_lane_f32(&rotate(1, 0), row1.v, 0); + row2.v = vld1q_lane_f32(&rotate(2, 0), row2.v, 0); + + row0.v = vld1q_lane_f32(&rotate(0, 1), row0.v, 1); + row1.v = vld1q_lane_f32(&rotate(1, 1), row1.v, 1); + row2.v = vld1q_lane_f32(&rotate(2, 1), row2.v, 1); + + row0.v = vld1q_lane_f32(&rotate(0, 2), row0.v, 2); + row1.v = vld1q_lane_f32(&rotate(1, 2), row1.v, 2); + row2.v = vld1q_lane_f32(&rotate(2, 2), row2.v, 2); + + row0.v = vld1q_lane_f32(&translate(0), row0.v, 3); + row1.v = vld1q_lane_f32(&translate(1), row1.v, 3); + row2.v = vld1q_lane_f32(&translate(2), row2.v, 3); +#else + rotate.getRows(row0, row1, row2); + row0[3] = translate[0]; + row1[3] = translate[1]; + row2[3] = translate[2]; +#endif + + row0.store<4>(out->m[0]); + row1.store<4>(out->m[1]); + row2.store<4>(out->m[2]); +} + +} // namespace ksys::phys