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

data/uking_functions.csv

@@ -93475,7 +93475,7 @@ Address,Quality,Size,Name
 0x0000007101216c58,U,000008,PhysicsMemSys::setRigidBodyDividedMeshShapeMgr
 0x0000007101216c60,U,000020,phys::System::setEntityContactListenerField90
 0x0000007101216c74,U,000016,phys::System::getEntityContactListenerField90
-0x0000007101216c84,U,000032,phys::System::getRagdollCtrlKeyList
+0x0000007101216c84,O,000032,_ZNK4ksys4phys6System21getRagdollCtrlKeyListEv
 0x0000007101216ca4,U,000072,PhysicsMemSys::isActorSystemIdle
 0x0000007101216cec,U,000148,PhysicsMemSys::getPhysicsTempHeap
 0x0000007101216d80,U,000112,
@@ -95307,17 +95307,17 @@ Address,Quality,Size,Name
 0x00000071012ab088,U,000008,
 0x00000071012ab090,U,000372,
 0x00000071012ab204,U,000052,
-0x00000071012ab238,U,000160,
+0x00000071012ab238,O,000160,_ZN4ksys4phys17RagdollControllerC1Ev
-0x00000071012ab2d8,U,000164,
+0x00000071012ab2d8,O,000164,_ZN4ksys4phys17RagdollControllerD1Ev
-0x00000071012ab37c,U,000036,
+0x00000071012ab37c,O,000036,_ZN4ksys4phys17RagdollControllerD0Ev
-0x00000071012ab3a0,U,000832,
+0x00000071012ab3a0,O,000832,_ZN4ksys4phys17RagdollController4initERKN4sead14SafeStringBaseIcEES6_PNS0_15RagdollInstanceEPNS2_4HeapE
-0x00000071012ab6e0,U,000364,
+0x00000071012ab6e0,m,000364,_ZN4ksys4phys17RagdollController13setBoneWeightEif
-0x00000071012ab84c,U,000192,
+0x00000071012ab84c,m,000192,_ZN4ksys4phys17RagdollController30recalculateEffectiveBoneWeightEi
-0x00000071012ab90c,U,000060,
+0x00000071012ab90c,O,000060,_ZN4ksys4phys17RagdollController13setBoneWeightERKN4sead14SafeStringBaseIcEEf
-0x00000071012ab948,U,000052,
+0x00000071012ab948,O,000052,_ZN4ksys4phys17RagdollController5resetEv
-0x00000071012ab97c,U,000344,
+0x00000071012ab97c,m,000344,_ZN4ksys4phys17RagdollController9setFactorEf
-0x00000071012abad4,U,000008,
+0x00000071012abad4,O,000008,_ZN4ksys4phys17RagdollController16reinitControllerEv
-0x00000071012abadc,U,000284,
+0x00000071012abadc,m,000284,_ZN4ksys4phys17RagdollController16resetMultipliersEv
 0x00000071012abbf8,O,000928,_ZN4ksys4phys24RagdollControllerKeyList20RagdollControllerKeyC1Ev
 0x00000071012abf98,O,000064,_ZN4ksys4phys24RagdollControllerKeyListC1Ev
 0x00000071012abfd8,O,000212,_ZN4ksys4phys24RagdollControllerKeyListD1Ev

lib/hkStubs/CMakeLists.txt

@@ -12,6 +12,8 @@ add_library(hkStubs OBJECT
   Havok/Animation/Animation/Rig/hkaBoneAttachment.h
   Havok/Animation/Animation/Rig/hkaSkeleton.h
   Havok/Animation/Animation/Rig/hkaPose.h
+  Havok/Animation/Physics2012Bridge/Controller/RigidBody/hkaKeyFrameHierarchyUtility.h
+  Havok/Animation/Physics2012Bridge/Controller/RigidBody/hkaRagdollRigidBodyController.h
   Havok/Animation/Physics2012Bridge/Instance/hkaRagdollInstance.h
 
   Havok/Common/Base/hkBase.h

lib/hkStubs/Havok/Animation/Physics2012Bridge/Controller/RigidBody/hkaKeyFrameHierarchyUtility.h

@@ -0,0 +1,82 @@
+#pragma once
+
+#include <Havok/Common/Base/hkBase.h>
+
+class hkpRigidBody;
+
+class hkaKeyFrameHierarchyUtility {
+public:
+    HK_DECLARE_CLASS_ALLOCATOR(hkaKeyFrameHierarchyUtility)
+    HK_DECLARE_REFLECTION()
+
+    struct ControlData {
+        HK_DECLARE_CLASS_ALLOCATOR(ControlData)
+        HK_DECLARE_REFLECTION()
+
+        hkReal m_hierarchyGain;
+        hkReal m_velocityDamping;
+        hkReal m_accelerationGain;
+        hkReal m_velocityGain;
+        hkReal m_positionGain;
+        hkReal m_positionMaxLinearVelocity;
+        hkReal m_positionMaxAngularVelocity;
+        hkReal m_snapGain;
+        hkReal m_snapMaxLinearVelocity;
+        hkReal m_snapMaxAngularVelocity;
+        hkReal m_snapMaxLinearDistance;
+        hkReal m_snapMaxAngularDistance;
+
+        ControlData()
+            : m_hierarchyGain(hkReal(0.17f)), m_velocityDamping(0), m_accelerationGain(1),
+              m_velocityGain(hkReal(0.6f)), m_positionGain(hkReal(0.05f)),
+              m_positionMaxLinearVelocity(hkReal(1.4f)), m_positionMaxAngularVelocity(hkReal(1.8f)),
+              m_snapGain(hkReal(0.1f)), m_snapMaxLinearVelocity(hkReal(0.3f)),
+              m_snapMaxAngularVelocity(hkReal(0.3f)), m_snapMaxLinearDistance(hkReal(0.03f)),
+              m_snapMaxAngularDistance(hkReal(0.1f)) {}
+
+        explicit ControlData(hkFinishLoadedObjectFlag flag) {}
+
+        hkBool isValid() const;
+    };
+
+    struct WorkElem {
+        HK_DECLARE_CLASS_ALLOCATOR(WorkElem)
+
+        hkVector4 m_prevPosition;
+        hkQuaternion m_prevRotation;
+        hkVector4 m_prevLinearVelocity;
+        hkVector4 m_prevAngularVelocity;
+    };
+
+    struct KeyFrameData {
+        HK_DECLARE_CLASS_ALLOCATOR(KeyFrameData)
+
+        hkQsTransform m_worldFromRoot;
+        /// The desired pose in local space, one transform for each rigid body
+        const hkQsTransform* m_desiredPoseLocalSpace;
+        WorkElem* m_internalReferencePose;
+    };
+
+    struct BodyData {
+        HK_DECLARE_CLASS_ALLOCATOR(BodyData)
+
+        int m_numRigidBodies;
+        hkpRigidBody* const* m_rigidBodies;
+        const hkInt16* m_parentIndices;
+        int* m_controlDataIndices = nullptr;
+        const hkReal* m_boneWeights = nullptr;
+    };
+
+    struct Output {
+        HK_DECLARE_CLASS_ALLOCATOR(Output)
+
+        hkReal m_stressSquared;
+    };
+
+    static void initialize(const hkpRigidBody* rb, WorkElem& workElem);
+
+    static void initialize(const BodyData& bodies, WorkElem* internalReferencePose);
+
+    static void applyKeyFrame(hkReal m_deltaTime, const KeyFrameData& pose, const BodyData& body,
+                              const ControlData* controlPalette, Output* outputArray = nullptr);
+};

lib/hkStubs/Havok/Animation/Physics2012Bridge/Controller/RigidBody/hkaRagdollRigidBodyController.h

@@ -0,0 +1,34 @@
+#pragma once
+
+#include <Havok/Animation/Physics2012Bridge/Controller/RigidBody/hkaKeyFrameHierarchyUtility.h>
+#include <Havok/Common/Base/hkBase.h>
+
+class hkaRagdollInstance;
+
+class hkaRagdollRigidBodyController {
+public:
+    HK_DECLARE_CLASS_ALLOCATOR(hkaRagdollRigidBodyController)
+
+    explicit hkaRagdollRigidBodyController(hkaRagdollInstance* ragdoll);
+    ~hkaRagdollRigidBodyController();
+
+    void driveToPose(hkReal deltaTime, const hkQsTransform* poseLocalSpace,
+                     const hkQsTransform& worldFromModel,
+                     hkaKeyFrameHierarchyUtility::Output* stressOut);
+
+    void reinitialize();
+    void reinitialize(int boneIndex);
+
+    const hkaRagdollInstance* getRagdollInstance() const;
+
+    void setBoneWeights(const hkReal* boneWeights);
+
+    hkArray<hkaKeyFrameHierarchyUtility::ControlData> m_controlDataPalette;
+    hkArray<int> m_bodyIndexToPaletteIndex;
+
+protected:
+    hkaKeyFrameHierarchyUtility::BodyData m_bodyData;
+    hkArray<hkaKeyFrameHierarchyUtility::WorkElem> m_internalData;
+    hkaRagdollInstance* m_ragdollInstance;
+    hkArray<hkInt16> m_rbParentIndices;
+};

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

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

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

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:

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

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);

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>;