ksys/phys: Add more StaticCompound functions, part 2

1 files changed, 73 insertions, 2 deletions

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;