ksys/phys: Finish RagdollRigidBody and add more RagdollController functions

8 files changed, 202 insertions, 21 deletions

diff --git a/src/KingSystem/Physics/Ragdoll/physRagdollController.cpp b/src/KingSystem/Physics/Ragdoll/physRagdollController.cpp
index a76eb8c3..d342744b 100644
--- a/src/KingSystem/Physics/Ragdoll/physRagdollController.cpp
+++ b/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))
diff --git a/src/KingSystem/Physics/Ragdoll/physRagdollController.h b/src/KingSystem/Physics/Ragdoll/physRagdollController.h
index 9823cc83..8091962a 100644
--- a/src/KingSystem/Physics/Ragdoll/physRagdollController.h
+++ b/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
diff --git a/src/KingSystem/Physics/Ragdoll/physRagdollRigidBody.cpp b/src/KingSystem/Physics/Ragdoll/physRagdollRigidBody.cpp
index 8d9d0bbb..bcc942aa 100644
--- a/src/KingSystem/Physics/Ragdoll/physRagdollRigidBody.cpp
+++ b/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
diff --git a/src/KingSystem/Physics/Ragdoll/physRagdollRigidBody.h b/src/KingSystem/Physics/Ragdoll/physRagdollRigidBody.h
index b0a20940..d53be897 100644
--- a/src/KingSystem/Physics/Ragdoll/physRagdollRigidBody.h
+++ b/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
diff --git a/src/KingSystem/Physics/Rig/physSkeletonMapper.h b/src/KingSystem/Physics/Rig/physSkeletonMapper.h
index 8972cce7..9295cb44 100644
--- a/src/KingSystem/Physics/Rig/physSkeletonMapper.h
+++ b/src/KingSystem/Physics/Rig/physSkeletonMapper.h
@@ -20,6 +20,7 @@ public:
     void mapPoseB();
 
     BoneAccessor& getBoneAccessor() { return mBoneAccessor; }
+    ModelBoneAccessor& getModelBoneAccessor() { return mModelBoneAccessor; }
 
 private:
     BoneAccessor mBoneAccessor;
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBody.cpp b/src/KingSystem/Physics/RigidBody/physRigidBody.cpp
index c114db5b..7f143cc8 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBody.cpp
+++ b/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) {
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBody.h b/src/KingSystem/Physics/RigidBody/physRigidBody.h
index eec185ff..6ba83767 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBody.h
+++ b/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{};
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodySet.cpp b/src/KingSystem/Physics/RigidBody/physRigidBodySet.cpp
index 36cebf94..1d5bf457 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBodySet.cpp
+++ b/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) {