ksys/phys: Implement the rest of the easy RagdollController functions

3 files changed, 162 insertions, 14 deletions

diff --git a/src/KingSystem/Physics/Ragdoll/physRagdollController.cpp b/src/KingSystem/Physics/Ragdoll/physRagdollController.cpp
index 9aee5065..de69b4a0 100644
--- a/src/KingSystem/Physics/Ragdoll/physRagdollController.cpp
+++ b/src/KingSystem/Physics/Ragdoll/physRagdollController.cpp
@@ -26,14 +26,11 @@
 
 namespace ksys::phys {
 
-static u8 sUnk1 = 15;
-
-void RagdollController::setUnk1(u8 value) {
-    sUnk1 = value;
-}
+static u8 sRagdollCtrlUnk1 = 15;
+static RagdollController::Config sRagdollCtrlConfig;
 
 RagdollController::RagdollController(SystemGroupHandler* handler)
-    : mGroupHandler(handler), _e8(sUnk1), _e9(sUnk1) {}
+    : mGroupHandler(handler), _e8(sRagdollCtrlUnk1), _e9(sRagdollCtrlUnk1) {}
 
 RagdollController::~RagdollController() {
     finalize();
@@ -471,6 +468,128 @@ RagdollRigidBody* RagdollController::getChildBoneRigidBody(const RigidBody* body
     return sead::DynamicCast<const RagdollRigidBody>(body)->getChildBodies_()[index];
 }
 
+int RagdollController::getConstraintIndexByName(const sead::SafeString& name) const {
+    for (int i = 0, n = getNumConstraints(); i < n; ++i) {
+        if (name == mRagdollInstance->m_constraints[i]->getName())
+            return i;
+    }
+    return -1;
+}
+
+int RagdollController::getNumConstraints() const {
+    return mRagdollInstance->m_constraints.size();
+}
+
+void RagdollController::enableConstraint(int index, bool enable) {
+    mDisabledConstraints.changeBit(index, !enable);
+}
+
+bool RagdollController::isConstraintEnabled(int index) const {
+    return mDisabledConstraints.isOffBit(index);
+}
+
+void RagdollController::setContactLayer(ContactLayer layer) {
+    if (mContactLayer.value() == layer)
+        return;
+
+    if (mFlags.isOn(Flag::_10)) {
+        for (int bone = 0, num_bones = mBoneRigidBodies.size(); bone < num_bones; ++bone) {
+            if (mKeyframedBones.isOffBit(bone))
+                mBoneRigidBodies[bone]->setContactLayer(layer);
+        }
+    }
+
+    mContactLayer = layer;
+}
+
+void RagdollController::setKeyframed(int bone_index, bool keyframed,
+                                     SyncToThisBone sync_to_this_bone) {
+    mKeyframedBones.changeBit(bone_index, keyframed);
+
+    if (keyframed) {
+        mBoneRigidBodies[bone_index]->setContactLayer(ContactLayer::EntityNoHit);
+        mBoneRigidBodies[bone_index]->changeMotionType(MotionType::Keyframed);
+    } else {
+        mBoneRigidBodies[bone_index]->setContactLayer(mContactLayer);
+        mBoneRigidBodies[bone_index]->changeMotionType(MotionType::Dynamic);
+    }
+
+    mKeyframedBonesToSyncTo.changeBit(bone_index, keyframed && bool(sync_to_this_bone));
+}
+
+// NON_MATCHING: swapped csel operands
+void RagdollController::setUnk1(u8 value) {
+    value = sead::Mathi::min(value, sRagdollCtrlUnk1);
+    _e9 = value;
+    _e8 = value;
+}
+
+void RagdollController::setMaximumUnk1(u8 value) {
+    sRagdollCtrlUnk1 = value;
+}
+
+void RagdollController::stopForcingKeyframing() {
+    if (mFlags.isOn(Flag::_10)) {
+        for (int i = 0, n = mBoneRigidBodies.size(); i < n; ++i) {
+            setKeyframed(i, false, {});
+        }
+    } else {
+        mKeyframedBones.makeAllZero();
+        mKeyframedBonesToSyncTo.makeAllZero();
+    }
+}
+
+void RagdollController::update() {
+    if (!isAddedToWorld()) {
+        for (int i = 0, n = getNumConstraints(); i < n; ++i) {
+            auto* constraint = mRagdollInstance->m_constraints[i];
+            if (constraint->getOwner() != nullptr) {
+                System::instance()
+                    ->getHavokWorld(ContactLayerType::Entity)
+                    ->removeConstraint(constraint);
+            }
+        }
+        return;
+    }
+
+    for (int i = 0, n = getNumConstraints(); i < n; ++i) {
+        auto* constraint = mRagdollInstance->m_constraints[i];
+        if (constraint->getOwner() == nullptr) {
+            System::instance()->getHavokWorld(ContactLayerType::Entity)->addConstraint(constraint);
+        }
+
+        const bool should_enable = mDisabledConstraints.isOffBit(i);
+        if (should_enable != constraint->isEnabled()) {
+            if (should_enable) {
+                constraint->setPriority(getConfig().priority);
+                constraint->enable();
+            } else {
+                constraint->setPriority(hkpConstraintInstance::PRIORITY_PSI);
+                constraint->disable();
+            }
+        }
+    }
+
+    updateGravityFactorOverride();
+}
+
+RagdollController::Config& RagdollController::getConfig() {
+    return sRagdollCtrlConfig;
+}
+
+void RagdollController::updateGravityFactorOverride() {
+    if (mFlags.isOff(Flag::_200))
+        return;
+
+    if (System::instance() == nullptr)
+        return;
+
+    const float factor_divisor = System::instance()->get6c();
+    for (int i = 0, n = mBoneRigidBodies.size(); i < n; ++i) {
+        mBoneRigidBodies[i]->setGravityFactor(mGravityFactorOverride / factor_divisor);
+    }
+}
+
 RagdollController::ScopedPhysicsLock::ScopedPhysicsLock(const RagdollController* ctrl)
     : mCtrl{ctrl}, mWorldLock{ctrl->isAddedToWorld(), ContactLayerType::Entity} {
     for (auto body : util::indexIter(ctrl->mBoneRigidBodies))
diff --git a/src/KingSystem/Physics/Ragdoll/physRagdollController.h b/src/KingSystem/Physics/Ragdoll/physRagdollController.h
index 3c333630..c7524abd 100644
--- a/src/KingSystem/Physics/Ragdoll/physRagdollController.h
+++ b/src/KingSystem/Physics/Ragdoll/physRagdollController.h
@@ -1,5 +1,6 @@
 #pragma once
 
+#include <Havok/Physics2012/Dynamics/Constraint/hkpConstraintInstance.h>
 #include <container/seadBuffer.h>
 #include <hostio/seadHostIONode.h>
 #include <math/seadMatrix.h>
@@ -40,15 +41,24 @@ enum class PreserveVelocities : bool;
 // TODO
 class RagdollController : public sead::hostio::Node {
 public:
+    enum class SyncToThisBone : bool;
+
+    struct Config {
+        float _0;
+        float _4;
+        float _8;
+        float _c;
+        float _10;
+        float _14;
+        hkpConstraintInstance::ConstraintPriority priority = hkpConstraintInstance::PRIORITY_TOI;
+    };
+
     explicit RagdollController(SystemGroupHandler* handler);
     virtual ~RagdollController();
 
     bool init(const RagdollParam* param, sead::DirectResource* res, gsys::Model* model,
               sead::Heap* heap);
 
-    // 0x0000007101223320
-    void update();
-
     bool isAddedToWorld() const;
     void removeFromWorldImmediately();
     void removeFromWorld();
@@ -93,8 +103,22 @@ public:
     int getNumChildBones(const RigidBody* body) const;
     RagdollRigidBody* getChildBoneRigidBody(const RigidBody* body, int index) const;
 
-    static void setUnk1(u8 value);
+    int getConstraintIndexByName(const sead::SafeString& name) const;
+    int getNumConstraints() const;
+    void enableConstraint(int index, bool enable);
+    bool isConstraintEnabled(int index) const;
 
+    void setContactLayer(ContactLayer layer);
+    /// Sets whether a bone is keyframed.
+    /// @param sync_to_this_bone Only used if keyframed = true.
+    void setKeyframed(int bone_index, bool keyframed, SyncToThisBone sync_to_this_bone);
+    void setUnk1(u8 value);
+    static void setMaximumUnk1(u8 value);
+    void stopForcingKeyframing();
+
+    void update();
+
+    static Config& getConfig();
     auto& getRigidBodies_() { return mBoneRigidBodies; }
 
 private:
@@ -109,9 +133,11 @@ private:
     };
 
     enum class Flag {
+        _10 = 0x10,
         _80 = 0x80,
         /// Whether this controller has been registered with the RagdollControllerMgr.
         IsRegistered = 0x100,
+        _200 = 0x200,
     };
 
     struct BoneVectors {
@@ -130,6 +156,8 @@ private:
     void registerSelf();
     void unregisterSelf();
 
+    void updateGravityFactorOverride();
+
     BoneAccessor* getBoneAccessor() const;
 
     SkeletonMapper* mSkeletonMapper = nullptr;
@@ -150,16 +178,16 @@ private:
     // TODO: rename
     sead::Buffer<float> mBoneStuff2;
     float _98 = 0.1;
-    float _9c = 1.0;
+    float mGravityFactorOverride = 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;
+    sead::BitFlag32 mDisabledConstraints;
+    sead::BitFlag32 mKeyframedBones;
+    sead::BitFlag32 mKeyframedBonesToSyncTo;
     gsys::Model* mModel = nullptr;
     RigidBody* mExtraRigidBody = nullptr;
     void* _e0 = nullptr;
diff --git a/src/KingSystem/Physics/System/physSystem.h b/src/KingSystem/Physics/System/physSystem.h
index d5d259d5..436e4727 100644
--- a/src/KingSystem/Physics/System/physSystem.h
+++ b/src/KingSystem/Physics/System/physSystem.h
@@ -42,6 +42,7 @@ class System {
 
 public:
     float get64() const { return _64; }
+    float get6c() const { return _6c; }
     float getTimeFactor() const { return mTimeFactor; }
     ContactMgr* getContactMgr() const { return mContactMgr; }
     StaticCompoundMgr* getStaticCompoundMgr() const { return mStaticCompoundMgr; }