ksys/phys: Finish SystemData and start implementing GroupFilter

9 files changed, 302 insertions, 38 deletions

diff --git a/src/KingSystem/Physics/CMakeLists.txt b/src/KingSystem/Physics/CMakeLists.txt
index 4307197e..928e58bd 100644
--- a/src/KingSystem/Physics/CMakeLists.txt
+++ b/src/KingSystem/Physics/CMakeLists.txt
@@ -42,6 +42,8 @@ target_sources(uking PRIVATE
   System/physContactInfoTable.h
   System/physDefines.cpp
   System/physDefines.h
+  System/physEntityGroupFilter.cpp
+  System/physEntityGroupFilter.h
   System/physGroupFilter.cpp
   System/physGroupFilter.h
   System/physInstanceSet.cpp
diff --git a/src/KingSystem/Physics/System/physEntityGroupFilter.cpp b/src/KingSystem/Physics/System/physEntityGroupFilter.cpp
new file mode 100644
index 00000000..52618d7a
--- /dev/null
+++ b/src/KingSystem/Physics/System/physEntityGroupFilter.cpp
@@ -0,0 +1,43 @@
+#include "KingSystem/Physics/System/physEntityGroupFilter.h"
+#include <heap/seadHeap.h>
+#include "KingSystem/Utils/HeapUtil.h"
+
+namespace ksys::phys {
+
+constexpr int NumEntityHandlersInList0 = 0x10;
+constexpr int NumEntityHandlers = 0x400;
+
+EntityGroupFilter* EntityGroupFilter::make(ContactLayer::ValueType first,
+                                           ContactLayer::ValueType last, sead::Heap* heap) {
+    auto* filter = util::alloc<EntityGroupFilter>(heap, first, last);
+    filter->initFilter(heap);
+    return filter;
+}
+
+EntityGroupFilter::EntityGroupFilter(ContactLayer::ValueType first, ContactLayer::ValueType last)
+    : GroupFilter(ContactLayerType::Entity, first, last) {}
+
+EntityGroupFilter::~EntityGroupFilter() = default;
+
+void EntityGroupFilter::doInit_(sead::Heap* heap) {
+    // Enable all collisions by default.
+    mMasks.fill(0xffffffff);
+}
+
+void EntityGroupFilter::doInitSystemGroupHandlerLists_(sead::Heap* heap) {
+    for (auto& list : mFreeLists)
+        list.initOffset(SystemGroupHandler::getFreeListNodeOffset());
+
+    mUsedList.initOffset(SystemGroupHandler::getUsedListNodeOffset());
+
+    for (int i = 1; i < NumEntityHandlers; ++i) {
+        auto& list = mFreeLists[i < NumEntityHandlersInList0];
+        list.pushBack(new (heap) EntitySystemGroupHandler(i, 0));
+    }
+}
+
+int EntityGroupFilter::getFreeListIndex(const SystemGroupHandler* handler) {
+    return handler->getIndex() < NumEntityHandlersInList0;
+}
+
+}  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/System/physEntityGroupFilter.h b/src/KingSystem/Physics/System/physEntityGroupFilter.h
new file mode 100644
index 00000000..ca83b49d
--- /dev/null
+++ b/src/KingSystem/Physics/System/physEntityGroupFilter.h
@@ -0,0 +1,63 @@
+#pragma once
+
+#include <container/seadSafeArray.h>
+#include "KingSystem/Physics/System/physDefines.h"
+#include "KingSystem/Physics/System/physGroupFilter.h"
+
+namespace ksys::phys {
+
+class EntitySystemGroupHandler : public SystemGroupHandler {
+public:
+    using SystemGroupHandler::SystemGroupHandler;
+
+    u32 m5() override;
+    u32 m6() override;
+    bool m8() override;
+};
+
+class EntityGroupFilter : public GroupFilter {
+    SEAD_RTTI_OVERRIDE(EntityGroupFilter, GroupFilter)
+public:
+    static EntityGroupFilter* make(ContactLayer::ValueType first, ContactLayer::ValueType last,
+                                   sead::Heap* heap);
+
+    EntityGroupFilter(ContactLayer::ValueType first, ContactLayer::ValueType last);
+    ~EntityGroupFilter() override;
+
+    hkBool isCollisionEnabled(const hkpCollidable& a, const hkpCollidable& b) const override;
+    hkBool isCollisionEnabled(const hkpCollisionInput& input, const hkpCdBody& a,
+                              const hkpCdBody& b, const hkpShapeContainer& bContainer,
+                              hkpShapeKey bKey) const override;
+    hkBool isCollisionEnabled(const hkpCollisionInput& input, const hkpCdBody& collectionBodyA,
+                              const hkpCdBody& collectionBodyB,
+                              const hkpShapeContainer& containerShapeA,
+                              const hkpShapeContainer& containerShapeB, hkpShapeKey keyA,
+                              hkpShapeKey keyB) const override;
+    hkBool isCollisionEnabled(const hkpShapeRayCastInput& aInput,
+                              const hkpShapeContainer& bContainer, hkpShapeKey bKey) const override;
+    hkBool isCollisionEnabled(const hkpWorldRayCastInput& inputA,
+                              const hkpCollidable& collidableB) const override;
+
+    bool m2() override { return GroupFilter::m2(); }
+    void m3() override {}
+    void m4() override {}
+    void m5() override {}
+    void m6() override {}
+    void m7() override {}
+    void m8() override {}
+
+    void setLayerCustomMask(ContactLayer layer, u32 mask) override { mMasks[layer] = mask; }
+
+    void m10() override {}
+
+private:
+    void doInitSystemGroupHandlerLists_(sead::Heap* heap) override;
+    int getFreeListIndex(const SystemGroupHandler* handler) override;
+    void doInit_(sead::Heap* heap) override;
+    virtual void m14();
+    virtual void m15();
+
+    sead::SafeArray<u32, ContactLayer::size()> mMasks;
+};
+
+}  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/System/physGroupFilter.cpp b/src/KingSystem/Physics/System/physGroupFilter.cpp
index ee698581..c8f0df8d 100644
--- a/src/KingSystem/Physics/System/physGroupFilter.cpp
+++ b/src/KingSystem/Physics/System/physGroupFilter.cpp
@@ -1,13 +1,61 @@
 #include "KingSystem/Physics/System/physGroupFilter.h"
+#include <prim/seadScopedLock.h>
+#include "KingSystem/Physics/System/physMemSystem.h"
 
 namespace ksys::phys {
 
 GroupFilter::GroupFilter(ContactLayerType type, ContactLayer::ValueType layer_first,
                          ContactLayer::ValueType layer_last)
-    : mIdxLayerFirst(layer_first), mIdxLayerLast(layer_last), mLayerType(type) {}
+    : mLayerFirst(layer_first), mLayerLast(layer_last), mLayerType(type) {}
 
 GroupFilter::~GroupFilter() = default;
 
-void GroupFilter::m13() {}
+void GroupFilter::initFilter(sead::Heap* heap) {
+    for (auto& mask : m_collisionLookupTable)
+        mask = 0;
+
+    doInit_(heap);
+    doInitSystemGroupHandlerLists_(heap);
+    mInhibitCollisions = false;
+}
+
+void GroupFilter::destroy() {
+    for (auto& list : mFreeLists) {
+        while (auto* handler = list.popFront()) {
+            delete handler;
+        }
+    }
+    delete this;
+}
+
+SystemGroupHandler* GroupFilter::addSystemGroupHandler(int free_list_idx) {
+    mCS.lock();
+
+    auto* handler = mFreeLists[free_list_idx].popFront();
+    if (!handler) {
+        mCS.unlock();
+        return nullptr;
+    }
+
+    mUsedList.pushBack(handler);
+    mCS.unlock();
+    return handler;
+}
+
+void GroupFilter::removeSystemGroupHandler(SystemGroupHandler* handler) {
+    auto lock = sead::makeScopedLock(mCS);
+
+    const int free_list_idx = getFreeListIndex(handler);
+    mFreeLists[free_list_idx].pushBack(handler);
+    mUsedList.erase(handler);
+}
+
+u32 SystemGroupHandler::m7() {
+    return 0;
+}
+
+void SystemGroupHandler::removeThis() {
+    MemSystem::instance()->removeSystemGroupHandler(this);
+}
 
 }  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/System/physGroupFilter.h b/src/KingSystem/Physics/System/physGroupFilter.h
index 08a17ae2..ddf4b327 100644
--- a/src/KingSystem/Physics/System/physGroupFilter.h
+++ b/src/KingSystem/Physics/System/physGroupFilter.h
@@ -3,12 +3,54 @@
 #include <Havok/Physics2012/Collide/Filter/Group/hkpGroupFilter.h>
 #include <basis/seadTypes.h>
 #include <container/seadOffsetList.h>
+#include <container/seadSafeArray.h>
 #include <prim/seadRuntimeTypeInfo.h>
 #include <thread/seadCriticalSection.h>
 #include "KingSystem/Physics/System/physDefines.h"
 
 namespace ksys::phys {
 
+class SystemGroupHandler {
+    SEAD_RTTI_BASE(SystemGroupHandler)
+public:
+    explicit SystemGroupHandler(int index, int filter_index)
+        : mIndex(index), mFilterIndex(filter_index) {}
+
+    virtual ~SystemGroupHandler() = default;
+    virtual u32 m5() = 0;
+    virtual u32 m6() = 0;
+    virtual u32 m7();
+    virtual bool m8() = 0;
+
+    int getIndex() const { return mIndex; }
+    int getFilterIndex() const { return mFilterIndex; }
+
+    const char* getActorName() const { return mActorName; }
+    void setActorName(const char* name) { mActorName = name; }
+
+    const char* getActorProfile() const { return mActorProfile; }
+    void setActorProfile(const char* name) { mActorProfile = name; }
+
+    /// Remove this handler from the filter it's attached to.
+    void removeThis();
+
+    static constexpr auto getFreeListNodeOffset() {
+        return offsetof(SystemGroupHandler, mFreeListNode);
+    }
+
+    static constexpr auto getUsedListNodeOffset() {
+        return offsetof(SystemGroupHandler, mUsedListNode);
+    }
+
+protected:
+    const char* mActorName = nullptr;
+    const char* mActorProfile = nullptr;
+    int mIndex = 0;
+    int mFilterIndex = 0;
+    sead::ListNode mFreeListNode;
+    sead::ListNode mUsedListNode;
+};
+
 class GroupFilter : public hkpGroupFilter {
     SEAD_RTTI_BASE(GroupFilter)
 public:
@@ -16,7 +58,22 @@ public:
                 ContactLayer::ValueType layer_last);
     ~GroupFilter() override;
 
-protected:
+    ContactLayer::ValueType getLayerFirst() const { return mLayerFirst; }
+    ContactLayer::ValueType getLayerLast() const { return mLayerLast; }
+    ContactLayerType getLayerType() const { return mLayerType; }
+
+    void initFilter(sead::Heap* heap);
+    /// Frees all memory associated with this instance and then self-destructs.
+    /// @warning Do not use this filter after a call to destroy!
+    void destroy();
+
+    SystemGroupHandler* addSystemGroupHandler(int free_list_idx);
+    void removeSystemGroupHandler(SystemGroupHandler* handler);
+
+    void setLayerCollisionEnabledMask(ContactLayer layer, u32 mask) {
+        m_collisionLookupTable[layer - getLayerFirst()] = mask;
+    }
+
     virtual bool m2() { return true; }
     virtual void m3() = 0;
     virtual void m4() = 0;
@@ -24,21 +81,23 @@ protected:
     virtual void m6() = 0;
     virtual void m7() = 0;
     virtual void m8() = 0;
-    virtual void m9() {}
+    virtual void setLayerCustomMask(ContactLayer layer, u32 mask) {}
     virtual void m10() = 0;
-    virtual void m11() = 0;
-    virtual void m12() = 0;
-    virtual void m13();
 
-    u32 mIdxLayerFirst{};
-    u32 mIdxLayerLast{};
+protected:
+    virtual void doInitSystemGroupHandlerLists_(sead::Heap* heap) = 0;
+    virtual int getFreeListIndex(const SystemGroupHandler* handler) = 0;
+
+    /// Initialises internal state.
+    virtual void doInit_(sead::Heap* heap) {}
+
+    ContactLayer::ValueType mLayerFirst{};
+    ContactLayer::ValueType mLayerLast{};
     ContactLayerType mLayerType{};
-    u8 _11c{};
-    // FIXME: types
-    sead::OffsetList<void*> _120;
-    sead::OffsetList<void*> _138;
+    bool mInhibitCollisions{};
+    sead::SafeArray<sead::OffsetList<SystemGroupHandler>, 2> mFreeLists;
     sead::CriticalSection mCS;
-    sead::OffsetList<void*> _190;
+    sead::OffsetList<SystemGroupHandler> mUsedList;
 };
 
 }  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/System/physMemSystem.h b/src/KingSystem/Physics/System/physMemSystem.h
index 5866d072..3bd26cab 100644
--- a/src/KingSystem/Physics/System/physMemSystem.h
+++ b/src/KingSystem/Physics/System/physMemSystem.h
@@ -6,6 +6,7 @@
 namespace ksys::phys {
 
 class RigidBody;
+class SystemGroupHandler;
 
 class MemSystem {
     SEAD_SINGLETON_DISPOSER(MemSystem)
@@ -14,6 +15,8 @@ public:
         void sub_7100FA6C8C(bool, RigidBody*);
     };
 
+    void removeSystemGroupHandler(SystemGroupHandler* handler);
+
     u8 _20[0x140];
     Struct160* _160;
 };
diff --git a/src/KingSystem/Physics/System/physSystemData.cpp b/src/KingSystem/Physics/System/physSystemData.cpp
index 41f3845e..319bb98e 100644
--- a/src/KingSystem/Physics/System/physSystemData.cpp
+++ b/src/KingSystem/Physics/System/physSystemData.cpp
@@ -1,5 +1,6 @@
 #include "KingSystem/Physics/System/physSystemData.h"
 #include "KingSystem/Physics/System/physContactInfoTable.h"
+#include "KingSystem/Physics/System/physGroupFilter.h"
 #include "KingSystem/Physics/System/physMaterialTable.h"
 #include "KingSystem/Physics/System/physRagdollControllerKeyList.h"
 #include "KingSystem/Resource/resHandle.h"
@@ -21,10 +22,10 @@ SystemData::~SystemData() {
     deleteAndNull(mMaterialTableHandle);
     deleteAndNull(mSubMaterialTableHandle);
 
-    deleteAndNull(mLayerTableInfo[0].mResHandle);
+    deleteAndNull(mLayerMatrices[0].mResHandle);
     deleteAndNull(mContactInfoTableHandles[0]);
 
-    deleteAndNull(mLayerTableInfo[1].mResHandle);
+    deleteAndNull(mLayerMatrices[1].mResHandle);
     deleteAndNull(mContactInfoTableHandles[1]);
 
     deleteAndNull(mCharacterCtrlTable.mResHandle);
@@ -36,11 +37,11 @@ SystemData::~SystemData() {
     }
 }
 
-void SystemData::load(sead::Heap* heap, LayerTable* entity_layer_table,
-                      LayerTable* sensor_layer_table, MaterialTable* material_table,
+void SystemData::load(sead::Heap* heap, GroupFilter* entity_group_filter,
+                      GroupFilter* sensor_group_filter, MaterialTable* material_table,
                       ContactInfoTable* contact_info_table) {
-    loadLayerTable(heap, entity_layer_table, ContactLayerType::Entity);
-    loadLayerTable(heap, sensor_layer_table, ContactLayerType::Sensor);
+    loadLayerTable(heap, entity_group_filter, ContactLayerType::Entity);
+    loadLayerTable(heap, sensor_group_filter, ContactLayerType::Sensor);
     loadMaterialTable(heap, material_table);
     loadSubMaterialTable(heap, material_table);
     loadContactInfoTable(heap, contact_info_table, ContactLayerType::Entity);
@@ -49,6 +50,34 @@ void SystemData::load(sead::Heap* heap, LayerTable* entity_layer_table,
     loadRagdollCtrlKeyList(heap);
 }
 
+void SystemData::loadLayerTable(sead::Heap* heap, GroupFilter* filter, ContactLayerType type) {
+    auto& matrix = mLayerMatrices[int(type)];
+    matrix.addList("LayerTable");
+
+    const int first = filter->getLayerFirst();
+    const int last = filter->getLayerLast();
+    const int num_layers = 1 + last - first;
+
+    for (int i = 0; i < num_layers; ++i) {
+        auto& table = matrix.mTables[i];
+        const ContactLayer layer = first + i;
+        table.num_layers = num_layers;
+        table.filter = filter;
+        table.layer = layer;
+
+        matrix.mParamList.addObj(&table, contactLayerToText(layer));
+
+        for (int j = 0; j < num_layers; ++j) {
+            const char* other_layer_name = contactLayerToText(first + j);
+            table.layer_values[j].init(0, other_layer_name, other_layer_name, &table);
+        }
+    }
+
+    matrix.mResHandle = new (heap) res::Handle;
+    const auto res = loadLayerTableRes(matrix, type);
+    matrix.mParamIO.applyResParameterArchive(res);
+}
+
 void SystemData::loadMaterialTable(sead::Heap* heap, MaterialTable* table) {
     mMaterialTableHandle = new (heap) res::Handle;
     const auto res = loadMaterialTableRes();
@@ -95,9 +124,8 @@ void SystemData::loadRagdollCtrlKeyList(sead::Heap* heap) {
     mRagdollCtrlKeyList = sead::DynamicCast<RagdollControllerKeyList>(res);
 }
 
-agl::utl::ResParameterArchive
-SystemData::loadLayerTableRes(const SystemData::LayerTableInfoContainer& container,
-                              ContactLayerType type) {
+agl::utl::ResParameterArchive SystemData::loadLayerTableRes(const SystemData::LayerMatrix& matrix,
+                                                            ContactLayerType type) {
     res::LoadRequest request;
     request.mRequester = "physSystemData";
     const char* path{};
@@ -109,7 +137,7 @@ SystemData::loadLayerTableRes(const SystemData::LayerTableInfoContainer& contain
         path = "Physics/System/SensorLayerTable.bphyslayer";
         break;
     }
-    const auto& resource = *container.mResHandle->load(path, &request);
+    const auto& resource = *matrix.mResHandle->load(path, &request);
     auto* direct_resource = sead::DynamicCast<const sead::DirectResource>(&resource);
     return agl::utl::ResParameterArchive{direct_resource->getRawData()};
 }
@@ -159,8 +187,20 @@ agl::utl::ResParameterArchive SystemData::loadCharacterCtrlTableRes() {
     return agl::utl::ResParameterArchive{direct_resource->getRawData()};
 }
 
-void LayerTableInfo::postRead_() {
-    // FIXME
+void LayerTable::postRead_() {
+    u32 collision_mask = 0;
+    u32 custom_mask = 0;
+
+    for (int i = 0; i < num_layers; ++i) {
+        const int value = layer_values[i].ref();
+        if (value & 1)
+            collision_mask |= 1 << i;
+        if (value & 2)
+            custom_mask |= 1 << i;
+    }
+
+    filter->setLayerCollisionEnabledMask(layer, collision_mask);
+    filter->setLayerCustomMask(layer, custom_mask);
 }
 
 }  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/System/physSystemData.h b/src/KingSystem/Physics/System/physSystemData.h
index 38d7b7ff..4ee13ef7 100644
--- a/src/KingSystem/Physics/System/physSystemData.h
+++ b/src/KingSystem/Physics/System/physSystemData.h
@@ -17,22 +17,22 @@ class Handle;
 namespace ksys::phys {
 
 class ContactInfoTable;
-class LayerTable;
+class GroupFilter;
 class MaterialTable;
 class RagdollControllerKeyList;
 
 constexpr int NumLayers = 32;
 
-struct LayerTableInfo : agl::utl::IParameterObj {
-    sead::SafeArray<agl::utl::Parameter<int>, NumLayers> params;
-    void* table;  // FIXME: type GroupFilter
-    int idx = 0;  // FIXME: ContactLayer
-    int count;
+struct LayerTable : agl::utl::IParameterObj {
+    sead::SafeArray<agl::utl::Parameter<int>, NumLayers> layer_values;
+    GroupFilter* filter;
+    ContactLayer layer;
+    int num_layers;
 
 protected:
     void postRead_() override;
 };
-KSYS_CHECK_SIZE_NX150(LayerTableInfo, 0x440);
+KSYS_CHECK_SIZE_NX150(LayerTable, 0x440);
 
 struct CharacterControllerTable : agl::utl::IParameterObj {
     SEAD_ENUM(Type, Default)
@@ -63,27 +63,27 @@ public:
     SystemData();
     virtual ~SystemData();
 
-    void load(sead::Heap* heap, LayerTable* entity_layer_table, LayerTable* sensor_layer_table,
+    void load(sead::Heap* heap, GroupFilter* entity_group_filter, GroupFilter* sensor_group_filter,
               MaterialTable* material_table, ContactInfoTable* contact_info_table);
 
 private:
-    using LayerTableInfoContainer = Tables<LayerTableInfo, NumLayers>;
+    using LayerMatrix = Tables<LayerTable, NumLayers>;
 
-    void loadLayerTable(sead::Heap* heap, LayerTable* table, ContactLayerType type);
+    void loadLayerTable(sead::Heap* heap, GroupFilter* filter, ContactLayerType type);
     void loadMaterialTable(sead::Heap* heap, MaterialTable* table);
     void loadSubMaterialTable(sead::Heap* heap, MaterialTable* table);
     void loadContactInfoTable(sead::Heap* heap, ContactInfoTable* table, ContactLayerType type);
     void loadCharacterCtrlTable(sead::Heap* heap);
     void loadRagdollCtrlKeyList(sead::Heap* heap);
 
-    agl::utl::ResParameterArchive loadLayerTableRes(const LayerTableInfoContainer& container,
+    agl::utl::ResParameterArchive loadLayerTableRes(const LayerMatrix& matrix,
                                                     ContactLayerType type);
     agl::utl::ResParameterArchive loadMaterialTableRes();
     agl::utl::ResParameterArchive loadSubMaterialTableRes();
     agl::utl::ResParameterArchive loadContactInfoTableRes(ContactLayerType type);
     agl::utl::ResParameterArchive loadCharacterCtrlTableRes();
 
-    sead::SafeArray<LayerTableInfoContainer, 2> mLayerTableInfo{};
+    sead::SafeArray<LayerMatrix, 2> mLayerMatrices{};
     res::Handle* mMaterialTableHandle{};
     res::Handle* mSubMaterialTableHandle{};
     sead::SafeArray<res::Handle*, 2> mContactInfoTableHandles{};
diff --git a/src/KingSystem/Utils/HeapUtil.h b/src/KingSystem/Utils/HeapUtil.h
index b5629aa7..238bfe63 100644
--- a/src/KingSystem/Utils/HeapUtil.h
+++ b/src/KingSystem/Utils/HeapUtil.h
@@ -124,4 +124,10 @@ KSYS_ALWAYS_INLINE inline sead::Heap* tryCreateDualHeap(sead::Heap* parent) {
                              sead::Heap::cHeapDirection_Forward, false);
 }
 
+template <typename T, typename... Args>
+inline T* alloc(sead::Heap* heap, Args&&... args) {
+    void* storage = heap->alloc(sizeof(T), alignof(T));
+    return new (storage) T(std::forward<Args>(args)...);
+}
+
 }  // namespace ksys::util