diff --git a/data/uking_functions.csv b/data/uking_functions.csv index dc470f95..55c18843 100644 --- a/data/uking_functions.csv +++ b/data/uking_functions.csv @@ -83118,29 +83118,29 @@ Address,Quality,Size,Name 0x0000007100f955c0,U,000768,phys::RigidBody::x_108 0x0000007100f958c0,U,000416,phys::RigidBody::x_109 0x0000007100f95a60,U,000376,phys::RigidBody::x_110 -0x0000007100f95bd8,U,000316,phys::RigidBody::x_111 -0x0000007100f95d14,U,000316,phys::RigidBody::x_112 -0x0000007100f95e50,U,000316,phys::RigidBody::x_113 +0x0000007100f95bd8,O,000316,_ZN4ksys4phys9RigidBody16clearFlag2000000Eb +0x0000007100f95d14,O,000316,_ZN4ksys4phys9RigidBody16clearFlag4000000Eb +0x0000007100f95e50,O,000316,_ZN4ksys4phys9RigidBody16clearFlag8000000Eb 0x0000007100f95f8c,U,001048,phys::RigidBody::x_114 -0x0000007100f963a4,U,000156,phys::RigidBody::x_115 -0x0000007100f96440,U,000156,phys::RigidBody::x_116 -0x0000007100f964dc,U,000148,phys::RigidBody::x_117 -0x0000007100f96570,U,000008,phys::RigidBody::m10 -0x0000007100f96578,U,000008,phys::RigidBody::m11 +0x0000007100f963a4,O,000156,_ZNK4ksys4phys9RigidBody22isEntityMotionFlag80OnEv +0x0000007100f96440,O,000156,_ZN4ksys4phys9RigidBody25setMagneMassScalingFactorEf +0x0000007100f964dc,O,000148,_ZNK4ksys4phys9RigidBody25getMagneMassScalingFactorEv +0x0000007100f96570,O,000008,_ZN4ksys4phys9RigidBody3m10Ev +0x0000007100f96578,O,000008,_ZN4ksys4phys9RigidBody3m11Ev 0x0000007100f96580,O,000240,_ZN4ksys4phys9RigidBody13resetPositionEv 0x0000007100f96670,O,000144,_ZN4ksys4phys9RigidBody7getNameEv -0x0000007100f96700,U,000068,phys::RigidBody::m5 -0x0000007100f96744,U,000176,phys::RigidBody::x_118 -0x0000007100f967f4,U,000428,phys::RigidBody::x_119 +0x0000007100f96700,O,000068,_ZNK4ksys4phys9RigidBody11logPositionEv +0x0000007100f96744,O,000176,_ZNK4ksys4phys9RigidBody14getAabbInLocalEPN4sead9BoundBox3IfEE +0x0000007100f967f4,m,000428,_ZNK4ksys4phys9RigidBody14getAabbInWorldEPN4sead9BoundBox3IfEE 0x0000007100f969a0,O,000072,_ZN4ksys4phys9RigidBody4lockEb 0x0000007100f969e8,O,000088,_ZN4ksys4phys9RigidBody6unlockEb 0x0000007100f96a40,O,000012,_ZNK4ksys4phys9RigidBody9getMotionEv 0x0000007100f96a4c,U,000076,phys::RigidBody::x_123 -0x0000007100f96a98,U,000188,phys::RigidBody::x_124 -0x0000007100f96b54,U,000156,phys::RigidBody::x_125 -0x0000007100f96bf0,U,000204,phys::RigidBody::x_126 -0x0000007100f96cbc,U,000156,phys::RigidBody::x_127 -0x0000007100f96d58,U,000204,phys::RigidBody::x_128 +0x0000007100f96a98,O,000188,_ZN4ksys4phys9RigidBody20setEntityMotionFlag1Eb +0x0000007100f96b54,O,000156,_ZNK4ksys4phys9RigidBody21isEntityMotionFlag1OnEv +0x0000007100f96bf0,O,000204,_ZN4ksys4phys9RigidBody22setEntityMotionFlag100Eb +0x0000007100f96cbc,O,000156,_ZNK4ksys4phys9RigidBody23isEntityMotionFlag100OnEv +0x0000007100f96d58,O,000204,_ZN4ksys4phys9RigidBody22setEntityMotionFlag200Eb 0x0000007100f96e24,U,000096, 0x0000007100f96e84,U,000384, 0x0000007100f97004,O,000112,_ZNK4ksys4phys9RigidBody27checkDerivedRuntimeTypeInfoEPKN4sead15RuntimeTypeInfo9InterfaceE 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(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()); +} diff --git a/src/KingSystem/Physics/RigidBody/physRigidBody.cpp b/src/KingSystem/Physics/RigidBody/physRigidBody.cpp index 4aa618ee..bb2b8c8c 100644 --- a/src/KingSystem/Physics/RigidBody/physRigidBody.cpp +++ b/src/KingSystem/Physics/RigidBody/physRigidBody.cpp @@ -1,4 +1,5 @@ #include "KingSystem/Physics/RigidBody/physRigidBody.h" +#include #include #include #include @@ -653,14 +654,14 @@ float RigidBody::getWaterFlowEffectiveRate() const { } void RigidBody::setMagneMassScalingFactor(float factor) { - if (!isEntity()) + if (!isEntity() || !mMotionAccessor) return; getEntityMotionAccessor()->setMagneMassScalingFactor(factor); } float RigidBody::getMagneMassScalingFactor() const { - if (!isEntity()) - return 0.0; + if (!isEntity() || !mMotionAccessor) + return -1.0; return getEntityMotionAccessor()->getMagneMassScalingFactor(); } @@ -758,6 +759,18 @@ bool RigidBody::isEntityMotionFlag20Off() const { return !getEntityMotionAccessor()->hasFlag(RigidBodyMotionEntity::Flag::_20); } +void RigidBody::setEntityMotionFlag80(bool set) { + if (!isEntity() || !mMotionAccessor) + return; + getEntityMotionAccessor()->changeFlag(RigidBodyMotionEntity::Flag::_80, set); +} + +bool RigidBody::isEntityMotionFlag80On() const { + if (!isEntity() || !mMotionAccessor) + return false; + return getEntityMotionAccessor()->hasFlag(RigidBodyMotionEntity::Flag::_80); +} + void RigidBody::setColImpulseScale(float scale) { if (!isEntity()) return; @@ -828,6 +841,50 @@ void RigidBody::computeShapeVolumeMassProperties(float* volume, sead::Vector3f* } } +void RigidBody::clearFlag2000000(bool clear) { + if (mFlags.isOff(Flag::_2000000) == clear) + return; + + mFlags.change(Flag::_2000000, !clear); + + if (isFlag8Set()) + setMotionFlag(MotionFlag::_10000); + else + updateDeactivation(); +} + +void RigidBody::clearFlag4000000(bool clear) { + if (mFlags.isOff(Flag::_4000000) == clear) + return; + + mFlags.change(Flag::_4000000, !clear); + + if (isFlag8Set()) + setMotionFlag(MotionFlag::_10000); + else + updateDeactivation(); +} + +void RigidBody::clearFlag8000000(bool clear) { + if (mFlags.isOff(Flag::_8000000) == clear) + return; + + mFlags.change(Flag::_8000000, !clear); + + if (isFlag8Set()) + setMotionFlag(MotionFlag::_10000); + else + updateDeactivation(); +} + +void* RigidBody::m10() { + return nullptr; +} + +void* RigidBody::m11() { + return nullptr; +} + void RigidBody::resetPosition() { // debug logging? [[maybe_unused]] sead::Vector3f position = getPosition(); @@ -838,6 +895,39 @@ const char* RigidBody::getName() { return mUserTag ? mUserTag->getName().cstr() : getHkBodyName().cstr(); } +void RigidBody::logPosition() const { + sead::Vector3f position; + getPosition(&position); + // debug logging? +} + +static void convertHkAabb(const hkAabb& hk_aabb, sead::BoundBox3f* aabb) { + hkVector4f center; + hk_aabb.getCenter(center); + + hkVector4f extents; + hk_aabb.getExtents(extents); + auto half_extents = 0.5f * toVec3(extents); + + aabb->setMin(toVec3(center) - half_extents); + aabb->setMax(toVec3(center) + half_extents); +} + +void RigidBody::getAabbInLocal(sead::BoundBox3f* aabb) const { + hkAabb hk_aabb; + getHkBody()->getCollidable()->getShape()->getAabb(hkTransformf::getIdentity(), 0.0, hk_aabb); + convertHkAabb(hk_aabb, aabb); +} + +// NON_MATCHING: paired stores in convertHkAabb that shouldn't be paired +void RigidBody::getAabbInWorld(sead::BoundBox3f* aabb) const { + hkTransformf hk_transform; + toHkTransform(&hk_transform, getTransform()); + hkAabb hk_aabb; + getHkBody()->getCollidable()->getShape()->getAabb(hk_transform, 0.0, hk_aabb); + convertHkAabb(hk_aabb, aabb); +} + void RigidBody::lock() { mCS.lock(); } @@ -862,6 +952,42 @@ hkpMotion* RigidBody::getMotion() const { return getHkBody()->getMotion(); } +void RigidBody::setEntityMotionFlag1(bool set) { + if (!isEntity() || !mMotionAccessor) + return; + getEntityMotionAccessor()->changeFlag(RigidBodyMotionEntity::Flag::_1, set); +} + +bool RigidBody::isEntityMotionFlag1On() const { + if (!isEntity() || !mMotionAccessor) + return false; + return getEntityMotionAccessor()->hasFlag(RigidBodyMotionEntity::Flag::_1); +} + +void RigidBody::setEntityMotionFlag100(bool set) { + if (!isEntity() || !mMotionAccessor) + return; + getEntityMotionAccessor()->changeFlag(RigidBodyMotionEntity::Flag::_100, set); +} + +bool RigidBody::isEntityMotionFlag100On() const { + if (!isEntity() || !mMotionAccessor) + return false; + return getEntityMotionAccessor()->hasFlag(RigidBodyMotionEntity::Flag::_100); +} + +void RigidBody::setEntityMotionFlag200(bool set) { + if (!isEntity() || !mMotionAccessor) + return; + getEntityMotionAccessor()->changeFlag(RigidBodyMotionEntity::Flag::_200, set); +} + +bool RigidBody::isEntityMotionFlag200On() const { + if (!isEntity() || !mMotionAccessor) + return false; + return getEntityMotionAccessor()->hasFlag(RigidBodyMotionEntity::Flag::_200); +} + void RigidBody::onInvalidParameter(int code) { sead::Vector3f pos, lin_vel, ang_vel; mRigidBodyAccessor.getPosition(&pos); @@ -876,4 +1002,13 @@ void RigidBody::notifyUserTag(int code) { mUserTag->m7(this, code); } +void RigidBody::updateDeactivation() { + if (mFlags.isOn(Flag::_2000000) || mFlags.isOn(Flag::_4000000) || mFlags.isOn(Flag::_8000000)) { + if (getHkBody()->isDeactivationEnabled()) + mHkBody->enableDeactivation(false); + } else if (!getHkBody()->isDeactivationEnabled()) { + mHkBody->enableDeactivation(true); + } +} + } // namespace ksys::phys diff --git a/src/KingSystem/Physics/RigidBody/physRigidBody.h b/src/KingSystem/Physics/RigidBody/physRigidBody.h index a308227d..2a0d07a9 100644 --- a/src/KingSystem/Physics/RigidBody/physRigidBody.h +++ b/src/KingSystem/Physics/RigidBody/physRigidBody.h @@ -2,6 +2,7 @@ #include #include +#include #include #include #include @@ -98,21 +99,27 @@ public: _80000 = 1 << 19, }; + class ScopedLock { + public: + explicit ScopedLock(RigidBody* body, bool also_lock_world) + : mBody(body), mAlsoLockWorld(also_lock_world) { + mBody->lock(also_lock_world); + } + ~ScopedLock() { mBody->unlock(mAlsoLockWorld); } + ScopedLock(const ScopedLock&) = delete; + auto operator=(const ScopedLock&) = delete; + + private: + RigidBody* mBody; + bool mAlsoLockWorld; + }; + RigidBody(Type type, ContactLayerType layer_type, hkpRigidBody* hk_body, const sead::SafeString& name, sead::Heap* heap, bool a7); ~RigidBody() override; // FIXME: types and names virtual float m4(); - virtual void m5(); - - /// Recalculate inertia, volume and center of mass based on the shape and mass of the rigid body - /// and update this rigid body to match the computed values. - virtual void resetInertiaAndCenterOfMass(); - - /// All three parameters may be null. - virtual void computeShapeVolumeMassProperties(float* volume, sead::Vector3f* center_of_mass, - sead::Vector3f* inertia_tensor); bool initMotionAccessorForDynamicMotion(sead::Heap* heap); bool initMotionAccessor(const RigidBodyInstanceParam& param, sead::Heap* heap, @@ -193,6 +200,9 @@ public: void setPosition(const sead::Vector3f& position, bool propagate_to_linked_motions); void getPosition(sead::Vector3f* position) const; sead::Vector3f getPosition() const; + virtual void logPosition() const; + void getAabbInLocal(sead::BoundBox3f* aabb) const; + void getAabbInWorld(sead::BoundBox3f* aabb) const; void getRotation(sead::Quatf* rotation) const; sead::Quatf getRotation() const; @@ -243,6 +253,14 @@ public: void getInertiaLocal(sead::Vector3f* inertia) const; sead::Vector3f getInertiaLocal() const; + /// Recalculate inertia, volume and center of mass based on the shape and mass of the rigid body + /// and update this rigid body to match the computed values. + virtual void resetInertiaAndCenterOfMass(); + + /// All three parameters may be null. + virtual void computeShapeVolumeMassProperties(float* volume, sead::Vector3f* center_of_mass, + sead::Vector3f* inertia_tensor); + void setLinearDamping(float value); float getLinearDamping() const; @@ -289,6 +307,9 @@ public: void clearEntityMotionFlag20(bool clear); bool isEntityMotionFlag20Off() const; + void setEntityMotionFlag80(bool set); + bool isEntityMotionFlag80On() const; + bool isSensor() const { return mFlags.isOn(Flag::IsSensor); } bool isEntity() const { return !mFlags.isOn(Flag::IsSensor); } ContactLayerType getLayerType() const { @@ -316,36 +337,33 @@ public: void setEntityMotionFlag40(bool set); bool isEntityMotionFlag40On() const; + void clearFlag2000000(bool clear); + void clearFlag4000000(bool clear); + void clearFlag8000000(bool clear); + void lock(); void lock(bool also_lock_world); void unlock(); void unlock(bool also_unlock_world); - - hkpMotion* getMotion() const; - - class ScopedLock { - public: - explicit ScopedLock(RigidBody* body, bool also_lock_world) - : mBody(body), mAlsoLockWorld(also_lock_world) { - mBody->lock(also_lock_world); - } - ~ScopedLock() { mBody->unlock(mAlsoLockWorld); } - ScopedLock(const ScopedLock&) = delete; - auto operator=(const ScopedLock&) = delete; - - private: - RigidBody* mBody; - bool mAlsoLockWorld; - }; - [[nodiscard]] auto makeScopedLock(bool also_lock_world) { return ScopedLock(this, also_lock_world); } + hkpMotion* getMotion() const; + + void setEntityMotionFlag1(bool set); + bool isEntityMotionFlag1On() const; + + void setEntityMotionFlag100(bool set); + bool isEntityMotionFlag100On() const; + + void setEntityMotionFlag200(bool set); + bool isEntityMotionFlag200On() const; + // FIXME: should be pure virtual void m9(); - virtual void m10(); - virtual void m11(); + virtual void* m10(); + virtual void* m11(); virtual float m12(float x, float y); virtual void resetPosition(); virtual const char* getName(); @@ -354,6 +372,7 @@ private: void createMotionAccessor(sead::Heap* heap); void onInvalidParameter(int code = 0); void notifyUserTag(int code); + void updateDeactivation(); sead::CriticalSection mCS; sead::TypedBitFlag> mFlags{};