ksys/phys: Add more StaticCompound functions, part 2

data/uking_functions.csv

@@ -84257,14 +84257,14 @@ Address,Quality,Size,Name
 0x0000007100fcbe40,O,000168,_ZN4ksys4phys28StaticCompoundRigidBodyGroupC1Ev
 0x0000007100fcbee8,O,000288,_ZN4ksys4phys28StaticCompoundRigidBodyGroupD1Ev
 0x0000007100fcc008,O,001492,_ZN4ksys4phys28StaticCompoundRigidBodyGroup4initERK16hkpPhysicsSystemPN4sead8Matrix34IfEEPNS0_14StaticCompoundEPNS5_4HeapE
-0x0000007100fcc5dc,U,000120,BodyGroup::x
-0x0000007100fcc654,U,000120,BodyGroup::x_0
-0x0000007100fcc6cc,U,000248,BodyGroup::x_1
-0x0000007100fcc7c4,U,000392,BodyGroup::x_2
-0x0000007100fcc94c,U,000212,BodyGroup::x_3
-0x0000007100fcca20,U,000112,BodyGroup::x_4
-0x0000007100fcca90,U,000108,BodyGroup::disableCollision
-0x0000007100fccafc,U,000116,BodyGroup::x_6
+0x0000007100fcc5dc,O,000120,_ZNK4ksys4phys28StaticCompoundRigidBodyGroup26isAnyRigidBodyAddedToWorldEv
+0x0000007100fcc654,O,000120,_ZNK4ksys4phys28StaticCompoundRigidBodyGroup31isAnyRigidBodyBeingAddedToWorldEv
+0x0000007100fcc6cc,O,000248,_ZN4ksys4phys28StaticCompoundRigidBodyGroup10addToWorldEv
+0x0000007100fcc7c4,U,000392,BodyGroup::getMatrix
+0x0000007100fcc94c,O,000212,_ZN4ksys4phys28StaticCompoundRigidBodyGroup15removeFromWorldEv
+0x0000007100fcca20,O,000112,_ZN4ksys4phys28StaticCompoundRigidBodyGroup26removeFromWorldImmediatelyEv
+0x0000007100fcca90,O,000108,_ZN4ksys4phys28StaticCompoundRigidBodyGroup18setInstanceEnabledENS0_13BodyLayerTypeEib
+0x0000007100fccafc,O,000116,_ZN4ksys4phys28StaticCompoundRigidBodyGroup30enableAllInstancesAndShapeKeysEv
 0x0000007100fccb70,U,000268,BodyGroup::x_7
 0x0000007100fccc7c,U,000312,BodyGroup::x_8
 0x0000007100fccdb4,U,000944,BodyGroup::x_9

src/KingSystem/Physics/RigidBody/physRigidBody.cpp

@@ -471,7 +471,7 @@ void RigidBody::x_10() {
     mFlags.set(Flag::_20);
     mFlags.set(Flag::_4);
 
-    x_8(nullptr);
+    removeFromWorldImmediately();
 }
 
 void RigidBody::setCollisionInfo(CollisionInfo* info) {

src/KingSystem/Physics/RigidBody/physRigidBody.h

@@ -183,7 +183,7 @@ public:
     bool isSensorMotionFlag40000Set() const;
 
     // 0x0000007100f8d840
-    void x_8(void* arg);
+    void removeFromWorldImmediately(void* arg = nullptr);
 
     MotionType getMotionType() const;
 

src/KingSystem/Physics/StaticCompound/physStaticCompound.cpp

@@ -93,7 +93,7 @@ bool StaticCompound::disableCollision(int actor_idx, bool x) {
         const auto type = static_cast<BodyLayerType>(info->m_BodyLayerType);
         const auto instance_id = info->m_InstanceId;
 
-        mFieldBodyGroups[group].disableCollision(type, instance_id, x);
+        mFieldBodyGroups[group].setInstanceEnabled(type, instance_id, x);
     }
     return true;
 }

src/KingSystem/Physics/StaticCompound/physStaticCompoundRigidBodyGroup.cpp

@@ -1,6 +1,8 @@
 #include "KingSystem/Physics/StaticCompound/physStaticCompoundRigidBodyGroup.h"
 #include <Havok/Physics2012/Dynamics/Entity/hkpRigidBody.h>
 #include <Havok/Physics2012/Dynamics/World/hkpPhysicsSystem.h>
+#include <Havok/Physics2012/Internal/Collide/StaticCompound/hkpStaticCompoundShape.h>
+#include "KingSystem/Physics/RigidBody/physRigidBody.h"
 #include "KingSystem/Physics/RigidBody/physRigidBodyFromResource.h"
 #include "KingSystem/Physics/RigidBody/physRigidBodyParam.h"
 #include "KingSystem/Physics/StaticCompound/physStaticCompoundInfo.h"
@@ -75,8 +77,8 @@ void StaticCompoundRigidBodyGroup::init(const hkpPhysicsSystem& system, sead::Ma
             mRigidBodies.pushBack(body);
 
             // XXX: eww, const_cast
-            auto* shape =
-                const_cast<hkpShape*>(system.getRigidBodies()[i]->getCollidable()->getShape());
+            auto* shape = static_cast<hkpStaticCompoundShape*>(
+                const_cast<hkpShape*>(system.getRigidBodies()[i]->getCollidable()->getShape()));
             shape->m_userData = reinterpret_cast<hkUlong>(this);
             mShapesPerBodyLayerType[int(body_layer_type)] = shape;
         }
@@ -92,6 +94,75 @@ void StaticCompoundRigidBodyGroup::init(const hkpPhysicsSystem& system, sead::Ma
     mFlags.set(Flag::Initialised);
 }
 
+bool StaticCompoundRigidBodyGroup::isAnyRigidBodyAddedToWorld() const {
+    for (int i = 0, n = mRigidBodies.size(); i < n; ++i) {
+        if (mRigidBodies[i] && mRigidBodies[i]->isAddedToWorld())
+            return true;
+    }
+    return false;
+}
+
+bool StaticCompoundRigidBodyGroup::isAnyRigidBodyBeingAddedToWorld() const {
+    for (int i = 0, n = mRigidBodies.size(); i < n; ++i) {
+        if (mRigidBodies[i] && mRigidBodies[i]->isAddingBodyToWorld())
+            return true;
+    }
+    return false;
+}
+
+void StaticCompoundRigidBodyGroup::addToWorld() {
+    for (int i = 0, n = mRigidBodies.size(); i < n; ++i) {
+        auto* body = mRigidBodies[i];
+
+        auto lock = body->makeScopedLock();
+
+        body->setTransform(getMatrix(), true);
+
+        if (body->getMotionFlags().isOn(RigidBody::MotionFlag::BodyRemovalRequested)) {
+            body->resetMotionFlagDirect(RigidBody::MotionFlag::BodyRemovalRequested);
+        } else if (!body->isAddedToWorld() &&
+                   !body->getMotionFlags().isOn(RigidBody::MotionFlag::BodyAddRequested)) {
+            body->setMotionFlag(RigidBody::MotionFlag::BodyAddRequested);
+        }
+    }
+}
+
+void StaticCompoundRigidBodyGroup::removeFromWorld() {
+    for (int i = 0, n = mRigidBodies.size(); i < n; ++i) {
+        auto* body = mRigidBodies[i];
+
+        auto lock = body->makeScopedLock();
+
+        if (body->getMotionFlags().isOn(RigidBody::MotionFlag::BodyAddRequested)) {
+            body->resetMotionFlagDirect(RigidBody::MotionFlag::BodyAddRequested);
+        } else if (body->isAddedToWorld() &&
+                   !body->getMotionFlags().isOn(RigidBody::MotionFlag::BodyRemovalRequested)) {
+            body->setMotionFlag(RigidBody::MotionFlag::BodyRemovalRequested);
+        }
+    }
+}
+
+void StaticCompoundRigidBodyGroup::removeFromWorldImmediately() {
+    for (int i = 0, n = mRigidBodies.size(); i < n; ++i)
+        mRigidBodies[i]->removeFromWorldImmediately();
+}
+
+bool StaticCompoundRigidBodyGroup::setInstanceEnabled(BodyLayerType body_layer_type,
+                                                      int instance_id, bool enabled) {
+    hkpStaticCompoundShape* shape = mShapesPerBodyLayerType[int(body_layer_type)];
+    shape->setInstanceEnabled(instance_id, enabled);
+    mFlags.set(Flag::HasEnabledOrDisabledInstance);
+    return true;
+}
+
+void StaticCompoundRigidBodyGroup::enableAllInstancesAndShapeKeys() {
+    for (int i = 0, n = mShapesPerBodyLayerType.size(); i < n; ++i) {
+        auto* shape = mShapesPerBodyLayerType[i];
+        if (shape)
+            shape->enableAllInstancesAndShapeKeys();
+    }
+}
+
 void StaticCompoundRigidBodyGroup::modifyMatrix(const sead::Matrix34f& matrix, int index) {
     if (mMatrices[index] == matrix)
         return;

src/KingSystem/Physics/StaticCompound/physStaticCompoundRigidBodyGroup.h

@@ -10,7 +10,7 @@
 #include "KingSystem/Utils/Types.h"
 
 class hkpPhysicsSystem;
-class hkpShape;
+class hkpStaticCompoundShape;
 
 namespace ksys::phys {
 
@@ -29,14 +29,31 @@ public:
     void init(const hkpPhysicsSystem& system, sead::Matrix34f* mtx, StaticCompound* sc,
               sead::Heap* heap);
 
-    void disableCollision(BodyLayerType body_layer_type, int instance_id, bool x);
+    /// @returns whether any rigid body in this group has been added to the world.
+    bool isAnyRigidBodyAddedToWorld() const;
+
+    /// @returns whether any rigid body in this group is being added to the world.
+    bool isAnyRigidBodyBeingAddedToWorld() const;
+
+    void addToWorld();
+    void removeFromWorld();
+    /// Force the removal of all rigid bodies in the group. This is unlike removeFromWorld,
+    /// which merely requests the removal asynchronously.
+    void removeFromWorldImmediately();
+
+    bool setInstanceEnabled(BodyLayerType body_layer_type, int instance_id, bool enabled);
+    void enableAllInstancesAndShapeKeys();
 
     void modifyMatrix(const sead::Matrix34f& matrix, int index);
 
 private:
     enum class Flag {
         Initialised = 1 << 0,
+        _2 = 1 << 1,
+        _4 = 1 << 2,
         HasModifiedMatrix = 1 << 3,
+        _10 = 1 << 4,
+        HasEnabledOrDisabledInstance = 1 << 5,
     };
 
     struct Unk1 {
@@ -45,10 +62,12 @@ private:
         u8 _8[0xc0];
     };
 
+    const sead::Matrix34f& getMatrix();
+
     sead::TypedBitFlag<Flag, sead::Atomic<u32>> mFlags;
     sead::Atomic<u32> mModifiedMatrices;
     sead::Buffer<RigidBody*> mRigidBodiesPerBodyLayerType;
-    sead::Buffer<const hkpShape*> mShapesPerBodyLayerType;
+    sead::Buffer<hkpStaticCompoundShape*> mShapesPerBodyLayerType;
     // TODO: rename
     sead::Buffer<sead::Matrix34f> mMatrices;
     sead::Buffer<sead::Matrix34f> mMatrices2;