ksys/phys: Add RagdollController (the real one, not RagdollInstance)

7 files changed, 227 insertions, 0 deletions

diff --git a/src/KingSystem/Physics/CMakeLists.txt b/src/KingSystem/Physics/CMakeLists.txt
index 1c682ccc..a4efb365 100644
--- a/src/KingSystem/Physics/CMakeLists.txt
+++ b/src/KingSystem/Physics/CMakeLists.txt
@@ -11,6 +11,8 @@ target_sources(uking PRIVATE
 
     Ragdoll/physRagdollConfig.cpp
     Ragdoll/physRagdollConfig.h
+    Ragdoll/physRagdollController.cpp
+    Ragdoll/physRagdollController.h
     Ragdoll/physRagdollControllerKeyList.h
     Ragdoll/physRagdollControllerKeyList.cpp
     Ragdoll/physRagdollInstance.cpp
diff --git a/src/KingSystem/Physics/Ragdoll/physRagdollController.cpp b/src/KingSystem/Physics/Ragdoll/physRagdollController.cpp
new file mode 100644
index 00000000..f8ded3a1
--- /dev/null
+++ b/src/KingSystem/Physics/Ragdoll/physRagdollController.cpp
@@ -0,0 +1,162 @@
+#include "KingSystem/Physics/Ragdoll/physRagdollController.h"
+#include <Havok/Animation/Physics2012Bridge/Controller/RigidBody/hkaRagdollRigidBodyController.h>
+#include <cmath>
+#include <math/seadMathCalcCommon.h>
+#include "KingSystem/Physics/Ragdoll/physRagdollControllerKeyList.h"
+#include "KingSystem/Physics/Ragdoll/physRagdollInstance.h"
+#include "KingSystem/Physics/System/physSystem.h"
+#include "KingSystem/Utils/Debug.h"
+#include "KingSystem/Utils/SafeDelete.h"
+
+namespace ksys::phys {
+
+static bool sForceDefaultWeights = false;
+
+void RagdollController::forceDefaultWeights(bool force) {
+    sForceDefaultWeights = force;
+}
+
+RagdollController::RagdollController() = default;
+
+RagdollController::~RagdollController() {
+    if (mRagdollRigidBodyCtrl)
+        util::safeDelete(mRagdollRigidBodyCtrl);
+    mConfiguredBoneWeights.freeBuffer();
+    mEffectiveBoneWeights.freeBuffer();
+    mMultipliers.freeBuffer();
+}
+
+bool RagdollController::init(const sead::SafeString& name, const sead::SafeString& system_key,
+                             RagdollInstance* instance, sead::Heap* heap) {
+    mName = name;
+    mInstance = instance;
+    util::PrintDebugFmt("creating RagdollController for %s", mName.cstr());
+    mRagdollRigidBodyCtrl = new hkaRagdollRigidBodyController(instance->getHavokRagdollInstance());
+
+    const int num_bones = instance->getRigidBodies_().size();
+
+    mConfiguredBoneWeights.allocBufferAssert(num_bones, heap);
+    mEffectiveBoneWeights.allocBufferAssert(num_bones, heap);
+    mMultipliers.allocBufferAssert(num_bones, heap);
+
+    for (int i = 0; i < num_bones; ++i) {
+        mConfiguredBoneWeights[i] = 1.0f;
+        mEffectiveBoneWeights[i] = 1.0f;
+        mMultipliers[i] = 1.0f;
+    }
+
+    // Configure the controller.
+    mRagdollRigidBodyCtrl->setBoneWeights(mEffectiveBoneWeights.getBufferPtr());
+    if (System::instance()->getRagdollCtrlKeyList() != nullptr) {
+        auto* config =
+            System::instance()->getRagdollCtrlKeyList()->getControllerKeyByKey(system_key);
+        if (config != nullptr) {
+            const auto get_control_data = [this]() -> decltype(auto) {
+                return mRagdollRigidBodyCtrl->m_controlDataPalette[0];
+            };
+
+            get_control_data().m_hierarchyGain = *config->hierarchy_gain;
+            get_control_data().m_velocityDamping = *config->velocity_damping;
+            get_control_data().m_accelerationGain = *config->acceleration_gain;
+            get_control_data().m_velocityGain = *config->velocity_gain;
+            get_control_data().m_positionGain = *config->position_gain;
+            get_control_data().m_positionMaxLinearVelocity = *config->position_max_linear_velocity;
+            get_control_data().m_positionMaxAngularVelocity =
+                *config->position_max_angular_velocity;
+            get_control_data().m_snapGain = *config->snap_gain;
+            get_control_data().m_snapMaxLinearVelocity = *config->snap_max_linear_velocity;
+            get_control_data().m_snapMaxAngularVelocity = *config->snap_max_angular_velocity;
+            get_control_data().m_snapMaxLinearDistance = *config->snap_max_linear_distance;
+            get_control_data().m_snapMaxAngularDistance = *config->snap_max_angular_distance;
+        }
+    }
+
+    return true;
+}
+
+bool RagdollController::setBoneWeight(int index, float weight) {
+    if (sForceDefaultWeights)
+        return false;
+
+    if (index < 0 || index >= mConfiguredBoneWeights.size())
+        return false;
+
+    if (std::isnan(weight))
+        return false;
+
+    weight = sead::Mathf::abs(weight);
+
+    if (weight > sead::Mathf::maxNumber())
+        return false;
+
+    weight = sead::Mathf::clamp(weight, 0.0, 1.0);
+
+    mConfiguredBoneWeights[index] = weight;
+    recalculateEffectiveBoneWeight(index);
+    return true;
+}
+
+bool RagdollController::setBoneWeight(const sead::SafeString& rigid_name, float weight) {
+    int index = mInstance->getBoneIndexByName(rigid_name);
+    return setBoneWeight(index, weight);
+}
+
+void RagdollController::setFactor(float factor) {
+    if (sForceDefaultWeights)
+        return;
+
+    if (std::isnan(factor))
+        return;
+
+    if (sead::Mathf::abs(factor) > sead::Mathf::maxNumber())
+        return;
+
+    factor = sead::Mathf::clamp(factor, -1.0, 1.0);
+
+    if (mFactor != factor) {
+        // Change the factor and recalculate all effective bone weights.
+        mFactor = factor;
+        for (int i = 0, n = mConfiguredBoneWeights.size(); i < n; ++i) {
+            recalculateEffectiveBoneWeight(i);
+        }
+    }
+}
+
+// NON_MATCHING
+void RagdollController::recalculateEffectiveBoneWeight(int index) {
+    const auto factor = [this] { return mFactor < 0 ? -mFactor : mFactor; };
+
+    if (mFactor == 0.0) {
+        mEffectiveBoneWeights[index] = mMultipliers[index] * mConfiguredBoneWeights[index];
+    } else if (mFactor < 0.0) {
+        mEffectiveBoneWeights[index] =
+            mMultipliers[index] *
+            (mConfiguredBoneWeights[index] + (0.0f - mConfiguredBoneWeights[index]) * factor());
+    } else {
+        mEffectiveBoneWeights[index] =
+            mMultipliers[index] *
+            (mConfiguredBoneWeights[index] + (1.0f - mConfiguredBoneWeights[index]) * factor());
+    }
+}
+
+void RagdollController::reset() {
+    reinitController();
+    mFactor = -1.0f;
+    setFactor(0.0f);
+}
+
+void RagdollController::reinitController() {
+    mRagdollRigidBodyCtrl->reinitialize();
+}
+
+void RagdollController::resetMultipliers() {
+    for (int i = 0, n = mMultipliers.size(); i < n; ++i) {
+        if (mMultipliers[i] == 1.0)
+            continue;
+
+        mMultipliers[i] = 1.0;
+        recalculateEffectiveBoneWeight(i);
+    }
+}
+
+}  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/Ragdoll/physRagdollController.h b/src/KingSystem/Physics/Ragdoll/physRagdollController.h
new file mode 100644
index 00000000..f987a5d7
--- /dev/null
+++ b/src/KingSystem/Physics/Ragdoll/physRagdollController.h
@@ -0,0 +1,51 @@
+#pragma once
+
+#include <container/seadBuffer.h>
+#include <hostio/seadHostIONode.h>
+#include <prim/seadSafeString.h>
+
+class hkaRagdollRigidBodyController;
+
+namespace ksys::phys {
+
+class RagdollInstance;
+
+// TODO
+class RagdollController : public sead::hostio::Node {
+public:
+    RagdollController();
+    virtual ~RagdollController();
+
+    const sead::SafeString& getName() const { return mName; }
+
+    // 0x00000071012ab3a0
+    bool init(const sead::SafeString& name, const sead::SafeString& system_key,
+              RagdollInstance* instance, sead::Heap* heap);
+
+    bool setBoneWeight(int index, float weight);
+    bool setBoneWeight(const sead::SafeString& rigid_name, float weight);
+    void setFactor(float factor);
+    void recalculateEffectiveBoneWeight(int index);
+
+    void reset();
+    void reinitController();
+
+    void setMultiplier(int index, float multiplier) { mMultipliers[index] = multiplier; }
+    void resetMultipliers();
+
+    static void forceDefaultWeights(bool force);
+
+private:
+    sead::FixedSafeString<32> mName;
+    hkaRagdollRigidBodyController* mRagdollRigidBodyCtrl = nullptr;
+    sead::Buffer<float> mConfiguredBoneWeights;
+    sead::Buffer<float> mEffectiveBoneWeights;
+    sead::Buffer<float> mMultipliers;
+    // If = 0, effective_weight = multiplier * configured_weight
+    // If < 0, effective_weight linearly decreases from (multiplier * configured_weight) to 0
+    // If > 0, effective_weight linearly increases from (multiplier * configured_weight) to 1
+    float mFactor = 0.0;
+    RagdollInstance* mInstance = nullptr;
+};
+
+}  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/Ragdoll/physRagdollInstance.h b/src/KingSystem/Physics/Ragdoll/physRagdollInstance.h
index 5e9423e5..06a833fc 100644
--- a/src/KingSystem/Physics/Ragdoll/physRagdollInstance.h
+++ b/src/KingSystem/Physics/Ragdoll/physRagdollInstance.h
@@ -129,6 +129,7 @@ public:
     void update();
 
     static Config& getConfig();
+    auto* getHavokRagdollInstance() const { return mRagdollInstance; }
     auto& getRigidBodies_() { return mBoneRigidBodies; }
 
 private:
diff --git a/src/KingSystem/Physics/System/physSystem.cpp b/src/KingSystem/Physics/System/physSystem.cpp
index 15d13578..0428a92a 100644
--- a/src/KingSystem/Physics/System/physSystem.cpp
+++ b/src/KingSystem/Physics/System/physSystem.cpp
@@ -67,4 +67,10 @@ void System::registerContactPointInfo(ContactPointInfo* info) const {
     mContactMgr->registerContactPointInfo(info);
 }
 
+RagdollControllerKeyList* System::getRagdollCtrlKeyList() const {
+    if (!mSystemData)
+        return nullptr;
+    return mSystemData->getRagdollCtrlKeyList();
+}
+
 }  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/System/physSystem.h b/src/KingSystem/Physics/System/physSystem.h
index a8c94b0b..07f6b501 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 RagdollControllerKeyList;
 class RagdollInstanceMgr;
 class RigidBody;
 class RigidBodyRequestMgr;
@@ -109,6 +110,8 @@ public:
     RayCastForRequest* allocRayCastRequest(SystemGroupHandler* group_handler = nullptr,
                                            GroundHit ground_hit = GroundHit::HitAll);
 
+    RagdollControllerKeyList* getRagdollCtrlKeyList() const;
+
     // TODO: rename
     // 0x0000007101216c60
     void setEntityContactListenerField90(bool value);
diff --git a/src/KingSystem/Physics/System/physSystemData.h b/src/KingSystem/Physics/System/physSystemData.h
index 4af94f81..ead0862c 100644
--- a/src/KingSystem/Physics/System/physSystemData.h
+++ b/src/KingSystem/Physics/System/physSystemData.h
@@ -64,6 +64,8 @@ public:
     void load(sead::Heap* heap, GroupFilter* entity_group_filter, GroupFilter* sensor_group_filter,
               MaterialTable* material_table, ContactMgr* contact_mgr);
 
+    auto* getRagdollCtrlKeyList() const { return mRagdollCtrlKeyList; }
+
 private:
     using LayerMatrix = Tables<LayerTable, MaxNumLayersPerType>;