ksys/phys: Finish StaticCompoundMgr

5 files changed, 198 insertions, 5 deletions

diff --git a/src/KingSystem/Physics/StaticCompound/physStaticCompound.cpp b/src/KingSystem/Physics/StaticCompound/physStaticCompound.cpp
index 47b3620f..88853b39 100644
--- a/src/KingSystem/Physics/StaticCompound/physStaticCompound.cpp
+++ b/src/KingSystem/Physics/StaticCompound/physStaticCompound.cpp
@@ -222,7 +222,7 @@ StaticCompoundRigidBodyGroup* StaticCompound::getFieldBodyGroup(int idx) {
     return &mFieldBodyGroups[idx];
 }
 
-bool StaticCompound::hasFieldBodyGroup(StaticCompoundRigidBodyGroup* group) const {
+bool StaticCompound::hasFieldBodyGroup(const StaticCompoundRigidBodyGroup* group) const {
     for (int i = 0, n = mFieldBodyGroups.size(); i < n; ++i) {
         if (&mFieldBodyGroups[i] == group)
             return true;
diff --git a/src/KingSystem/Physics/StaticCompound/physStaticCompound.h b/src/KingSystem/Physics/StaticCompound/physStaticCompound.h
index 78c7f5cf..4c328e51 100644
--- a/src/KingSystem/Physics/StaticCompound/physStaticCompound.h
+++ b/src/KingSystem/Physics/StaticCompound/physStaticCompound.h
@@ -27,6 +27,8 @@ public:
     StaticCompound();
     ~StaticCompound() override;
 
+    bool isInitialised() const { return mFlags.isOn(Flag::Initialised); }
+
     bool isAnyRigidBodyAddedToWorld() const;
     bool isAnyRigidBodyAddedOrBeingAddedToWorld() const;
     void removeFromWorld();
@@ -45,7 +47,7 @@ public:
 
     int getNumFieldBodyGroups() const { return mFieldBodyGroups.size(); }
     StaticCompoundRigidBodyGroup* getFieldBodyGroup(int idx);
-    bool hasFieldBodyGroup(StaticCompoundRigidBodyGroup* group) const;
+    bool hasFieldBodyGroup(const StaticCompoundRigidBodyGroup* group) const;
 
     // res::Resource interface
     void doCreate_(u8* buffer, u32 buffer_size, sead::Heap* parent_heap) override;
diff --git a/src/KingSystem/Physics/StaticCompound/physStaticCompoundMgr.cpp b/src/KingSystem/Physics/StaticCompound/physStaticCompoundMgr.cpp
index f5d79b25..c16285a2 100644
--- a/src/KingSystem/Physics/StaticCompound/physStaticCompoundMgr.cpp
+++ b/src/KingSystem/Physics/StaticCompound/physStaticCompoundMgr.cpp
@@ -1,4 +1,9 @@
 #include "KingSystem/Physics/StaticCompound/physStaticCompoundMgr.h"
+#include <Havok/Physics2012/Internal/Collide/StaticCompound/hkpStaticCompoundShape.h>
+#include <prim/seadScopedLock.h>
+#include "KingSystem/Physics/StaticCompound/physStaticCompound.h"
+#include "KingSystem/Physics/StaticCompound/physStaticCompoundInfo.h"
+#include "KingSystem/Physics/StaticCompound/physStaticCompoundRigidBodyGroup.h"
 #include "KingSystem/Physics/System/physSystem.h"
 
 namespace ksys::phys {
@@ -19,4 +24,154 @@ bool StaticCompoundMgr::init() {
     return true;
 }
 
+void StaticCompoundMgr::addStaticCompound(StaticCompound* sc) {
+    auto lock = sead::makeScopedLock(mCS);
+
+    if (mStaticCompounds.indexOf(sc) < 0)
+        mStaticCompounds.pushBack(sc);
+}
+
+void StaticCompoundMgr::removeStaticCompound(StaticCompound* sc) {
+    auto lock = sead::makeScopedLock(mCS);
+
+    int index = mStaticCompounds.indexOf(sc);
+    if (index >= 0)
+        mStaticCompounds.erase(index);
+}
+
+void StaticCompoundMgr::resetExtraTransformsAndApply() {
+    auto lock = sead::makeScopedLock(mCS);
+
+    for (int i = 0, n = mStaticCompounds.size(); i < n; ++i)
+        mStaticCompounds[i]->resetExtraTransformsAndApply();
+}
+
+// NON_MATCHING: should be a tail call but somehow isn't
+StaticCompoundRigidBodyGroup* StaticCompoundMgr::getBodyGroup(int group_index) const {
+    if (mStaticCompounds.size() < 1)
+        return nullptr;
+
+    return mStaticCompounds[0]->getFieldBodyGroup(group_index + 1);
+}
+
+bool StaticCompoundMgr::hasBodyGroup(const StaticCompoundRigidBodyGroup* group) const {
+    if (!group)
+        return false;
+
+    auto lock = sead::makeScopedLock(mCS);
+
+    for (int i = 0, n = mStaticCompounds.size(); i < n; ++i) {
+        if (!mStaticCompounds[i]->isInitialised())
+            continue;
+        if (!mStaticCompounds[i]->hasFieldBodyGroup(group))
+            continue;
+
+        return true;
+    }
+
+    return false;
+}
+
+void StaticCompoundMgr::processUpdates() {
+    auto lock = sead::makeScopedLock(mCS);
+
+    for (int i = 0, n = mStaticCompounds.size(); i < n; ++i)
+        mStaticCompounds[i]->processUpdates();
+}
+
+void StaticCompoundMgr::recomputeTransformMatrix() {
+    auto lock = sead::makeScopedLock(mCS);
+
+    for (int i = 0, n = mStaticCompounds.size(); i < n; ++i)
+        mStaticCompounds[i]->recomputeTransformMatrix();
+}
+
+sead::Matrix34f
+StaticCompoundMgr::getTransformedMatrix(StaticCompoundRigidBodyGroup* const& p_group,
+                                        const sead::Matrix34f& mtx) const {
+    auto* group = p_group;
+    if (!hasBodyGroup(group))
+        return mtx;
+    return group->getTransformedMatrix(mtx);
+}
+
+sead::Matrix34f
+StaticCompoundMgr::getInvTransformedMatrix(StaticCompoundRigidBodyGroup* const& p_group,
+                                           const sead::Matrix34f& mtx) const {
+    auto* group = p_group;
+    if (!hasBodyGroup(group))
+        return mtx;
+    return group->getInvTransformedMatrix(mtx);
+}
+
+sead::Vector3f StaticCompoundMgr::getTransformedPos(StaticCompoundRigidBodyGroup* const& p_group,
+                                                    const sead::Vector3f& pos) const {
+    auto* group = p_group;
+    if (!hasBodyGroup(group))
+        return pos;
+    return group->getTransformedPos(pos);
+}
+
+sead::Vector3f StaticCompoundMgr::getRotatedDir(StaticCompoundRigidBodyGroup* const& p_group,
+                                                const sead::Vector3f& dir) const {
+    auto* group = p_group;
+    if (!hasBodyGroup(group))
+        return dir;
+    return group->getRotatedDir(dir);
+}
+
+void StaticCompoundMgr::setExtraTransform(StaticCompoundRigidBodyGroup* const& p_group,
+                                          const sead::Matrix34f& transform, int index) {
+    auto* group = p_group;
+    if (!hasBodyGroup(group))
+        return;
+    group->setExtraTransform(transform, index);
+}
+
+const sead::Matrix34f&
+StaticCompoundMgr::getExtraTransform(StaticCompoundRigidBodyGroup* const& p_group,
+                                     int index) const {
+    auto* group = p_group;
+    if (!hasBodyGroup(group))
+        return sead::Matrix34f::ident;
+    return group->getExtraTransform(index);
+}
+
+RigidBody* StaticCompoundMgr::getRigidBody(StaticCompoundRigidBodyGroup* const& p_group) const {
+    auto* group = p_group;
+    if (!hasBodyGroup(group))
+        return nullptr;
+
+    for (int i = 0; i < NumBodyLayerTypes; ++i) {
+        if (auto* body = group->getRigidBody(BodyLayerType(i)))
+            return body;
+    }
+
+    return nullptr;
+}
+
+void StaticCompoundMgr::getBodyGroupAndMapObject(StaticCompoundRigidBodyGroup** p_group,
+                                                 map::Object** p_map_object,
+                                                 const hkpStaticCompoundShape& shape,
+                                                 const u32* shape_key) const {
+    auto* group = reinterpret_cast<StaticCompoundRigidBodyGroup*>(shape.getUserData());
+
+    int instance_id;
+    hkpShapeKey child_key;
+    auto extract_key = [&] {
+        shape.decomposeShapeKey(*shape_key, instance_id, child_key);
+        return instance_id != -1;
+    };
+
+    if (!group || !extract_key()) {
+        *p_group = nullptr;
+        *p_map_object = nullptr;
+    } else {
+        auto object_idx = static_cast<int>(shape.getInstanceUserData(instance_id));
+        auto* sc = group->getStaticCompound();
+        *p_group = group;
+        *p_map_object = sc->getMapObject(object_idx);
+    }
+}
+
 }  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/StaticCompound/physStaticCompoundMgr.h b/src/KingSystem/Physics/StaticCompound/physStaticCompoundMgr.h
index 5b15b9e5..ab84d282 100644
--- a/src/KingSystem/Physics/StaticCompound/physStaticCompoundMgr.h
+++ b/src/KingSystem/Physics/StaticCompound/physStaticCompoundMgr.h
@@ -2,11 +2,21 @@
 
 #include <container/seadPtrArray.h>
 #include <hostio/seadHostIONode.h>
+#include <math/seadMatrix.h>
+#include <math/seadVector.h>
 #include <thread/seadCriticalSection.h>
 
+class hkpStaticCompoundShape;
+
+namespace ksys::map {
+class Object;
+}
+
 namespace ksys::phys {
 
+class RigidBody;
 class StaticCompound;
+class StaticCompoundRigidBodyGroup;
 class SystemGroupHandler;
 
 /// Provides convenient access to all loaded StaticCompounds.
@@ -17,18 +27,43 @@ public:
 
     bool init();
 
-    // 0x0000007100fcb554
     void addStaticCompound(StaticCompound* sc);
-    // 0x0000007100fcb5e0
     void removeStaticCompound(StaticCompound* sc);
 
+    void resetExtraTransformsAndApply();
+    StaticCompoundRigidBodyGroup* getBodyGroup(int group_index) const;
+    bool hasBodyGroup(const StaticCompoundRigidBodyGroup* group) const;
+    void processUpdates();
+    void recomputeTransformMatrix();
+
+    sead::Matrix34f getTransformedMatrix(StaticCompoundRigidBodyGroup* const& p_group,
+                                         const sead::Matrix34f& mtx) const;
+    sead::Matrix34f getInvTransformedMatrix(StaticCompoundRigidBodyGroup* const& p_group,
+                                            const sead::Matrix34f& mtx) const;
+    sead::Vector3f getTransformedPos(StaticCompoundRigidBodyGroup* const& p_group,
+                                     const sead::Vector3f& pos) const;
+    sead::Vector3f getRotatedDir(StaticCompoundRigidBodyGroup* const& p_group,
+                                 const sead::Vector3f& dir) const;
+
+    void setExtraTransform(StaticCompoundRigidBodyGroup* const& p_group,
+                           const sead::Matrix34f& transform, int index);
+
+    const sead::Matrix34f& getExtraTransform(StaticCompoundRigidBodyGroup* const& p_group,
+                                             int index) const;
+
+    RigidBody* getRigidBody(StaticCompoundRigidBodyGroup* const& p_group) const;
+
+    void getBodyGroupAndMapObject(StaticCompoundRigidBodyGroup** p_group,
+                                  map::Object** p_map_object, const hkpStaticCompoundShape& shape,
+                                  const u32* shape_key) const;
+
     SystemGroupHandler* getGroupHandler() const { return mGroupHandler; }
 
     // TODO: more functions
 
 private:
     sead::FixedPtrArray<StaticCompound, 0x40> mStaticCompounds;
-    sead::CriticalSection mCS;
+    mutable sead::CriticalSection mCS;
     SystemGroupHandler* mGroupHandler{};
 };
 
diff --git a/src/KingSystem/Physics/StaticCompound/physStaticCompoundRigidBodyGroup.h b/src/KingSystem/Physics/StaticCompound/physStaticCompoundRigidBodyGroup.h
index 7800c1b6..21f8df37 100644
--- a/src/KingSystem/Physics/StaticCompound/physStaticCompoundRigidBodyGroup.h
+++ b/src/KingSystem/Physics/StaticCompound/physStaticCompoundRigidBodyGroup.h
@@ -71,6 +71,7 @@ public:
     sead::Vector3f getRotatedDir(const sead::Vector3f& dir) const;
 
     RigidBody* getRigidBody(BodyLayerType body_layer_type) const;
+    StaticCompound* getStaticCompound() const { return mStaticCompound; }
 
     void processUpdates();