diff --git a/data/uking_functions.csv b/data/uking_functions.csv index 90a2dd7e..de0e6d8b 100644 --- a/data/uking_functions.csv +++ b/data/uking_functions.csv @@ -82966,8 +82966,8 @@ Address,Quality,Size,Name 0x0000007100f8c900,O,000168,_ZN4ksys4phys9RigidBodyD0Ev 0x0000007100f8c9a8,O,000168,_ZThn32_N4ksys4phys9RigidBodyD0Ev 0x0000007100f8ca50,O,000584,_ZN4ksys4phys9RigidBody34initMotionAccessorForDynamicMotionEPN4sead4HeapE -0x0000007100f8cc98,U,000172,phys::RigidBody::initRigidMotion -0x0000007100f8cd44,U,000552,phys::RigidBody::initMotion +0x0000007100f8cc98,O,000172,_ZN4ksys4phys9RigidBody18initMotionAccessorERKNS0_22RigidBodyInstanceParamEPN4sead4HeapEb +0x0000007100f8cd44,O,000552,_ZN4ksys4phys9RigidBody12createMotionEP16hkpMaxSizeMotionNS0_10MotionTypeERKNS0_22RigidBodyInstanceParamE 0x0000007100f8cf6c,O,000052,_ZNK4ksys4phys9RigidBody13getHkBodyNameEv 0x0000007100f8cfa0,U,000424,phys::RigidBody::x_0 0x0000007100f8d148,O,000144,_ZN4ksys4phys9RigidBody13setMotionFlagENS1_10MotionFlagE @@ -83036,7 +83036,7 @@ Address,Quality,Size,Name 0x0000007100f90094,U,000968,phys::RigidBody::x_3 0x0000007100f9045c,U,001132,phys::RigidBody::x_39 0x0000007100f908c8,U,001632,phys::RigidBody::x_40 -0x0000007100f90f28,U,000140,phys::RigidBody::getMotionAccessorType1_0 +0x0000007100f90f28,U,000140,phys::RigidBody::getMotionAccessorForProxy 0x0000007100f90fb4,U,000332,phys::RigidBody::x_41 0x0000007100f91100,U,000140,phys::RigidBody::x_42 0x0000007100f9118c,U,000032,phys::RigidBody::getLinearVelocity @@ -83132,9 +83132,9 @@ Address,Quality,Size,Name 0x0000007100f96700,U,000068,phys::RigidBody::m5 0x0000007100f96744,U,000176,phys::RigidBody::x_118 0x0000007100f967f4,U,000428,phys::RigidBody::x_119 -0x0000007100f969a0,U,000072,phys::RigidBody::x_120 -0x0000007100f969e8,U,000088,phys::RigidBody::x_121 -0x0000007100f96a40,U,000012,phys::RigidBody::x_122 +0x0000007100f969a0,U,000072,phys::RigidBody::lock +0x0000007100f969e8,U,000088,phys::RigidBody::unlock +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 @@ -93414,8 +93414,8 @@ Address,Quality,Size,Name 0x000000710121542c,U,000276, 0x0000007101215540,U,000016, 0x0000007101215550,U,000516,PhysicsMemSys::runJobsMaybe -0x0000007101215754,U,000048,PhysicsMemSys::__auto13 -0x0000007101215784,U,000048,PhysicsMemSys::__auto16 +0x0000007101215754,U,000048,PhysicsMemSys::lockWorld +0x0000007101215784,U,000048,PhysicsMemSys::unlockWorld 0x00000071012157b4,U,000464,PhysicsMemSys::__auto0 0x0000007101215984,U,000484,PhysicsMemSys::initGroupFilterXs 0x0000007101215b68,U,000036,PhysicsMemSys::removeGroupFilterX @@ -95480,8 +95480,8 @@ Address,Quality,Size,Name 0x00000071012b3948,U,000908,phys::createHkpWorldStuff 0x00000071012b3cd4,U,000020, 0x00000071012b3ce8,U,000052, -0x00000071012b3d1c,U,000104, -0x00000071012b3d84,U,000104, +0x00000071012b3d1c,U,000104,phys::World::lockCS +0x00000071012b3d84,U,000104,phys::World::unlockCS 0x00000071012b3dec,O,000356,_ZN13hkpWorldCinfoD2Ev 0x00000071012b3f50,U,000096, 0x00000071012b3fb0,U,000024, diff --git a/lib/hkStubs/Havok/Common/Base/Math/Quaternion/hkQuaternionf.h b/lib/hkStubs/Havok/Common/Base/Math/Quaternion/hkQuaternionf.h index 3619fc13..3508cfbd 100644 --- a/lib/hkStubs/Havok/Common/Base/Math/Quaternion/hkQuaternionf.h +++ b/lib/hkStubs/Havok/Common/Base/Math/Quaternion/hkQuaternionf.h @@ -23,6 +23,8 @@ public: HK_FORCE_INLINE void mul(hkQuaternionfParameter q); HK_FORCE_INLINE void setMul(hkQuaternionfParameter q0, hkQuaternionfParameter q1); + HK_FORCE_INLINE static const hkQuaternionf& getIdentity(); + hkVector4f m_vec; }; @@ -76,3 +78,7 @@ inline void hkQuaternionf::setMul(hkQuaternionfParameter r, hkQuaternionfParamet vec.addMul(qReal, rImag); m_vec.setXYZ_W(vec, (rReal * qReal) - rImag.dot<3>(qImag)); } + +inline const hkQuaternionf& hkQuaternionf::getIdentity() { + return reinterpret_cast(g_vectorfConstants[HK_QUADREAL_0001]); +} diff --git a/src/KingSystem/Physics/RigidBody/physRigidBody.cpp b/src/KingSystem/Physics/RigidBody/physRigidBody.cpp index b6dea349..017437a9 100644 --- a/src/KingSystem/Physics/RigidBody/physRigidBody.cpp +++ b/src/KingSystem/Physics/RigidBody/physRigidBody.cpp @@ -1,6 +1,8 @@ #include "KingSystem/Physics/RigidBody/physRigidBody.h" #include #include +#include +#include #include "KingSystem/Physics/RigidBody/physMotionAccessor.h" #include "KingSystem/Physics/RigidBody/physRigidBodyMotion.h" #include "KingSystem/Physics/RigidBody/physRigidBodyMotionProxy.h" @@ -10,6 +12,8 @@ namespace ksys::phys { +constexpr float MinInertia = 0.001; + RigidBody::RigidBody(Type type, u32 mass_scaling, hkpRigidBody* hk_body, const sead::SafeString& name, sead::Heap* heap, bool a7) : mCS(heap), mHkBody(hk_body), mRigidBodyAccessor(hk_body), mType(type) { @@ -44,15 +48,19 @@ RigidBody::~RigidBody() { } } +inline void RigidBody::createMotionAccessor(sead::Heap* heap) { + if (isMassScaling()) + mMotionAccessor = new (heap) RigidBodyMotionProxy(this); + else + mMotionAccessor = new (heap) RigidBodyMotion(this); +} + namespace { struct RigidBodyDynamicInstanceParam : RigidBodyInstanceParam {}; } // namespace bool RigidBody::initMotionAccessorForDynamicMotion(sead::Heap* heap) { - if (isMassScaling()) - mMotionAccessor = new (heap) RigidBodyMotionProxy(this); - else - mMotionAccessor = new (heap) RigidBodyMotion(this); + createMotionAccessor(heap); RigidBodyDynamicInstanceParam param; auto* body = getHkBody(); @@ -61,7 +69,6 @@ bool RigidBody::initMotionAccessorForDynamicMotion(sead::Heap* heap) { hkMatrix3 inertia; body->getInertiaLocal(inertia); - constexpr float MinInertia = 0.001; param.inertia = {sead::Mathf::max(inertia(0, 0), MinInertia), sead::Mathf::max(inertia(1, 1), MinInertia), sead::Mathf::max(inertia(2, 2), MinInertia)}; @@ -77,6 +84,76 @@ bool RigidBody::initMotionAccessorForDynamicMotion(sead::Heap* heap) { return true; } +bool RigidBody::initMotionAccessor(const RigidBodyInstanceParam& param, sead::Heap* heap, + bool init_motion) { + if (init_motion) + createMotion(static_cast(getMotion()), param.motion_type, param); + + createMotionAccessor(heap); + mMotionAccessor->init(param, heap); + return true; +} + +bool RigidBody::createMotion(hkpMaxSizeMotion* motion, MotionType motion_type, + const RigidBodyInstanceParam& param) { + auto position = hkVector4f::zero(); + auto rotation = hkQuaternionf::getIdentity(); + + hkVector4f center_of_mass; + loadFromVec3(¢er_of_mass, param.center_of_mass); + + auto velocity = hkVector4f::zero(); + + switch (motion_type) { + case MotionType::Fixed: + new (motion) hkpFixedRigidMotion(position, rotation); + break; + + case MotionType::Dynamic: { + hkMatrix3f inertia_local; + inertia_local.m_col0.set(sead::Mathf::max(param.inertia.x, MinInertia), 0, 0); + inertia_local.m_col1.set(0, sead::Mathf::max(param.inertia.y, MinInertia), 0); + inertia_local.m_col2.set(0, 0, sead::Mathf::max(param.inertia.z, MinInertia)); + + hkpRigidBody::createDynamicRigidMotion( + hkpMotion::MOTION_DYNAMIC, position, rotation, param.mass, inertia_local, + center_of_mass, param.max_linear_velocity, param.max_angular_velocity_rad, motion); + + motion->getMotionState()->m_maxLinearVelocity = param.max_linear_velocity; + motion->getMotionState()->m_maxAngularVelocity = param.max_angular_velocity_rad; + motion->setLinearDamping(param.linear_damping); + motion->setAngularDamping(param.angular_damping); + motion->setTimeFactor(param.time_factor); + motion->setGravityFactor(param.gravity_factor); + motion->setLinearVelocity(velocity); + motion->setAngularVelocity(velocity); + break; + } + + case MotionType::Keyframed: + new (motion) hkpKeyframedRigidMotion(position, rotation); + motion->setCenterOfMassInLocal(center_of_mass); + motion->getMotionState()->m_maxLinearVelocity = param.max_linear_velocity; + motion->getMotionState()->m_maxAngularVelocity = param.max_angular_velocity_rad; + motion->setTimeFactor(param.time_factor); + motion->setLinearVelocity(velocity); + motion->setAngularVelocity(velocity); + break; + + case MotionType::Unknown: + case MotionType::Invalid: + break; + } + + if (mFlags.isOff(Flag::_2000000) && mFlags.isOff(Flag::_4000000) && + mFlags.isOff(Flag::_8000000)) { + mHkBody->enableDeactivation(false); + mHkBody->enableDeactivation(true); + } + + return true; +} + sead::SafeString RigidBody::getHkBodyName() const { const char* name = mHkBody->getName(); if (!name) @@ -148,4 +225,8 @@ void RigidBody::setContactNone() { mContactMask.makeAllZero(); } +hkpMotion* RigidBody::getMotion() const { + return getHkBody()->getMotion(); +} + } // namespace ksys::phys diff --git a/src/KingSystem/Physics/RigidBody/physRigidBody.h b/src/KingSystem/Physics/RigidBody/physRigidBody.h index 68995b0f..3f76d8fe 100644 --- a/src/KingSystem/Physics/RigidBody/physRigidBody.h +++ b/src/KingSystem/Physics/RigidBody/physRigidBody.h @@ -14,6 +14,7 @@ class hkQuaternionf; class hkVector4f; class hkpRigidBody; +class hkpMaxSizeMotion; class hkpMotion; namespace ksys::phys { @@ -63,6 +64,14 @@ public: _20000 = 1 << 17, _40000 = 1 << 18, _80000 = 1 << 19, + _100000 = 1 << 20, + _200000 = 1 << 21, + _400000 = 1 << 22, + _800000 = 1 << 23, + _1000000 = 1 << 24, + _2000000 = 1 << 25, + _4000000 = 1 << 26, + _8000000 = 1 << 27, }; enum class MotionFlag { @@ -106,13 +115,12 @@ public: virtual void m13(); virtual const char* getName(); - // 0x0000007100f8ca50 bool initMotionAccessorForDynamicMotion(sead::Heap* heap); - // 0x0000007100f8cc98 - void initMotionAccessor(); - // 0x0000007100f8cd44 - void initMotion(hkpMotion* motion, MotionType motion_type, - const RigidBodyInstanceParam& params); + bool initMotionAccessor(const RigidBodyInstanceParam& param, sead::Heap* heap, + bool init_motion); + /// Create a hkpMotion in the specified motion storage and initialise it. + bool createMotion(hkpMaxSizeMotion* motion, MotionType motion_type, + const RigidBodyInstanceParam& param); sead::SafeString getHkBodyName() const; @@ -184,7 +192,6 @@ public: void sub_7100F8F8CC(ContactLayer, GroundHit, void*); void sub_7100F8F9E8(ReceiverMask*, void*); void sub_7100F8FA44(ContactLayer, u32); - hkpMotion* getMotion() const; // 0x0000007100f9004c void getTransform(sead::Matrix34f* mtx) const; @@ -251,6 +258,8 @@ public: // 0x0000007100f969e8 void unlock(bool also_unlock_world); + hkpMotion* getMotion() const; + class ScopedLock { public: explicit ScopedLock(RigidBody* body, bool also_lock_world) @@ -271,6 +280,8 @@ public: } private: + void createMotionAccessor(sead::Heap* heap); + sead::CriticalSection mCS; sead::TypedBitFlag> mFlags{}; sead::TypedBitFlag> mMotionFlags{}; diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodyMotion.cpp b/src/KingSystem/Physics/RigidBody/physRigidBodyMotion.cpp index 472c0fea..ee2daabb 100644 --- a/src/KingSystem/Physics/RigidBody/physRigidBodyMotion.cpp +++ b/src/KingSystem/Physics/RigidBody/physRigidBodyMotion.cpp @@ -30,7 +30,7 @@ RigidBodyMotion::~RigidBodyMotion() { bool RigidBodyMotion::init(const RigidBodyInstanceParam& params, sead::Heap* heap) { auto* motion_storage = new (heap, alignof(hkpMaxSizeMotion)) u8[sizeof(hkpMaxSizeMotion)]; mMotion = new (motion_storage) hkpMaxSizeMotion; - mBody->initMotion(mMotion, MotionType::Dynamic, params); + mBody->createMotion(static_cast(mMotion), MotionType::Dynamic, params); mMaxImpulse = params.max_impulse; mColImpulseScale = params.col_impulse_scale; mFrictionScale = params.friction_scale; diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodyMotion.h b/src/KingSystem/Physics/RigidBody/physRigidBodyMotion.h index c029fe7d..8609abd7 100644 --- a/src/KingSystem/Physics/RigidBody/physRigidBodyMotion.h +++ b/src/KingSystem/Physics/RigidBody/physRigidBodyMotion.h @@ -7,6 +7,7 @@ #include "KingSystem/Physics/RigidBody/physMotionAccessor.h" #include "KingSystem/Physics/RigidBody/physRigidBody.h" +class hkpMaxSizeMotion; class hkpMotion; namespace ksys::phys { diff --git a/src/KingSystem/Physics/physConversions.h b/src/KingSystem/Physics/physConversions.h index 84e47f48..507bfe34 100644 --- a/src/KingSystem/Physics/physConversions.h +++ b/src/KingSystem/Physics/physConversions.h @@ -17,10 +17,6 @@ inline void toVec3(sead::Vector3f* out, const hkVector4f& vec) { return {vec.getX(), vec.getY(), vec.getZ()}; } -inline void storeToVec3(sead::Vector3f* out, const hkVector4f& vec) { - vec.store<3>(out->e.data()); -} - inline void toHkVec4(hkVector4f* out, const sead::Vector3f& vec) { out->set(vec.x, vec.y, vec.z); } @@ -29,6 +25,15 @@ inline void toHkVec4(hkVector4f* out, const sead::Vector3f& vec) { return {vec.x, vec.y, vec.z}; } +inline void storeToVec3(sead::Vector3f* out, const hkVector4f& vec) { + vec.store<3>(out->e.data()); +} + +inline void loadFromVec3(hkVector4f* out, const sead::Vector3f& vec) { + out->load<3>(vec.e.data()); + out->setW(0); +} + 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()); }