diff --git a/data/uking_functions.csv b/data/uking_functions.csv index 23f69abd..0ae9903b 100644 --- a/data/uking_functions.csv +++ b/data/uking_functions.csv @@ -83385,7 +83385,7 @@ Address,Quality,Size,Name 0x0000007100fa2a7c,O,000168,_ZN4ksys4phys15RigidBodyMotion7setMassEf 0x0000007100fa2b24,O,000028,_ZNK4ksys4phys15RigidBodyMotion7getMassEv 0x0000007100fa2b40,O,000040,_ZNK4ksys4phys15RigidBodyMotion10getMassInvEv -0x0000007100fa2b68,U,000776,phys::RigidBodyMotion::setInertiaLocal +0x0000007100fa2b68,O,000776,_ZN4ksys4phys15RigidBodyMotion15setInertiaLocalERKN4sead7Vector3IfEE 0x0000007100fa2e70,O,000040,_ZNK4ksys4phys15RigidBodyMotion16getGravityFactorEv 0x0000007100fa2e98,O,000420,_ZN4ksys4phys15RigidBodyMotion32updateRigidBodyMotionExceptStateEv 0x0000007100fa303c,O,000112,_ZNK4ksys4phys15RigidBodyMotion15getInertiaLocalEPN4sead7Vector3IfEE diff --git a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h b/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h index 78df61b0..15cde5af 100644 --- a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h +++ b/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h @@ -29,6 +29,7 @@ public: HK_FORCE_INLINE void set(hkFloat32 x, hkFloat32 y, hkFloat32 z, hkFloat32 w = 0); HK_FORCE_INLINE void setAll(hkFloat32 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 e9e04d32..6ed28c86 100644 --- a/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.inl +++ b/lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.inl @@ -32,6 +32,10 @@ inline void hkVector4f::setAll(hkReal x) { v = {x, x, x, x}; } +inline void hkVector4f::setZero() { + setAll(0); +} + inline void hkVector4f::add(hkVector4fParameter a) { setAdd(*this, a); } diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodyMotion.cpp b/src/KingSystem/Physics/RigidBody/physRigidBodyMotion.cpp index dc0a7318..9f5b3e19 100644 --- a/src/KingSystem/Physics/RigidBody/physRigidBodyMotion.cpp +++ b/src/KingSystem/Physics/RigidBody/physRigidBodyMotion.cpp @@ -1,11 +1,13 @@ #include "KingSystem/Physics/RigidBody/physRigidBodyMotion.h" #include +#include #include #include #include #include #include #include +#include "Havok/Physics2012/Dynamics/Motion/Rigid/hkpSphereMotion.h" #include "KingSystem/Physics/RigidBody/physRigidBodyMotionProxy.h" #include "KingSystem/Physics/physConversions.h" #include "KingSystem/Utils/Debug.h" @@ -82,14 +84,21 @@ void RigidBodyMotion::setPosition(const sead::Vector3f& position, } void RigidBodyMotion::getPosition(sead::Vector3f* position) { + storeToVec3(position, getPosition()); +} + +hkVector4f RigidBodyMotion::getPosition() const { auto* motion = getHkBodyMotionOrLocalMotionIf(RigidBody::MotionFlag::DirtyTransform); - const auto hk_position = motion->getPosition(); - storeToVec3(position, hk_position); + return motion->getPosition(); } void RigidBodyMotion::getRotation(sead::Quatf* rotation) { + toQuat(rotation, getRotation()); +} + +hkQuaternionf RigidBodyMotion::getRotation() const { auto* motion = getHkBodyMotionOrLocalMotionIf(RigidBody::MotionFlag::DirtyTransform); - toQuat(rotation, motion->getRotation()); + return motion->getRotation(); } void RigidBodyMotion::getTransform(sead::Matrix34f* mtx) { @@ -292,6 +301,87 @@ float RigidBodyMotion::getMassInv() const { return mMotion->getMassInv(); } +static inline float max3(float a, float b, float c) { + return sead::Mathf::max(c, a > b ? a : b); +} + +static inline float min3(float a, float b, float c) { + return sead::Mathf::min(a < b ? a : b, c); +} + +void RigidBodyMotion::setInertiaLocal(const sead::Vector3f& inertia) { + if (mBody->isCharacterControllerType()) + return; + + if (arePropertyChangesBlocked()) { + mInertiaLocal = inertia; + return; + } + + const float max = max3(inertia.x, inertia.y, inertia.z); + const float min = min3(inertia.x, inertia.y, inertia.z); + const float threshold = max * 0.8f; + + bool need_to_recreate_motion = false; + switch (mMotion->getType()) { + case hkpMotion::MOTION_BOX_INERTIA: + need_to_recreate_motion = min > threshold; + break; + case hkpMotion::MOTION_SPHERE_INERTIA: + need_to_recreate_motion = min > threshold; + // The condition is inverted for spheres. + need_to_recreate_motion ^= true; + break; + default: + break; + } + + if (need_to_recreate_motion) { + const float mass = getMass(); + const auto position = getPosition(); + const auto rotation = getRotation(); + const auto gravity_factor = getGravityFactor(); + + // Recreate the Havok motion. + if (min > threshold) { + hkpSphereMotion tmp_motion(position, rotation); + mMotion->getMotionStateAndVelocitiesAndDeactivationType(&tmp_motion); + new (mMotion) hkpSphereMotion(position, rotation); + tmp_motion.getMotionStateAndVelocitiesAndDeactivationType(mMotion); + } else { + // This little trick lets us copy the motion state and various other state + // out of the existing Havok motion so we can recreate it safely. + hkpBoxMotion tmp_motion(position, rotation); + mMotion->getMotionStateAndVelocitiesAndDeactivationType(&tmp_motion); + new (mMotion) hkpBoxMotion(position, rotation); + tmp_motion.getMotionStateAndVelocitiesAndDeactivationType(mMotion); + } + + // Some properties are not automatically transferred over. Copy them manually. + mMotion->setGravityFactor(gravity_factor); + mMotion->setMass(mass); + if (mBody->isFlag8Set()) + setMotionFlag(RigidBody::MotionFlag::DirtyMiscState); + else if (mBody->getMotionType() == MotionType::Dynamic) + updateRigidBodyMotionExceptState(); + } + + hkMatrix3f hk_inertia; + hk_inertia.m_col0.set(inertia.x, 0, 0); + hk_inertia.m_col1.set(0, inertia.y, 0); + hk_inertia.m_col2.set(0, 0, inertia.z); + mMotion->setInertiaLocal(hk_inertia); + + if (mBody->isFlag8Set()) { + setMotionFlag(RigidBody::MotionFlag::DirtyInertiaLocal); + } else if (mBody->getMotionType() == MotionType::Dynamic && + !mBody->isCharacterControllerType()) { + hkMatrix3f inertia_inv; + mMotion->getInertiaInvLocal(inertia_inv); + getHkBody()->getMotion()->setInertiaInvLocal(inertia_inv); + } +} + void RigidBodyMotion::getInertiaLocal(sead::Vector3f* inertia) const { if (arePropertyChangesBlocked()) { inertia->e = mInertiaLocal.e; diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodyMotion.h b/src/KingSystem/Physics/RigidBody/physRigidBodyMotion.h index 01db7202..c029fe7d 100644 --- a/src/KingSystem/Physics/RigidBody/physRigidBodyMotion.h +++ b/src/KingSystem/Physics/RigidBody/physRigidBodyMotion.h @@ -27,7 +27,9 @@ public: void setTransform(const sead::Matrix34f& mtx, bool propagate_to_linked_motions) override; void setPosition(const sead::Vector3f& position, bool propagate_to_linked_motions) override; void getPosition(sead::Vector3f* position) override; + hkVector4f getPosition() const; void getRotation(sead::Quatf* rotation) override; + hkQuaternionf getRotation() const; void getTransform(sead::Matrix34f* mtx) override; void setCenterOfMassInLocal(const sead::Vector3f& center) override; @@ -66,8 +68,7 @@ public: float getMass() const; float getMassInv() const; - // 0x0000007100fa2b68 - void setInertiaLocal(const sead::Vector3f&); + void setInertiaLocal(const sead::Vector3f& inertia); void getInertiaLocal(sead::Vector3f* inertia) const; void setLinearDamping(float value);