ksys/phys: Finish RagdollRigidBody and add more RagdollController functions

data/uking_functions.csv

@@ -93587,13 +93587,13 @@ Address,Quality,Size,Name
 0x000000710121efd0,O,000260,_ZN4ksys4phys17RagdollController12setTransformERKN4sead8Matrix34IfEE
 0x000000710121f0d4,O,000512,_ZN4ksys4phys17RagdollController12setTransformERK14hkQsTransformf
 0x000000710121f2d4,O,000736,_ZN4ksys4phys17RagdollController8setScaleEf
-0x000000710121f5b4,U,000076,
-0x000000710121f600,U,000052,
-0x000000710121f634,U,000108,
-0x000000710121f6a0,U,000108,
-0x000000710121f70c,U,000068,
-0x000000710121f750,U,000076,
-0x000000710121f79c,U,000024,
+0x000000710121f5b4,O,000076,_ZN4ksys4phys17RagdollController26setFixedAndPreserveImpulseENS0_5FixedENS0_20MarkLinearVelAsDirtyE
+0x000000710121f600,O,000052,_ZN4ksys4phys17RagdollController16resetFrozenStateEv
+0x000000710121f634,O,000108,_ZN4ksys4phys17RagdollController22setUseSystemTimeFactorEb
+0x000000710121f6a0,O,000108,_ZN4ksys4phys17RagdollController15clearFlag400000Eb
+0x000000710121f70c,O,000068,_ZN4ksys4phys17RagdollController22setEntityMotionFlag200Eb
+0x000000710121f750,O,000076,_ZN4ksys4phys17RagdollController8setFixedENS0_5FixedENS0_18PreserveVelocitiesE
+0x000000710121f79c,O,000024,_ZNK4ksys4phys17RagdollController20getModelBoneAccessorEv
 0x000000710121f7b4,U,000740,
 0x000000710121fa98,U,001604,
 0x00000071012200dc,U,001728,
@@ -93601,10 +93601,10 @@ Address,Quality,Size,Name
 0x0000007101220b50,U,000604,
 0x0000007101220dac,U,001460,
 0x0000007101221360,O,000004,_ZN4ksys4phys17RagdollController2m3Ev
-0x0000007101221364,U,000116,
+0x0000007101221364,O,000116,_ZN4ksys4phys17RagdollController10setUserTagEPNS0_7UserTagE
 0x00000071012213d8,U,000076,
 0x0000007101221424,U,000632,
-0x000000710122169c,U,000068,
+0x000000710122169c,O,000068,_ZN4ksys4phys17RagdollController19setContactPointInfoEPNS0_16ContactPointInfoE
 0x00000071012216e0,U,000072,
 0x0000007101221728,U,000072,
 0x0000007101221770,U,000056,
@@ -93618,7 +93618,7 @@ Address,Quality,Size,Name
 0x0000007101221d44,U,000324,
 0x0000007101221e88,U,000300,
 0x0000007101221fb4,U,000380,
-0x0000007101222130,U,000020,
+0x0000007101222130,O,000020,_ZNK4ksys4phys17RagdollController15getParentOfBoneEi
 0x0000007101222144,U,000132,
 0x00000071012221c8,U,000132,
 0x000000710122224c,U,000156,
@@ -95346,13 +95346,13 @@ Address,Quality,Size,Name
 0x00000071012acd78,O,000076,_ZThn32_N4ksys4phys16RagdollRigidBodyD1Ev
 0x00000071012acdc4,O,000084,_ZN4ksys4phys16RagdollRigidBodyD0Ev
 0x00000071012ace18,O,000084,_ZThn32_N4ksys4phys16RagdollRigidBodyD0Ev
-0x00000071012ace6c,U,000376,
-0x00000071012acfe4,U,000088,
-0x00000071012ad03c,U,000076,
-0x00000071012ad088,U,000076,
-0x00000071012ad0d4,U,000036,
-0x00000071012ad0f8,U,000032,
-0x00000071012ad118,U,000228,
+0x00000071012ace6c,O,000376,_ZN4ksys4phys16RagdollRigidBody4initEPN4sead4HeapE
+0x00000071012acfe4,O,000088,_ZN4ksys4phys16RagdollRigidBody17getCollisionMasksEPNS0_23RigidBodyCollisionMasksEPKjRKN4sead7Vector3IfEE
+0x00000071012ad03c,O,000076,_ZN4ksys4phys16RagdollRigidBody18enableContactLayerENS0_12ContactLayerEb
+0x00000071012ad088,O,000076,_ZN4ksys4phys16RagdollRigidBody19disableContactLayerENS0_12ContactLayerEb
+0x00000071012ad0d4,O,000036,_ZN4ksys4phys16RagdollRigidBody13setContactAllEb
+0x00000071012ad0f8,O,000032,_ZN4ksys4phys16RagdollRigidBody14setContactNoneEb
+0x00000071012ad118,O,000228,_ZN4ksys4phys16RagdollRigidBody9getVolumeEv
 0x00000071012ad1fc,O,000204,_ZNK4ksys4phys16RagdollRigidBody27checkDerivedRuntimeTypeInfoEPKN4sead15RuntimeTypeInfo9InterfaceE
 0x00000071012ad2c8,O,000092,_ZNK4ksys4phys16RagdollRigidBody18getRuntimeTypeInfoEv
 0x00000071012ad324,U,000048,

lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h

@@ -147,6 +147,10 @@ public:
     template <int N>
     HK_FORCE_INLINE void setDot(hkVector4fParameter a, hkVector4fParameter b);
 
+    /// Get the length of this vector as if it had N components.
+    template <int N>
+    HK_FORCE_INLINE hkSimdFloat32 length() const;
+
     /// Get the squared length (|v|^2) of this vector as if it had N components.
     template <int N>
     HK_FORCE_INLINE hkSimdFloat32 lengthSquared() const;

lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.inl

@@ -481,6 +481,11 @@ inline void hkVector4f::setDot(hkVector4fParameter a, hkVector4fParameter b) {
     setAll(a.dot<N>(b));
 }
 
+template <int N>
+inline hkSimdFloat32 hkVector4f::length() const {
+    return lengthSquared<N>().sqrt();
+}
+
 template <int N>
 inline hkSimdFloat32 hkVector4f::lengthSquared() const {
     return dot<N>(*this);

lib/hkStubs/Havok/Physics2012/Collide/Shape/Convex/Capsule/hkpCapsuleShape.h

@@ -17,7 +17,7 @@ public:
     hkpCapsuleShape() = default;
     hkpCapsuleShape(const hkVector4& vertexA, const hkVector4& vertexB, hkReal radius);
     explicit hkpCapsuleShape(hkFinishLoadedObjectFlag flag);
-    virtual ~hkpCapsuleShape();
+    ~hkpCapsuleShape() override;
 
     HK_FORCE_INLINE void getSupportingVertex(hkVector4Parameter direction,
                                              hkcdVertex& supportingVertexOut) const override;
@@ -57,3 +57,16 @@ protected:
     // The line's second point.
     hkVector4 m_vertexB;
 };
+
+inline const hkVector4* hkpCapsuleShape::getVertices() const {
+    return &m_vertexA;
+}
+
+inline const hkVector4& hkpCapsuleShape::getVertex(int i) const {
+    return getVertices()[i];
+}
+
+template <int I>
+inline const hkVector4& hkpCapsuleShape::getVertex() const {
+    return getVertices()[I];
+}

src/KingSystem/Physics/Ragdoll/physRagdollController.cpp

@@ -9,6 +9,7 @@
 #include <Havok/Physics2012/Utilities/Dynamics/ScaleSystem/hkpSystemScalingUtility.h>
 #include <math/seadMathCalcCommon.h>
 #include "KingSystem/Physics/Ragdoll/physRagdollControllerMgr.h"
+#include "KingSystem/Physics/Ragdoll/physRagdollRigidBody.h"
 #include "KingSystem/Physics/Rig/physModelBoneAccessor.h"
 #include "KingSystem/Physics/Rig/physSkeletonMapper.h"
 #include "KingSystem/Physics/RigidBody/physRigidBody.h"
@@ -195,8 +196,65 @@ void RagdollController::setScale(float scale) {
         mSkeletonMapper->getBoneAccessor().setScale(scale);
 }
 
+void RagdollController::setFixedAndPreserveImpulse(Fixed fixed,
+                                                   MarkLinearVelAsDirty mark_linear_vel_as_dirty) {
+    for (auto* body : mRigidBodies)
+        body->setFixedAndPreserveImpulse(fixed, mark_linear_vel_as_dirty);
+}
+
+void RagdollController::resetFrozenState() {
+    for (auto* body : mRigidBodies)
+        body->resetFrozenState();
+}
+
+void RagdollController::setUseSystemTimeFactor(bool use) {
+    for (auto* body : mRigidBodies)
+        body->setUseSystemTimeFactor(use);
+}
+
+void RagdollController::clearFlag400000(bool clear) {
+    for (auto* body : mRigidBodies)
+        body->clearFlag400000(clear);
+}
+
+void RagdollController::setEntityMotionFlag200(bool set) {
+    for (auto* body : mRigidBodies)
+        body->setEntityMotionFlag200(set);
+}
+
+void RagdollController::setFixed(Fixed fixed, PreserveVelocities preserve_velocities) {
+    for (auto* body : mRigidBodies)
+        body->setFixed(fixed, preserve_velocities);
+}
+
+BoneAccessor* RagdollController::getModelBoneAccessor() const {
+    if (mSkeletonMapper)
+        return &mSkeletonMapper->getModelBoneAccessor();
+
+    return mModelBoneAccessor;
+}
+
 void RagdollController::m3() {}
 
+void RagdollController::setUserTag(UserTag* tag) {
+    for (auto* body : mRigidBodies)
+        body->setUserTag(tag);
+}
+
+void RagdollController::setSystemGroupHandler(SystemGroupHandler* handler) {
+    for (auto* body : mRigidBodies)
+        body->setSystemGroupHandler(handler);
+}
+
+void RagdollController::setContactPointInfo(ContactPointInfo* info) {
+    for (auto* body : mRigidBodies)
+        body->setContactPointInfo(info);
+}
+
+int RagdollController::getParentOfBone(int index) const {
+    return mRagdollInstance->getParentOfBone(index);
+}
+
 RagdollController::ScopedPhysicsLock::ScopedPhysicsLock(const RagdollController* ctrl)
     : mCtrl{ctrl}, mWorldLock{ctrl->isAddedToWorld(), ContactLayerType::Entity} {
     for (auto body : util::indexIter(ctrl->mRigidBodies))

src/KingSystem/Physics/Ragdoll/physRagdollController.h

@@ -27,9 +27,15 @@ namespace ksys::phys {
 class BoneAccessor;
 class ModelBoneAccessor;
 class RagdollParam;
+class RagdollRigidBody;
 class RigidBody;
 class SkeletonMapper;
 class SystemGroupHandler;
+class UserTag;
+
+enum class Fixed : bool;
+enum class MarkLinearVelAsDirty : bool;
+enum class PreserveVelocities : bool;
 
 // TODO
 class RagdollController : public sead::hostio::Node {
@@ -52,6 +58,14 @@ public:
 
     void setTransform(const sead::Matrix34f& transform);
     void setScale(float scale);
+    void setFixedAndPreserveImpulse(Fixed fixed, MarkLinearVelAsDirty mark_linear_vel_as_dirty);
+    void resetFrozenState();
+    void setUseSystemTimeFactor(bool use);
+    void clearFlag400000(bool clear);
+    void setEntityMotionFlag200(bool set);
+    void setFixed(Fixed fixed, PreserveVelocities preserve_velocities);
+
+    BoneAccessor* getModelBoneAccessor() const;
 
     u32 sub_7101221CC4();
     void sub_7101221728(ContactLayer layer);
@@ -60,8 +74,28 @@ public:
     // TODO: rename
     virtual void m3();
 
+    void setUserTag(UserTag* tag);
+    void setSystemGroupHandler(SystemGroupHandler* handler);
+    // 0x0000007101221424
+    void x_22(int index, float value);
+    void setContactPointInfo(ContactPointInfo* info);
+    // 0x00000071012216e0
+    void x_24();
+    // 0x0000007101221728
+    void x_25();
+    // 0x0000007101221770
+    void x_26();
+    // 0x00000071012217a8
+    void x_27();
+    // 0x00000071012217e0
+    void x_28();
+
+    int getParentOfBone(int index) const;
+
     static void setUnk1(u8 value);
 
+    auto& getRigidBodies_() { return mRigidBodies; }
+
 private:
     class ScopedPhysicsLock {
     public:
@@ -94,7 +128,7 @@ private:
     ModelBoneAccessor* mModelBoneAccessor = nullptr;
     hkaRagdollInstance* mRagdollInstance = nullptr;
     SystemGroupHandler* mGroupHandler = nullptr;
-    sead::Buffer<RigidBody*> mRigidBodies;
+    sead::Buffer<RagdollRigidBody*> mRigidBodies;
     // TODO: rename
     sead::Buffer<BoneVectors> mBoneVectors;
     // TODO: rename

src/KingSystem/Physics/Ragdoll/physRagdollRigidBody.cpp

@@ -1,19 +1,96 @@
 #include "KingSystem/Physics/Ragdoll/physRagdollRigidBody.h"
+#include "Havok/Physics2012/Collide/Shape/Convex/Capsule/hkpCapsuleShape.h"
+#include "Havok/Physics2012/Dynamics/Entity/hkpRigidBody.h"
+#include "KingSystem/Physics/Ragdoll/physRagdollController.h"
+#include "KingSystem/Physics/physMaterialMask.h"
 
 namespace ksys::phys {
 
 RagdollRigidBody::RagdollRigidBody(const sead::SafeString& name, RagdollController* ctrl,
-                                   int ragdoll_body_index, hkpRigidBody* hkp_rigid_body,
-                                   sead::Heap* heap)
+                                   int bone_index, hkpRigidBody* hkp_rigid_body, sead::Heap* heap)
     : RigidBody(RigidBody::Type::Ragdoll, ContactLayerType::Entity, hkp_rigid_body, name, heap,
                 true),
-      mCtrl(ctrl), mRagdollBodyIndex(ragdoll_body_index) {
+      mCtrl(ctrl), mBoneIndex(bone_index) {
     updateCollidableQualityType(true);
     mFlags.set(Flag::NoCharStandingOn);
 }
 
 RagdollRigidBody::~RagdollRigidBody() {
-    _e8.freeBuffer();
+    mChildBodies.freeBuffer();
+}
+
+void RagdollRigidBody::init(sead::Heap* heap) {
+    const int parent_index = mCtrl->getParentOfBone(mBoneIndex);
+    if (parent_index >= 0)
+        mParentBody = mCtrl->getRigidBodies_()[parent_index];
+
+    int num_children = 0;
+    for (int i = 0, n = mCtrl->getRigidBodies_().size(); i < n; ++i) {
+        if (mCtrl->getParentOfBone(i) == mBoneIndex)
+            ++num_children;
+    }
+
+    if (num_children > 0) {
+        mChildBodies.allocBufferAssert(num_children, heap);
+        int child_index = 0;
+        for (int i = 0, n = mCtrl->getRigidBodies_().size(); i < n; ++i) {
+            if (mCtrl->getParentOfBone(i) == mBoneIndex) {
+                mChildBodies[child_index] = mCtrl->getRigidBodies_()[i];
+                ++child_index;
+            }
+        }
+    }
+}
+
+u32 RagdollRigidBody::getCollisionMasks(RigidBody::CollisionMasks* masks, const u32* shape_key,
+                                        const sead::Vector3f& contact_point) {
+    masks->ignored_layers = ~mContactMask;
+    masks->collision_filter_info = getCollisionFilterInfo();
+    MaterialMaskData data;
+    data.material = Material::Ragdoll;
+    masks->material_mask = MaterialMask(data.raw).getRawData();
+    return 0;
+}
+
+void RagdollRigidBody::enableContactLayer(ContactLayer layer, bool alt_mask) {
+    (!alt_mask ? mContactMask1 : mContactMask2) |= 1 << getLayerBit(layer);
+    updateContactMask();
+}
+
+void RagdollRigidBody::disableContactLayer(ContactLayer layer, bool alt_mask) {
+    (!alt_mask ? mContactMask1 : mContactMask2) &= ~(1 << getLayerBit(layer));
+    updateContactMask();
+}
+
+void RagdollRigidBody::setContactAll(bool alt_mask) {
+    (!alt_mask ? mContactMask1 : mContactMask2) = ~0;
+    updateContactMask();
+}
+
+void RagdollRigidBody::setContactNone(bool alt_mask) {
+    (!alt_mask ? mContactMask1 : mContactMask2) = 0;
+    updateContactMask();
+}
+
+void RagdollRigidBody::updateContactMask() {
+    setContactMask(mContactMask2 | mContactMask1);
+}
+
+float RagdollRigidBody::getVolume() {
+    auto lock = makeScopedLock();
+
+    const hkpShape* shape = mHkBody->getCollidable()->getShape();
+    if (shape->getType() != hkcdShapeType::CAPSULE)
+        return 0.0f;
+
+    auto* capsule = static_cast<const hkpCapsuleShape*>(shape);
+
+    hkVector4f diff;
+    diff.setSub(capsule->getVertex<0>(), capsule->getVertex<1>());
+    const float radius = capsule->getRadius();
+    const float side = diff.length<3>();
+    return sead::Mathf::pi() * radius * radius * 4.0f * radius / 3.0f +
+           sead::Mathf::pi() * radius * radius * side;
 }
 
 }  // namespace ksys::phys

src/KingSystem/Physics/Ragdoll/physRagdollRigidBody.h

@@ -7,24 +7,33 @@ namespace ksys::phys {
 
 class RagdollController;
 
-// TODO
 class RagdollRigidBody : public RigidBody {
     SEAD_RTTI_OVERRIDE(RagdollRigidBody, RigidBody)
 public:
-    RagdollRigidBody(const sead::SafeString& name, RagdollController* ctrl, int ragdoll_body_index,
+    RagdollRigidBody(const sead::SafeString& name, RagdollController* ctrl, int bone_index,
                      hkpRigidBody* hkp_rigid_body, sead::Heap* heap);
     ~RagdollRigidBody() override;
 
+    void init(sead::Heap* heap);
+
     float getVolume() override;
     u32 getCollisionMasks(RigidBody::CollisionMasks* masks, const u32* shape_key,
                           const sead::Vector3f& contact_point) override;
 
+    void enableContactLayer(ContactLayer layer, bool alt_mask);
+    void disableContactLayer(ContactLayer layer, bool alt_mask);
+    void setContactAll(bool alt_mask);
+    void setContactNone(bool alt_mask);
+
 private:
+    void updateContactMask();
+
     RagdollController* mCtrl{};
-    int mRagdollBodyIndex{};
-    void* _e0{};
-    sead::Buffer<void*> _e8{};
-    void* _f8{};
+    int mBoneIndex{};
+    RagdollRigidBody* mParentBody{};
+    sead::Buffer<RagdollRigidBody*> mChildBodies{};
+    u32 mContactMask1{};
+    u32 mContactMask2{};
 };
 
 }  // namespace ksys::phys

src/KingSystem/Physics/Rig/physSkeletonMapper.h

@@ -20,6 +20,7 @@ public:
     void mapPoseB();
 
     BoneAccessor& getBoneAccessor() { return mBoneAccessor; }
+    ModelBoneAccessor& getModelBoneAccessor() { return mModelBoneAccessor; }
 
 private:
     BoneAccessor mBoneAccessor;

src/KingSystem/Physics/RigidBody/physRigidBody.cpp

@@ -565,20 +565,14 @@ void RigidBody::setCollidableQualityType(hkpCollidableQualityType quality) {
     getHkBody()->getCollidableRw()->setQualityType(quality);
 }
 
-static int getLayerBit(int layer, ContactLayerType type) {
-    // This is layer for Entity layers and layer - 0x20 for Sensor layers.
-    // XXX: this should be using makeContactLayerMask.
-    return layer - FirstSensor * int(type);
-}
-
 void RigidBody::enableContactLayer(ContactLayer layer) {
     assertLayerType(layer);
-    mContactMask.setBit(getLayerBit(layer, getLayerType()));
+    mContactMask.setBit(getLayerBit(layer));
 }
 
 void RigidBody::disableContactLayer(ContactLayer layer) {
     assertLayerType(layer);
-    mContactMask.resetBit(getLayerBit(layer, getLayerType()));
+    mContactMask.resetBit(getLayerBit(layer));
 }
 
 void RigidBody::setContactMask(u32 value) {

src/KingSystem/Physics/RigidBody/physRigidBody.h

@@ -583,7 +583,7 @@ public:
     // Internal.
     void setUseSystemTimeFactor(bool use) { mFlags.change(Flag::UseSystemTimeFactor, use); }
     // Internal.
-    void setFlag400000(bool set) { mFlags.change(Flag::_400000, set); }
+    void clearFlag400000(bool clear) { mFlags.change(Flag::_400000, !clear); }
     // Internal.
     void setUpdateRequestedFlag() { mFlags.set(Flag::UpdateRequested); }
     // Internal.
@@ -609,6 +609,14 @@ protected:
     void updateDeactivation();
     void setCollidableQualityType(hkpCollidableQualityType quality);
 
+    static int getLayerBit(int layer, ContactLayerType type) {
+        // This is layer for Entity layers and layer - 0x20 for Sensor layers.
+        // XXX: this should be using makeContactLayerMask.
+        return layer - FirstSensor * int(type);
+    }
+
+    int getLayerBit(int layer) const { return getLayerBit(layer, getLayerType()); }
+
     sead::CriticalSection mCS;
     sead::TypedBitFlag<Flag, sead::Atomic<u32>> mFlags{};
     sead::TypedBitFlag<MotionFlag, sead::Atomic<u32>> mMotionFlags{};

src/KingSystem/Physics/RigidBody/physRigidBodySet.cpp

@@ -29,7 +29,7 @@ void RigidBodySet::setUseSystemTimeFactor(bool use) {
 
 void RigidBodySet::clearFlag400000(bool clear) {
     for (auto& body : mRigidBodies)
-        body.setFlag400000(!clear);
+        body.clearFlag400000(clear);
 }
 
 void RigidBodySet::setEntityMotionFlag200(bool set) {