ksys/phys: Add more StaticCompound stuff, part 1

7 files changed, 184 insertions, 11 deletions

diff --git a/src/KingSystem/Physics/CMakeLists.txt b/src/KingSystem/Physics/CMakeLists.txt
index c5ca57de..a9a37fb2 100644
--- a/src/KingSystem/Physics/CMakeLists.txt
+++ b/src/KingSystem/Physics/CMakeLists.txt
@@ -96,6 +96,8 @@ target_sources(uking PRIVATE
   StaticCompound/physStaticCompoundBodyGroup.h
   StaticCompound/physStaticCompoundInfo.cpp
   StaticCompound/physStaticCompoundInfo.h
+  StaticCompound/physStaticCompoundMgr.cpp
+  StaticCompound/physStaticCompoundMgr.h
   StaticCompound/physStaticCompoundUtil.cpp
   StaticCompound/physStaticCompoundUtil.h
 
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBody.h b/src/KingSystem/Physics/RigidBody/physRigidBody.h
index 304c3129..88a56ec2 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBody.h
+++ b/src/KingSystem/Physics/RigidBody/physRigidBody.h
@@ -59,7 +59,7 @@ public:
         _1 = 1,
         _2 = 2,
         TerrainHeightField = 3,
-        _4 = 4,
+        StaticCompoundBody = 4,
         CharacterController = 5,
         TeraMesh = 6,
     };
diff --git a/src/KingSystem/Physics/StaticCompound/physStaticCompoundBodyGroup.cpp b/src/KingSystem/Physics/StaticCompound/physStaticCompoundBodyGroup.cpp
index dc0e4ef1..3a95859d 100644
--- a/src/KingSystem/Physics/StaticCompound/physStaticCompoundBodyGroup.cpp
+++ b/src/KingSystem/Physics/StaticCompound/physStaticCompoundBodyGroup.cpp
@@ -1,7 +1,104 @@
 #include "KingSystem/Physics/StaticCompound/physStaticCompoundBodyGroup.h"
+#include <Havok/Physics2012/Dynamics/Entity/hkpRigidBody.h>
+#include <Havok/Physics2012/Dynamics/World/hkpPhysicsSystem.h>
+#include "KingSystem/Physics/RigidBody/physRigidBodyFromResource.h"
+#include "KingSystem/Physics/RigidBody/physRigidBodyParam.h"
+#include "KingSystem/Physics/StaticCompound/physStaticCompoundInfo.h"
+#include "KingSystem/Physics/StaticCompound/physStaticCompoundMgr.h"
+#include "KingSystem/Physics/System/physSystem.h"
 
 namespace ksys::phys {
 
+// TODO: rename
+constexpr int BodyGroupNumMatrices = 8;
+
 BodyGroup::BodyGroup() = default;
 
+BodyGroup::~BodyGroup() {
+    mFlags.reset(Flag::Initialised);
+
+    for (int i = 0, n = mRigidBodiesPerBodyLayerType.size(); i < n; ++i) {
+        delete mRigidBodiesPerBodyLayerType[i];
+    }
+
+    mRigidBodiesPerBodyLayerType.freeBuffer();
+    mShapesPerBodyLayerType.freeBuffer();
+    mMatrices2.freeBuffer();
+    mMatrices.freeBuffer();
+    _e8.freeBuffer();
+    mRigidBodies.freeBuffer();
+}
+
+void BodyGroup::init(const hkpPhysicsSystem& system, sead::Matrix34f* mtx, StaticCompound* sc,
+                     sead::Heap* heap) {
+    mStaticCompound = sc;
+    mMtxPtr = mtx;
+
+    const int num_rigid_bodies = system.getRigidBodies().getSize();
+
+    mRigidBodies.allocBuffer(num_rigid_bodies + NumBodyLayerTypes, heap);
+
+    if (num_rigid_bodies > 0) {
+        auto* group_handler = System::instance()->getStaticCompoundMgr()->getGroupHandler();
+
+        mRigidBodiesPerBodyLayerType.allocBufferAssert(NumBodyLayerTypes, heap);
+        mShapesPerBodyLayerType.allocBufferAssert(NumBodyLayerTypes, heap);
+        for (int i = 0; i < NumBodyLayerTypes; ++i) {
+            mRigidBodiesPerBodyLayerType[i] = nullptr;
+            mShapesPerBodyLayerType[i] = nullptr;
+        }
+
+        for (int i = 0; i < num_rigid_bodies; ++i) {
+            hkpRigidBody* hk_body = system.getRigidBodies()[i];
+
+            bool is_entity = sead::SafeString(hk_body->getName()).include("Entity");
+            auto layer_type = is_entity ? ContactLayerType::Entity : ContactLayerType::Sensor;
+
+            auto* body = new (heap) RigidBodyFromResource(0.0, hk_body, layer_type, heap,
+                                                          RigidBody::Type::StaticCompoundBody);
+
+            RigidBodyInstanceParam param;
+            param.max_linear_velocity = 1000;
+            body->initMotionAccessor(param, heap, /* init_motion */ false);
+
+            BodyLayerType body_layer_type = getBodyLayerType(body->getContactLayer());
+            mRigidBodiesPerBodyLayerType[int(body_layer_type)] = body;
+
+            body->updateCollidableQualityType(true);
+            body->changeMotionType(MotionType::Fixed);
+            if (body->isEntity()) {
+                body->setSystemGroupHandler(group_handler);
+                body->enableGroundCollision(false);
+                body->enableWaterCollision(false);
+            }
+
+            mRigidBodies.pushBack(body);
+
+            // XXX: eww, const_cast
+            auto* shape =
+                const_cast<hkpShape*>(system.getRigidBodies()[i]->getCollidable()->getShape());
+            shape->m_userData = reinterpret_cast<hkUlong>(this);
+            mShapesPerBodyLayerType[int(body_layer_type)] = shape;
+        }
+    }
+
+    mMatrices2.allocBufferAssert(BodyGroupNumMatrices, heap);
+    mMatrices.allocBufferAssert(BodyGroupNumMatrices, heap);
+    for (int i = 0, n = mMatrices2.size(); i < n; ++i) {
+        mMatrices2[i].makeIdentity();
+        mMatrices[i].makeIdentity();
+    }
+
+    mFlags.set(Flag::Initialised);
+}
+
+void BodyGroup::modifyMatrix(const sead::Matrix34f& matrix, int index) {
+    if (mMatrices[index] == matrix)
+        return;
+
+    mMatrices[index] = matrix;
+    mModifiedMatrices |= 1 << index;
+    mFlags.set(Flag::HasModifiedMatrix);
+}
+
 }  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/StaticCompound/physStaticCompoundBodyGroup.h b/src/KingSystem/Physics/StaticCompound/physStaticCompoundBodyGroup.h
index a5f1764a..f6be6605 100644
--- a/src/KingSystem/Physics/StaticCompound/physStaticCompoundBodyGroup.h
+++ b/src/KingSystem/Physics/StaticCompound/physStaticCompoundBodyGroup.h
@@ -5,10 +5,12 @@
 #include <container/seadPtrArray.h>
 #include <math/seadMatrix.h>
 #include <math/seadVector.h>
+#include <prim/seadTypedBitFlag.h>
 #include <thread/seadAtomic.h>
 #include "KingSystem/Utils/Types.h"
 
 class hkpPhysicsSystem;
+class hkpShape;
 
 namespace ksys::phys {
 
@@ -29,12 +31,27 @@ public:
 
     void disableCollision(BodyLayerType body_layer_type, int instance_id, bool x);
 
-    sead::Atomic<u32> _0;
-    sead::Atomic<u32> _4;
-    sead::Buffer<void*> _8;
-    sead::Buffer<void*> _18;
-    sead::Buffer<void*> _28;
-    sead::Buffer<void*> _38;
+    void modifyMatrix(const sead::Matrix34f& matrix, int index);
+
+private:
+    enum class Flag {
+        Initialised = 1 << 0,
+        HasModifiedMatrix = 1 << 3,
+    };
+
+    struct Unk1 {
+        virtual ~Unk1();
+
+        u8 _8[0xc0];
+    };
+
+    sead::TypedBitFlag<Flag, sead::Atomic<u32>> mFlags;
+    sead::Atomic<u32> mModifiedMatrices;
+    sead::Buffer<RigidBody*> mRigidBodiesPerBodyLayerType;
+    sead::Buffer<const hkpShape*> mShapesPerBodyLayerType;
+    // TODO: rename
+    sead::Buffer<sead::Matrix34f> mMatrices;
+    sead::Buffer<sead::Matrix34f> mMatrices2;
     sead::Matrix34f mMtx0 = sead::Matrix34f::ident;
     sead::Matrix34f mMtx1 = sead::Matrix34f::ident;
     sead::Matrix34f* mMtxPtr{};
@@ -43,7 +60,7 @@ public:
     sead::Vector3f _c8 = sead::Vector3f::zero;
     sead::Vector3f _d4 = sead::Vector3f::zero;
     u32 _e0{};
-    sead::Buffer<void*> _e8;
+    sead::Buffer<Unk1> _e8;
 };
 KSYS_CHECK_SIZE_NX150(BodyGroup, 0xf8);
 
diff --git a/src/KingSystem/Physics/StaticCompound/physStaticCompoundMgr.cpp b/src/KingSystem/Physics/StaticCompound/physStaticCompoundMgr.cpp
new file mode 100644
index 00000000..f5d79b25
--- /dev/null
+++ b/src/KingSystem/Physics/StaticCompound/physStaticCompoundMgr.cpp
@@ -0,0 +1,22 @@
+#include "KingSystem/Physics/StaticCompound/physStaticCompoundMgr.h"
+#include "KingSystem/Physics/System/physSystem.h"
+
+namespace ksys::phys {
+
+StaticCompoundMgr::StaticCompoundMgr() = default;
+
+StaticCompoundMgr::~StaticCompoundMgr() {
+    if (mGroupHandler) {
+        System::instance()->removeSystemGroupHandler(mGroupHandler);
+        mGroupHandler = nullptr;
+    }
+}
+
+bool StaticCompoundMgr::init() {
+    if (!mGroupHandler)
+        mGroupHandler = System::instance()->addSystemGroupHandler(ContactLayerType::Entity);
+
+    return true;
+}
+
+}  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/StaticCompound/physStaticCompoundMgr.h b/src/KingSystem/Physics/StaticCompound/physStaticCompoundMgr.h
new file mode 100644
index 00000000..7cab39f5
--- /dev/null
+++ b/src/KingSystem/Physics/StaticCompound/physStaticCompoundMgr.h
@@ -0,0 +1,30 @@
+#pragma once
+
+#include <container/seadPtrArray.h>
+#include <hostio/seadHostIONode.h>
+#include <thread/seadCriticalSection.h>
+
+namespace ksys::phys {
+
+class StaticCompound;
+class SystemGroupHandler;
+
+/// Provides convenient access to all loaded StaticCompounds.
+class StaticCompoundMgr : public sead::hostio::Node {
+public:
+    StaticCompoundMgr();
+    virtual ~StaticCompoundMgr();
+
+    bool init();
+
+    SystemGroupHandler* getGroupHandler() const { return mGroupHandler; }
+
+    // TODO: more functions
+
+private:
+    sead::FixedPtrArray<StaticCompound, 0x40> mStaticCompounds;
+    sead::CriticalSection mCS;
+    SystemGroupHandler* mGroupHandler{};
+};
+
+}  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/System/physSystem.h b/src/KingSystem/Physics/System/physSystem.h
index b9439b99..4e01e60b 100644
--- a/src/KingSystem/Physics/System/physSystem.h
+++ b/src/KingSystem/Physics/System/physSystem.h
@@ -15,13 +15,14 @@ class CollisionInfo;
 class ContactLayerCollisionInfo;
 class ContactLayerCollisionInfoGroup;
 class ContactMgr;
+class ContactPointInfo;
 class GroupFilter;
+class LayerContactPointInfo;
 class MaterialTable;
 class RayCastForRequest;
 class RigidBody;
 class RigidBodyRequestMgr;
-class ContactPointInfo;
-class LayerContactPointInfo;
+class StaticCompoundMgr;
 class SystemData;
 class SystemGroupHandler;
 
@@ -42,6 +43,7 @@ public:
     float get64() const { return _64; }
     float getTimeFactor() const { return mTimeFactor; }
     ContactMgr* getContactMgr() const { return mContactMgr; }
+    StaticCompoundMgr* getStaticCompoundMgr() const { return mStaticCompoundMgr; }
     RigidBodyRequestMgr* getRigidBodyRequestMgr() const { return mRigidBodyRequestMgr; }
     SystemData* getSystemData() const { return mSystemData; }
     MaterialTable* getMaterialTable() const { return mMaterialTable; }
@@ -83,6 +85,9 @@ public:
     // 0x0000007101216a20
     void removeRigidBodyFromContactSystem(RigidBody* body);
 
+    // 0x000000710121686c
+    SystemGroupHandler* addSystemGroupHandler(ContactLayerType layer_type, int free_list_idx = 0);
+    // 0x0000007101215b68
     void removeSystemGroupHandler(SystemGroupHandler* handler);
 
     hkpWorld* getHavokWorld(ContactLayerType type) const;
@@ -138,7 +143,7 @@ private:
     sead::FixedPtrArray<void*, 2> _128;
     ContactMgr* mContactMgr;
     void* _150;
-    void* _158;
+    StaticCompoundMgr* mStaticCompoundMgr;
     RigidBodyRequestMgr* mRigidBodyRequestMgr;
     void* _168;
     void* mRigidBodyDividedMeshShapeMgr;