ksys/phys: Add more StaticCompound functions, part 2

5 files changed, 98 insertions, 8 deletions

diff --git a/src/KingSystem/Physics/RigidBody/physRigidBody.cpp b/src/KingSystem/Physics/RigidBody/physRigidBody.cpp
index a7e506dc..58dde2fb 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBody.cpp
+++ b/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) {
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBody.h b/src/KingSystem/Physics/RigidBody/physRigidBody.h
index 0d85df51..dbdd2ee8 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBody.h
+++ b/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;
 
diff --git a/src/KingSystem/Physics/StaticCompound/physStaticCompound.cpp b/src/KingSystem/Physics/StaticCompound/physStaticCompound.cpp
index b18e0201..59abc724 100644
--- a/src/KingSystem/Physics/StaticCompound/physStaticCompound.cpp
+++ b/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;
 }
diff --git a/src/KingSystem/Physics/StaticCompound/physStaticCompoundRigidBodyGroup.cpp b/src/KingSystem/Physics/StaticCompound/physStaticCompoundRigidBodyGroup.cpp
index 830ecb19..e16101f8 100644
--- a/src/KingSystem/Physics/StaticCompound/physStaticCompoundRigidBodyGroup.cpp
+++ b/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;
diff --git a/src/KingSystem/Physics/StaticCompound/physStaticCompoundRigidBodyGroup.h b/src/KingSystem/Physics/StaticCompound/physStaticCompoundRigidBodyGroup.h
index 896f83a5..308c8516 100644
--- a/src/KingSystem/Physics/StaticCompound/physStaticCompoundRigidBodyGroup.h
+++ b/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;