ksys/phys: Add RigidBodySet

7 files changed, 283 insertions, 33 deletions

diff --git a/src/KingSystem/Physics/CMakeLists.txt b/src/KingSystem/Physics/CMakeLists.txt
index 7b7f5a78..d334a10b 100644
--- a/src/KingSystem/Physics/CMakeLists.txt
+++ b/src/KingSystem/Physics/CMakeLists.txt
@@ -33,6 +33,8 @@ target_sources(uking PRIVATE
   RigidBody/physRigidBodyRequestMgr.h
   RigidBody/physRigidBodyResource.cpp
   RigidBody/physRigidBodyResource.h
+  RigidBody/physRigidBodySet.cpp
+  RigidBody/physRigidBodySet.h
   RigidBody/physRigidBodySetParam.cpp
   RigidBody/physRigidBodySetParam.h
   RigidBody/Shape/physBoxShape.cpp
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBody.cpp b/src/KingSystem/Physics/RigidBody/physRigidBody.cpp
index de1b1af3..58eaac47 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBody.cpp
+++ b/src/KingSystem/Physics/RigidBody/physRigidBody.cpp
@@ -247,7 +247,7 @@ bool RigidBody::isMotionFlag2Set() const {
     return mMotionFlags.isOn(MotionFlag::_2);
 }
 
-void RigidBody::sub_7100F8D21C() {
+void RigidBody::addOrRemoveRigidBodyToWorld() {
     // debug code that survived because mFlags is atomic?
     static_cast<void>(mFlags.getDirect());
 
@@ -571,12 +571,12 @@ static int getLayerBit(int layer, ContactLayerType type) {
     return layer - FirstSensor * int(type);
 }
 
-void RigidBody::addContactLayer(ContactLayer layer) {
+void RigidBody::enableContactLayer(ContactLayer layer) {
     assertLayerType(layer);
     mContactMask.setBit(getLayerBit(layer, getLayerType()));
 }
 
-void RigidBody::removeContactLayer(ContactLayer layer) {
+void RigidBody::disableContactLayer(ContactLayer layer) {
     assertLayerType(layer);
     mContactMask.resetBit(getLayerBit(layer, getLayerType()));
 }
@@ -926,17 +926,17 @@ void RigidBody::updateShape() {
         mUserTag->onBodyShapeChanged(this);
 }
 
-void RigidBody::updateShapeIfNeeded(float x) {
+void RigidBody::setScale(float scale) {
     if (!hasFlag(Flag::_10))
         return;
 
-    if (x <= 0.0)
-        x = 1.0;
+    if (scale <= 0.0)
+        scale = 1.0;
 
-    if (sead::Mathf::equalsEpsilon(_b0, x))
+    if (sead::Mathf::equalsEpsilon(mScale, scale))
         return;
 
-    _b0 = m12(x, _b0);
+    mScale = m12(scale, mScale);
     updateShape();
 }
 
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBody.h b/src/KingSystem/Physics/RigidBody/physRigidBody.h
index 5cf7848b..14894e9a 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBody.h
+++ b/src/KingSystem/Physics/RigidBody/physRigidBody.h
@@ -147,7 +147,7 @@ public:
     bool isFlag8Set() const;
     bool isMotionFlag1Set() const;
     bool isMotionFlag2Set() const;
-    void sub_7100F8D21C();
+    void addOrRemoveRigidBodyToWorld();
     bool x_6();
 
     /// Get the motion accessor if it is a RigidBodyMotionEntity. Returns nullptr otherwise.
@@ -191,8 +191,8 @@ public:
 
     void updateCollidableQualityType(bool high_quality);
 
-    void addContactLayer(ContactLayer layer);
-    void removeContactLayer(ContactLayer layer);
+    void enableContactLayer(ContactLayer layer);
+    void disableContactLayer(ContactLayer layer);
     void setContactMask(u32);
     void setContactAll();
     void setContactNone();
@@ -278,7 +278,7 @@ public:
     bool isTransformDirty() const;
 
     void updateShape();
-    void updateShapeIfNeeded(float x);
+    void setScale(float scale);
 
     void changeMotionType(MotionType motion_type);
     // 0x0000007100f9045c - calls a bunch of Havok world functions
@@ -459,10 +459,13 @@ public:
     void setMotionFlag(MotionFlag flag);
 
     hkpRigidBody* getHkBody() const { return mHkBody; }
-    UserTag* getUserTag() const { return mUserTag; }
+
     Type getType() const { return mType; }
     bool isCharacterControllerType() const { return mType == Type::CharacterController; }
 
+    UserTag* getUserTag() const { return mUserTag; }
+    void setUserTag(UserTag* tag) { mUserTag = tag; }
+
     bool hasConstraintWithUserData();
     // 0x0000007100f94e80
     bool x_103(int a);
@@ -522,6 +525,13 @@ public:
     virtual const char* getName();
 
     // Internal.
+    void setUseSystemTimeFactor(bool use) { mFlags.change(Flag::UseSystemTimeFactor, use); }
+    // Internal.
+    void setFlag400000(bool set) { mFlags.change(Flag::_400000, set); }
+    // Internal.
+    void setUpdateRequestedFlag() { mFlags.set(Flag::UpdateRequested); }
+
+    // Internal.
     void onCollisionAdded() {
         if (mCollisionCount.increment() == 0)
             clearFlag4000000(false);
@@ -551,7 +561,7 @@ private:
     void* _90 = nullptr;
     u16 _98 = 0;
     RigidBodyAccessor mRigidBodyAccessor;
-    f32 _b0 = 1.0f;
+    f32 mScale = 1.0f;
     Type mType{};
     MotionAccessor* mMotionAccessor = nullptr;
     sead::Atomic<int> mCollisionCount;
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodySet.cpp b/src/KingSystem/Physics/RigidBody/physRigidBodySet.cpp
new file mode 100644
index 00000000..6e96750a
--- /dev/null
+++ b/src/KingSystem/Physics/RigidBody/physRigidBodySet.cpp
@@ -0,0 +1,179 @@
+#include "KingSystem/Physics/RigidBody/physRigidBodySet.h"
+#include "KingSystem/Physics/RigidBody/physRigidBody.h"
+#include "KingSystem/Physics/System/physGroupFilter.h"
+#include "KingSystem/Utils/Debug.h"
+
+namespace ksys::phys {
+
+RigidBodySet::RigidBodySet(const sead::SafeString& name) : mName(name) {}
+
+RigidBodySet::~RigidBodySet() {
+    util::PrintDebug("~RigidBodySet");
+}
+
+void RigidBodySet::setFixedAndPreserveImpulse(bool fixed, bool mark_linear_vel_as_dirty) {
+    for (auto& body : mRigidBodies)
+        body.setFixedAndPreserveImpulse(fixed, mark_linear_vel_as_dirty);
+}
+
+void RigidBodySet::resetFrozenState() {
+    for (auto& body : mRigidBodies)
+        body.resetFrozenState();
+}
+
+void RigidBodySet::setUseSystemTimeFactor(bool use) {
+    for (auto& body : mRigidBodies)
+        body.setUseSystemTimeFactor(use);
+}
+
+void RigidBodySet::clearFlag400000(bool clear) {
+    for (auto& body : mRigidBodies)
+        body.setFlag400000(!clear);
+}
+
+void RigidBodySet::setEntityMotionFlag200(bool set) {
+    for (auto& body : mRigidBodies)
+        body.setEntityMotionFlag200(set);
+}
+
+void RigidBodySet::setFixed(bool fixed, bool preserve_velocities) {
+    for (auto& body : mRigidBodies)
+        body.setFixed(fixed, preserve_velocities);
+}
+
+void RigidBodySet::updateMotionTypeRelatedFlags() {
+    for (auto& body : mRigidBodies)
+        body.updateMotionTypeRelatedFlags();
+}
+
+void RigidBodySet::triggerScheduledMotionTypeChange() {
+    for (auto& body : mRigidBodies)
+        body.triggerScheduledMotionTypeChange();
+}
+
+bool RigidBodySet::hasActiveEntityBody() const {
+    for (const auto& body : mRigidBodies) {
+        if (body.isEntity() && body.isActive())
+            return true;
+    }
+    return false;
+}
+
+RigidBody* RigidBodySet::findBodyByHavokName(const sead::SafeString& name) {
+    const int index = findBodyIndexByHavokName(name);
+    if (index < 0)
+        return nullptr;
+    return mRigidBodies[index];
+}
+
+const RigidBody* RigidBodySet::findBodyByHavokName(const sead::SafeString& name) const {
+    const int index = findBodyIndexByHavokName(name);
+    if (index < 0)
+        return nullptr;
+    return mRigidBodies[index];
+}
+
+int RigidBodySet::findBodyIndexByHavokName(const sead::SafeString& name) const {
+    int idx = 0;
+    for (const auto& body : mRigidBodies) {
+        if (name == body.getHkBodyName())
+            return idx;
+        ++idx;
+    }
+    return -1;
+}
+
+void RigidBodySet::setUserTag(UserTag* tag) {
+    for (auto& body : mRigidBodies)
+        body.setUserTag(tag);
+}
+
+void RigidBodySet::setSystemGroupHandler(SystemGroupHandler* handler) {
+    for (auto& body : mRigidBodies) {
+        if (handler == nullptr || handler->getLayerType() == body.getLayerType())
+            body.setSystemGroupHandler(handler);
+    }
+}
+
+void RigidBodySet::setSystemGroupHandler(SystemGroupHandler* handler, ContactLayerType layer_type) {
+    if (handler != nullptr && handler->getLayerType() != layer_type)
+        return;
+
+    for (auto& body : mRigidBodies) {
+        if (body.getLayerType() == layer_type)
+            body.setSystemGroupHandler(handler);
+    }
+}
+
+void RigidBodySet::setTransform(const sead::Matrix34f& mtx) {
+    for (auto& body : mRigidBodies)
+        body.setTransform(mtx, true);
+}
+
+void RigidBodySet::enableContactLayer(ContactLayer layer) {
+    const auto type = getContactLayerType(layer);
+    for (auto& body : mRigidBodies) {
+        if (body.getLayerType() == type)
+            body.enableContactLayer(layer);
+    }
+}
+
+void RigidBodySet::disableContactLayer(ContactLayer layer) {
+    const auto type = getContactLayerType(layer);
+    for (auto& body : mRigidBodies) {
+        if (body.getLayerType() == type)
+            body.disableContactLayer(layer);
+    }
+}
+
+void RigidBodySet::disableAllContactLayers() {
+    for (auto& body : mRigidBodies)
+        body.setContactNone();
+}
+
+void RigidBodySet::setScaleAndUpdateImmediately(float scale) {
+    for (auto it = mRigidBodies.begin(), end = mRigidBodies.end(); it != end; ++it) {
+        it->setUpdateRequestedFlag();
+        it->setScale(scale);
+        it->processUpdateRequests(nullptr, nullptr);
+    }
+}
+
+void RigidBodySet::setScale(float scale) {
+    for (auto& body : mRigidBodies)
+        body.setScale(scale);
+}
+
+void RigidBodySet::callRigidBody_x_0() {
+    for (auto& body : mRigidBodies)
+        body.x_0();
+}
+
+void RigidBodySet::addOrRemoveRigidBodiesToWorld() {
+    for (auto& body : mRigidBodies)
+        body.addOrRemoveRigidBodyToWorld();
+}
+
+bool RigidBodySet::areAllTrueRigidBody_x_6() {
+    bool ok = true;
+    for (auto& body : mRigidBodies)
+        ok &= body.x_6();
+    return ok;
+}
+
+bool RigidBodySet::hasNoRigidBodyWithFlag8(bool require_motion_flag_1_to_be_unset) {
+    for (auto it = mRigidBodies.begin(), end = mRigidBodies.end(); it != end; ++it) {
+        if (it->isFlag8Set())
+            return false;
+        if (require_motion_flag_1_to_be_unset && it->isMotionFlag1Set())
+            return false;
+    }
+    return true;
+}
+
+void RigidBodySet::callRigidBody_x_7(u8 type) {
+    for (auto& body : mRigidBodies)
+        body.x_17(type);
+}
+
+}  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodySet.h b/src/KingSystem/Physics/RigidBody/physRigidBodySet.h
new file mode 100644
index 00000000..aeb9aeaf
--- /dev/null
+++ b/src/KingSystem/Physics/RigidBody/physRigidBodySet.h
@@ -0,0 +1,72 @@
+#pragma once
+
+#include <container/seadPtrArray.h>
+#include <hostio/seadHostIONode.h>
+#include <math/seadMatrix.h>
+#include <prim/seadNamable.h>
+#include <prim/seadSafeString.h>
+#include "KingSystem/Physics/System/physDefines.h"
+
+namespace ksys::phys {
+
+class RigidBody;
+class SystemGroupHandler;
+class UserTag;
+
+class RigidBodySet : public sead::hostio::Node {
+public:
+    explicit RigidBodySet(const sead::SafeString& name);
+    virtual ~RigidBodySet();
+
+    const sead::SafeString& getName() const { return mName; }
+
+    sead::PtrArray<RigidBody>& getRigidBodies() { return mRigidBodies; }
+    const sead::PtrArray<RigidBody>& getRigidBodies() const { return mRigidBodies; }
+    RigidBody* getRigidBody(int idx) const { return mRigidBodies[idx]; }
+
+    void setFixedAndPreserveImpulse(bool fixed, bool mark_linear_vel_as_dirty);
+    void resetFrozenState();
+    void setUseSystemTimeFactor(bool use);
+    void clearFlag400000(bool clear);
+    void setEntityMotionFlag200(bool set);
+    void setFixed(bool fixed, bool preserve_velocities);
+
+    void updateMotionTypeRelatedFlags();
+    void triggerScheduledMotionTypeChange();
+
+    bool hasActiveEntityBody() const;
+
+    RigidBody* findBodyByHavokName(const sead::SafeString& name);
+    const RigidBody* findBodyByHavokName(const sead::SafeString& name) const;
+    int findBodyIndexByHavokName(const sead::SafeString& name) const;
+
+    void setUserTag(UserTag* tag);
+
+    /// Set the specified handler for all rigid bodies whose type (entity/sensor) matches
+    /// the layer type of the handler.
+    void setSystemGroupHandler(SystemGroupHandler* handler);
+
+    /// Set the specified handler for all rigid bodies whose type (entity/sensor) matches
+    /// both `layer_type` and the layer type of the handler.
+    void setSystemGroupHandler(SystemGroupHandler* handler, ContactLayerType layer_type);
+
+    void setTransform(const sead::Matrix34f& mtx);
+
+    void enableContactLayer(ContactLayer layer);
+    void disableContactLayer(ContactLayer layer);
+    void disableAllContactLayers();
+
+    void setScaleAndUpdateImmediately(float scale);
+    void setScale(float scale);
+    void callRigidBody_x_0();
+    void addOrRemoveRigidBodiesToWorld();
+    bool areAllTrueRigidBody_x_6();
+    bool hasNoRigidBodyWithFlag8(bool require_motion_flag_1_to_be_unset);
+    void callRigidBody_x_7(u8 type);
+
+private:
+    sead::SafeString mName;
+    sead::PtrArray<RigidBody> mRigidBodies;
+};
+
+}  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/System/physInstanceSet.cpp b/src/KingSystem/Physics/System/physInstanceSet.cpp
index 33d3f741..696405f6 100644
--- a/src/KingSystem/Physics/System/physInstanceSet.cpp
+++ b/src/KingSystem/Physics/System/physInstanceSet.cpp
@@ -1,4 +1,5 @@
 #include "KingSystem/Physics/System/physInstanceSet.h"
+#include "KingSystem/Physics/RigidBody/physRigidBodySet.h"
 
 namespace ksys::phys {
 
@@ -77,7 +78,7 @@ u32 InstanceSet::sub_7100FB9C2C() const {
 
 void InstanceSet::sub_7100FBA9BC() {
     for (auto& rb : mRigidBodySets) {
-        rb.sub_7100FA97FC();
+        rb.callRigidBody_x_0();
     }
 
     for (auto& body : mList) {
@@ -92,7 +93,7 @@ void InstanceSet::sub_7100FBACE0(phys::ContactLayer layer) {
     bool sensor = phys::getContactLayerType(layer) != ContactLayerType::Entity;
 
     for (auto& rb : mRigidBodySets) {
-        rb.disableCollisionMaybe(layer);
+        rb.disableContactLayer(layer);
     }
     if (sensor)
         return;
@@ -106,7 +107,7 @@ void InstanceSet::sub_7100FBACE0(phys::ContactLayer layer) {
 
 void InstanceSet::sub_7100FBAD74() {
     for (auto& rb : mRigidBodySets) {
-        rb.disableAllContact();
+        rb.disableAllContactLayers();
     }
     if (mRagdollController != nullptr) {
         mRagdollController->sub_71012217A8();
@@ -143,7 +144,7 @@ void InstanceSet::sub_7100FBB00C(phys::RigidBody* body, phys::RigidBodyParam* pa
 
 void* InstanceSet::sub_7100FBBC28(const sead::SafeString& name) const {
     for (auto& rb : mRigidBodySets) {
-        void* p = rb.findXByName(name);
+        void* p = rb.findBodyByHavokName(name);
         if (p != nullptr)
             return p;
     }
diff --git a/src/KingSystem/Physics/System/physInstanceSet.h b/src/KingSystem/Physics/System/physInstanceSet.h
index e6aa9238..b98b98b7 100644
--- a/src/KingSystem/Physics/System/physInstanceSet.h
+++ b/src/KingSystem/Physics/System/physInstanceSet.h
@@ -18,6 +18,7 @@ public:
 
 namespace ksys::phys {
 
+class RigidBodySet;
 class SystemGroupHandler;
 
 class Ragdoll {};
@@ -46,21 +47,6 @@ struct ContactInfo {
     sead::SafeString mName;
 };
 
-class RigidBodySet {
-public:
-    void disableAllContact();
-    void sub_7100FA97FC();
-    void disableCollisionMaybe(ContactLayer);
-    void* findXByName(const sead::SafeString& name) const;
-
-    RigidBody* getRigidBody() const { return mBodies[0]; }
-    RigidBody* getRigidBody(s32 idx) const { return mBodies[idx]; }
-
-private:
-    u8 _0[0x18];
-    sead::PtrArray<RigidBody> mBodies;
-};
-
 class InstanceSet : public sead::hostio::Node {
 public:
     enum class Flag : u32 {