ksys/phys: Start adding RagdollController

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;
@@ -169,6 +170,11 @@ inline int hkArrayBase<T>::getSize() const {
 }
 
 template <typename T>
+inline int hkArrayBase<T>::size() const {
+    return getSize();
+}
+
+template <typename T>
 inline int hkArrayBase<T>::getCapacity() const {
     return m_capacityAndFlags & static_cast<int>(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 <Havok/Animation/Animation/Rig/hkaPose.h>
+#include <Havok/Animation/Physics2012Bridge/Instance/hkaRagdollInstance.h>
+#include <Havok/Common/Serialize/Util/hkNativePackfileUtils.h>
+#include <Havok/Physics2012/Dynamics/Constraint/hkpConstraintInstance.h>
+#include <Havok/Physics2012/Dynamics/Entity/hkpRigidBody.h>
+#include <Havok/Physics2012/Dynamics/World/hkpPhysicsSystem.h>
+#include <Havok/Physics2012/Dynamics/World/hkpWorld.h>
+#include <Havok/Physics2012/Utilities/Dynamics/ScaleSystem/hkpSystemScalingUtility.h>
+#include <math/seadMathCalcCommon.h>
+#include <optional>
+#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 <container/seadBuffer.h>
+#include <hostio/seadHostIONode.h>
+#include <math/seadMatrix.h>
+#include <math/seadVector.h>
+#include <prim/seadTypedBitFlag.h>
+#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<RigidBody*> mRigidBodies;
+    // TODO: rename
+    sead::Buffer<BoneVectors> mBoneVectors;
+    // TODO: rename
+    sead::Buffer<u32> mBoneStuff;
+    hkRootLevelContainer* mRootLevelContainer = nullptr;
+    sead::SafeString mName;
+    u8* mRawRagdollData = nullptr;
+    int mRawRagdollDataSize = 0;
+    hkRotationf* mRotations = nullptr;
+    // TODO: type
+    sead::Buffer<void*> 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<Flag> 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;