ksys/phys: Finish StaticCompoundMgr

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