From 9d3bc8cfe1c4ddd74c3b072bbe6418665aa06de1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?L=C3=A9o=20Lam?= Date: Tue, 21 Jun 2022 12:10:34 +0200 Subject: [PATCH] ksys/phys: Start adding RagdollController --- data/uking_functions.csv | 26 +-- .../Common/Base/Container/Array/hkArray.h | 6 + .../Physics/Ragdoll/physRagdollController.cpp | 218 ++++++++++++++++++ .../Physics/Ragdoll/physRagdollController.h | 116 +++++++++- .../Physics/Rig/physSkeletonMapper.h | 2 + src/KingSystem/Physics/System/physSystem.h | 17 +- 6 files changed, 367 insertions(+), 18 deletions(-) diff --git a/data/uking_functions.csv b/data/uking_functions.csv index 433e025e..68ec6ebf 100644 --- a/data/uking_functions.csv +++ b/data/uking_functions.csv @@ -93574,19 +93574,19 @@ Address,Quality,Size,Name 0x000000710121d4c8,U,003172, 0x000000710121e12c,U,000088, 0x000000710121e184,U,000304, -0x000000710121e2b4,U,000164, -0x000000710121e358,U,000020, -0x000000710121e36c,U,000572, -0x000000710121e5a8,U,000052, +0x000000710121e2b4,O,000164,_ZN4ksys4phys17RagdollControllerC1EPNS0_18SystemGroupHandlerE +0x000000710121e358,O,000020,_ZN4ksys4phys17RagdollControllerD1Ev +0x000000710121e36c,O,000572,_ZN4ksys4phys17RagdollController8finalizeEv +0x000000710121e5a8,O,000052,_ZN4ksys4phys17RagdollControllerD0Ev 0x000000710121e5dc,U,001660,makeRagdollRigidBody -0x000000710121ec58,U,000032, -0x000000710121ec78,U,000512, -0x000000710121ee78,U,000120, -0x000000710121eef0,U,000192, -0x000000710121efb0,U,000032, -0x000000710121efd0,U,000260, -0x000000710121f0d4,U,000512, -0x000000710121f2d4,U,000736, +0x000000710121ec58,O,000032,_ZNK4ksys4phys17RagdollController14isAddedToWorldEv +0x000000710121ec78,O,000512,_ZN4ksys4phys17RagdollController26removeFromWorldImmediatelyEv +0x000000710121ee78,O,000120,_ZN4ksys4phys17RagdollController15removeFromWorldEv +0x000000710121eef0,O,000192,_ZN4ksys4phys17RagdollController28removeFromWorldAndResetLinksEv +0x000000710121efb0,O,000032,_ZNK4ksys4phys17RagdollController15isAddingToWorldEv +0x000000710121efd0,O,000260,_ZN4ksys4phys17RagdollController12setTransformERKN4sead8Matrix34IfEE +0x000000710121f0d4,O,000512,_ZN4ksys4phys17RagdollController12setTransformERK14hkQsTransformf +0x000000710121f2d4,O,000736,_ZN4ksys4phys17RagdollController8setScaleEf 0x000000710121f5b4,U,000076, 0x000000710121f600,U,000052, 0x000000710121f634,U,000108, @@ -93600,7 +93600,7 @@ Address,Quality,Size,Name 0x000000710122079c,U,000948, 0x0000007101220b50,U,000604, 0x0000007101220dac,U,001460, -0x0000007101221360,U,000004,nullsub_4713 +0x0000007101221360,O,000004,_ZN4ksys4phys17RagdollController2m3Ev 0x0000007101221364,U,000116, 0x00000071012213d8,U,000076, 0x0000007101221424,U,000632, diff --git a/lib/hkStubs/Havok/Common/Base/Container/Array/hkArray.h b/lib/hkStubs/Havok/Common/Base/Container/Array/hkArray.h index ba542278..13460547 100644 --- a/lib/hkStubs/Havok/Common/Base/Container/Array/hkArray.h +++ b/lib/hkStubs/Havok/Common/Base/Container/Array/hkArray.h @@ -30,6 +30,7 @@ public: auto operator=(const hkArrayBase&) = delete; HK_FORCE_INLINE int getSize() const; + HK_FORCE_INLINE int size() const; HK_FORCE_INLINE int getCapacity() const; HK_FORCE_INLINE int getCapacityAndFlags() const; HK_FORCE_INLINE hkBool isEmpty() const; @@ -168,6 +169,11 @@ inline int hkArrayBase::getSize() const { return m_size; } +template +inline int hkArrayBase::size() const { + return getSize(); +} + template inline int hkArrayBase::getCapacity() const { return m_capacityAndFlags & static_cast(CAPACITY_MASK); diff --git a/src/KingSystem/Physics/Ragdoll/physRagdollController.cpp b/src/KingSystem/Physics/Ragdoll/physRagdollController.cpp index fee0b35f..4f26679a 100644 --- a/src/KingSystem/Physics/Ragdoll/physRagdollController.cpp +++ b/src/KingSystem/Physics/Ragdoll/physRagdollController.cpp @@ -1 +1,219 @@ #include "KingSystem/Physics/Ragdoll/physRagdollController.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "KingSystem/Physics/Ragdoll/physRagdollControllerMgr.h" +#include "KingSystem/Physics/Rig/physModelBoneAccessor.h" +#include "KingSystem/Physics/Rig/physSkeletonMapper.h" +#include "KingSystem/Physics/RigidBody/physRigidBody.h" +#include "KingSystem/Physics/System/physGroupFilter.h" +#include "KingSystem/Physics/System/physSystem.h" +#include "KingSystem/Physics/physConversions.h" +#include "KingSystem/Utils/IteratorUtil.h" +#include "KingSystem/Utils/SafeDelete.h" + +namespace ksys::phys { + +static u8 sUnk1 = 15; + +void RagdollController::setUnk1(u8 value) { + sUnk1 = value; +} + +RagdollController::RagdollController(SystemGroupHandler* handler) + : mGroupHandler(handler), _e8(sUnk1), _e9(sUnk1) {} + +RagdollController::~RagdollController() { + finalize(); +} + +void RagdollController::finalize() { + for (auto body : util::indexIter(mRigidBodies)) { + body.get()->setCollisionInfo(nullptr); + body.get()->setContactPointInfo(nullptr); + } + + if (isAddedToWorld()) + removeFromWorldImmediately(); + + if (mRagdollInstance != nullptr) + removeConstraints(); + + for (auto body : util::indexIter(mRigidBodies)) + delete body.get(); + + mRigidBodies.freeBuffer(); + + if (mSkeletonMapper) + util::safeDelete(mSkeletonMapper); + + if (mModelBoneAccessor) + util::safeDelete(mModelBoneAccessor); + + if (mRawRagdollData) { + hkNativePackfileUtils::unloadInPlace(mRawRagdollData, mRawRagdollDataSize); + delete[] mRawRagdollData; + mRawRagdollData = nullptr; + mRootLevelContainer = nullptr; + mRagdollInstance = nullptr; + } + + if (mRotations) + util::safeDeleteArray(mRotations); + + mBoneVectors.freeBuffer(); + mBoneStuff.freeBuffer(); + mBones.freeBuffer(); + + if (_b0) { + delete[] _a8; + _b0 = 0; + } +} + +void RagdollController::removeConstraints() { + for (int i = 0, n = mRagdollInstance->m_constraints.getSize(); i < n; ++i) { + auto* constraint = mRagdollInstance->m_constraints[i]; + if (constraint->getOwner() != nullptr) { + auto* world = System::instance()->getHavokWorld(ContactLayerType::Entity); + world->removeConstraint(constraint); + } + } +} + +bool RagdollController::isAddedToWorld() const { + return mRigidBodies.size() > 0 && mRigidBodies.back()->isAddedToWorld(); +} + +void RagdollController::removeFromWorldImmediately() { + ScopedPhysicsLock lock{this}; + + removeConstraints(); + for (auto body : util::indexIter(mRigidBodies)) + body.get()->removeFromWorldImmediately(); +} + +void RagdollController::removeFromWorld() { + for (auto body : util::indexIter(mRigidBodies)) + body.get()->removeFromWorld(); +} + +bool RagdollController::removeFromWorldAndResetLinks() { + bool removed = true; + for (auto body : util::indexIter(mRigidBodies)) + removed &= body.get()->removeFromWorldAndResetLinks(); + + if (mFlags.isOn(Flag::IsRegistered)) { + System::instance()->getRagdollControllerMgr()->removeController(this); + mFlags.reset(Flag::IsRegistered); + } + + return removed; +} + +bool RagdollController::isAddingToWorld() const { + return mRigidBodies.size() > 0 && mRigidBodies.back()->isAddingBodyToWorld(); +} + +void RagdollController::setTransform(const sead::Matrix34f& transform) { + hkQsTransformf hk_transform; + toHkQsTransform(&hk_transform, transform, sead::Vector3f::ones); + setTransform(hk_transform); +} + +void RagdollController::setTransform(const hkQsTransformf& transform) { + ScopedPhysicsLock lock{this}; + + if (mSkeletonMapper) + mSkeletonMapper->mapPoseA(); + + mRagdollInstance->setPoseModelSpace( + getBoneAccessor()->getPose()->getSyncedPoseModelSpace().data(), transform); + + if (mExtraRigidBody) { + sead::Matrix34f old_transform; + mRigidBodies[mRigidBodyIndex]->getTransform(&old_transform); + mExtraRigidBody->setTransform(old_transform, true); + + if (mGroupHandler) { + mExtraRigidBody->setCollisionFilterInfo( + mGroupHandler->makeRagdollCollisionFilterInfo(GroundHit::HitAll)); + } + } +} + +void RagdollController::setScale(float scale) { + if (sead::Mathf::equalsEpsilon(scale, 1.0)) + return; + + { + ScopedPhysicsLock lock{this}; + + const hkVector4 translation = + mRagdollInstance->getRigidBodyOfBone(0)->getTransform().getTranslation(); + + // Construct a system with all the rigid bodies and constraints, then scale it. + hkpPhysicsSystem system; + + const int num_bodies = mRagdollInstance->getRigidBodyArray().size(); + for (int i = 0; i < num_bodies; ++i) { + hkpRigidBody* body = mRagdollInstance->getRigidBodyArray()[i]; + + // Fix the positions to be in model space. + hkVector4f pos; + pos.setSub(body->getTransform().getTranslation(), translation); + body->setPosition(pos); + + system.addRigidBody(body); + } + + for (int i = 0, n = mRagdollInstance->getConstraintArray().size(); i < n; ++i) { + system.addConstraint(mRagdollInstance->getConstraintArray()[i]); + } + + hkpSystemScalingUtility::scaleSystem(&system, scale); + + for (int i = 0; i < num_bodies; ++i) { + hkpRigidBody* body = mRagdollInstance->getRigidBodyArray()[i]; + + // Fix the positions to be in world space. + hkVector4f pos; + pos.setAdd(body->getTransform().getTranslation(), translation); + body->setPosition(pos); + + mRigidBodies[i]->updateShape(); + } + } + + if (mSkeletonMapper) + mSkeletonMapper->getBoneAccessor().setScale(scale); +} + +void RagdollController::m3() {} + +RagdollController::ScopedPhysicsLock::ScopedPhysicsLock(const RagdollController* ctrl) + : mCtrl{ctrl}, mWorldLock{ctrl->isAddedToWorld(), ContactLayerType::Entity} { + for (auto body : util::indexIter(ctrl->mRigidBodies)) + body.get()->lock(RigidBody::AlsoLockWorld::No); +} + +RagdollController::ScopedPhysicsLock::~ScopedPhysicsLock() { + for (auto body : util::indexIter(mCtrl->mRigidBodies)) + body.get()->unlock(RigidBody::AlsoLockWorld::No); +} + +BoneAccessor* RagdollController::getBoneAccessor() const { + if (mSkeletonMapper) + return &mSkeletonMapper->getBoneAccessor(); + + return mModelBoneAccessor; +} + +} // namespace ksys::phys diff --git a/src/KingSystem/Physics/Ragdoll/physRagdollController.h b/src/KingSystem/Physics/Ragdoll/physRagdollController.h index d9a4ad93..9823cc83 100644 --- a/src/KingSystem/Physics/Ragdoll/physRagdollController.h +++ b/src/KingSystem/Physics/Ragdoll/physRagdollController.h @@ -1,17 +1,131 @@ #pragma once +#include +#include +#include +#include +#include +#include "KingSystem/Physics/System/physSystem.h" #include "KingSystem/Physics/physDefines.h" +#include "KingSystem/Utils/Types.h" + +class hkaRagdollInstance; +class hkQsTransformf; +class hkRootLevelContainer; +class hkRotationf; + +namespace gsys { +class Model; +} + +namespace sead { +class DirectResource; +} namespace ksys::phys { +class BoneAccessor; +class ModelBoneAccessor; +class RagdollParam; +class RigidBody; +class SkeletonMapper; +class SystemGroupHandler; + // TODO -class RagdollController { +class RagdollController : public sead::hostio::Node { public: + explicit RagdollController(SystemGroupHandler* handler); + virtual ~RagdollController(); + + // 0x000000710121e5dc + void init(const RagdollParam* param, sead::DirectResource* res, gsys::Model* model, + sead::Heap* heap); + + // 0x0000007101223320 void update(); + bool isAddedToWorld() const; + void removeFromWorldImmediately(); + void removeFromWorld(); + bool removeFromWorldAndResetLinks(); + bool isAddingToWorld() const; + + void setTransform(const sead::Matrix34f& transform); + void setScale(float scale); + u32 sub_7101221CC4(); void sub_7101221728(ContactLayer layer); void sub_71012217A8(); + + // TODO: rename + virtual void m3(); + + static void setUnk1(u8 value); + +private: + class ScopedPhysicsLock { + public: + explicit ScopedPhysicsLock(const RagdollController* ctrl); + ~ScopedPhysicsLock(); + + private: + const RagdollController* mCtrl; + ScopedWorldLock mWorldLock; + }; + + enum class Flag { + /// Whether this controller has been registered with the RagdollControllerMgr. + IsRegistered = 0x100, + }; + + struct BoneVectors { + sead::Vector3f _0; + sead::Vector3f _c; + float _18; + }; + + void finalize(); + void removeConstraints(); + void setTransform(const hkQsTransformf& transform); + + BoneAccessor* getBoneAccessor() const; + + SkeletonMapper* mSkeletonMapper = nullptr; + ModelBoneAccessor* mModelBoneAccessor = nullptr; + hkaRagdollInstance* mRagdollInstance = nullptr; + SystemGroupHandler* mGroupHandler = nullptr; + sead::Buffer mRigidBodies; + // TODO: rename + sead::Buffer mBoneVectors; + // TODO: rename + sead::Buffer mBoneStuff; + hkRootLevelContainer* mRootLevelContainer = nullptr; + sead::SafeString mName; + u8* mRawRagdollData = nullptr; + int mRawRagdollDataSize = 0; + hkRotationf* mRotations = nullptr; + // TODO: type + sead::Buffer mBones; + float _98 = 0.1; + float _9c = 1.0; + u32 _a0 = 0; + // TODO: type + u8* _a8 = nullptr; + u32 _b0 = 0; + const RagdollParam* mRagdollParam = nullptr; + sead::TypedBitFlag mFlags; + u32 _c4 = 0; + u32 _c8 = 0; + u32 _cc = 0; + gsys::Model* mModel = nullptr; + RigidBody* mExtraRigidBody = nullptr; + void* _e0 = nullptr; + u8 _e8; + u8 _e9; + u8 _ea = 0; + u8 mRigidBodyIndex = 0; + u32 _ec = 5; }; +KSYS_CHECK_SIZE_NX150(RagdollController, 0xf0); } // namespace ksys::phys diff --git a/src/KingSystem/Physics/Rig/physSkeletonMapper.h b/src/KingSystem/Physics/Rig/physSkeletonMapper.h index 04ebb16a..8972cce7 100644 --- a/src/KingSystem/Physics/Rig/physSkeletonMapper.h +++ b/src/KingSystem/Physics/Rig/physSkeletonMapper.h @@ -19,6 +19,8 @@ public: void mapPoseA(); void mapPoseB(); + BoneAccessor& getBoneAccessor() { return mBoneAccessor; } + private: BoneAccessor mBoneAccessor; ModelBoneAccessor mModelBoneAccessor; diff --git a/src/KingSystem/Physics/System/physSystem.h b/src/KingSystem/Physics/System/physSystem.h index 9d363650..d5d259d5 100644 --- a/src/KingSystem/Physics/System/physSystem.h +++ b/src/KingSystem/Physics/System/physSystem.h @@ -20,6 +20,7 @@ class GroupFilter; class LayerContactPointInfo; class MaterialTable; class RayCastForRequest; +class RagdollControllerMgr; class RigidBody; class RigidBodyRequestMgr; class StaticCompoundMgr; @@ -45,6 +46,7 @@ public: ContactMgr* getContactMgr() const { return mContactMgr; } StaticCompoundMgr* getStaticCompoundMgr() const { return mStaticCompoundMgr; } RigidBodyRequestMgr* getRigidBodyRequestMgr() const { return mRigidBodyRequestMgr; } + RagdollControllerMgr* getRagdollControllerMgr() const { return mRagdollControllerMgr; } SystemData* getSystemData() const { return mSystemData; } MaterialTable* getMaterialTable() const { return mMaterialTable; } @@ -148,7 +150,7 @@ private: void* _150; StaticCompoundMgr* mStaticCompoundMgr; RigidBodyRequestMgr* mRigidBodyRequestMgr; - void* _168; + RagdollControllerMgr* mRagdollControllerMgr; void* mRigidBodyDividedMeshShapeMgr; SystemData* mSystemData; MaterialTable* mMaterialTable; @@ -168,19 +170,26 @@ class ScopedWorldLock { public: explicit ScopedWorldLock(ContactLayerType type, const char* description = nullptr, int unk = 0, OnlyLockIfNeeded only_lock_if_needed = OnlyLockIfNeeded::No) - : mType(type), mDescription(description), mUnk(unk), + : ScopedWorldLock(true, type, description, unk, only_lock_if_needed) {} + + ScopedWorldLock(bool condition, ContactLayerType type, const char* description = nullptr, + int unk = 0, OnlyLockIfNeeded only_lock_if_needed = OnlyLockIfNeeded::No) + : mCondition(condition), mType(type), mDescription(description), mUnk(unk), mOnlyLockIfNeeded(only_lock_if_needed) { - System::instance()->lockWorld(mType, mDescription, mUnk, mOnlyLockIfNeeded); + if (mCondition) + System::instance()->lockWorld(mType, mDescription, mUnk, mOnlyLockIfNeeded); } ~ScopedWorldLock() { - System::instance()->unlockWorld(mType, mDescription, mUnk, mOnlyLockIfNeeded); + if (mCondition) + System::instance()->unlockWorld(mType, mDescription, mUnk, mOnlyLockIfNeeded); } ScopedWorldLock(const ScopedWorldLock&) = delete; auto operator=(const ScopedWorldLock&) = delete; private: + bool mCondition; ContactLayerType mType; const char* mDescription; int mUnk;