ksys/phys: Finish ContactMgr

3 files changed, 62 insertions, 5 deletions

diff --git a/src/KingSystem/Physics/System/physContactLayerCollisionInfoGroup.h b/src/KingSystem/Physics/System/physContactLayerCollisionInfoGroup.h
index 59cff64e..69f0bbec 100644
--- a/src/KingSystem/Physics/System/physContactLayerCollisionInfoGroup.h
+++ b/src/KingSystem/Physics/System/physContactLayerCollisionInfoGroup.h
@@ -48,6 +48,10 @@ public:
     CollidingBodiesIterator collidingBodiesEnd() const;
     CollidingBodiesRange getCollidingBodies() const;
 
+    const sead::PtrArray<ContactLayerCollisionInfo>& getCollisionInfo() const {
+        return mCollisionInfoInstances;
+    }
+
     static constexpr size_t getListNodeOffset() {
         return offsetof(ContactLayerCollisionInfoGroup, mListNode);
     }
diff --git a/src/KingSystem/Physics/System/physContactMgr.cpp b/src/KingSystem/Physics/System/physContactMgr.cpp
index a467736c..ccc25385 100644
--- a/src/KingSystem/Physics/System/physContactMgr.cpp
+++ b/src/KingSystem/Physics/System/physContactMgr.cpp
@@ -5,6 +5,7 @@
 #include "KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h"
 #include "KingSystem/Physics/System/physCollisionInfo.h"
 #include "KingSystem/Physics/System/physContactLayerCollisionInfo.h"
+#include "KingSystem/Physics/System/physContactLayerCollisionInfoGroup.h"
 #include "KingSystem/Physics/System/physContactPointInfo.h"
 #include "KingSystem/Physics/System/physEntityGroupFilter.h"
 #include "KingSystem/Physics/System/physGroupFilter.h"
@@ -33,8 +34,7 @@ struct ContactMgr::ImpulseEntry {
 ContactMgr::ContactMgr() {
     mContactPointInfoInstances.initOffset(ContactPointInfo::getListNodeOffset());
     mCollisionInfoInstances.initOffset(CollisionInfo::getListNodeOffset());
-    // FIXME: figure out what this offset is
-    mList3.initOffset(0x40);
+    mLayerColInfoGroups.initOffset(ContactLayerCollisionInfoGroup::getListNodeOffset());
     mCollidingBodiesFreeList.initOffset(CollidingBodies::getListNodeOffset());
     mImpulseEntriesFreeList.initOffset(ImpulseEntry::getListNodeOffset());
     mImpulseEntries.initOffset(ImpulseEntry::getListNodeOffset());
@@ -135,6 +135,15 @@ CollisionInfo* ContactMgr::makeCollisionInfo(sead::Heap* heap, const sead::SafeS
     return new (heap) CollisionInfo(name);
 }
 
+ContactLayerCollisionInfoGroup*
+ContactMgr::makeContactLayerCollisionInfoGroup(sead::Heap* heap, ContactLayer layer, int capacity,
+                                               const sead::SafeString& name) {
+    auto* group = new (heap) ContactLayerCollisionInfoGroup(layer, name);
+    group->init(heap, capacity);
+    registerContactLayerCollisionInfoGroup(group);
+    return group;
+}
+
 void ContactMgr::registerContactPointInfo(ContactPointInfoBase* info) {
     auto lock = sead::makeScopedLock(mContactPointInfoMutex);
     if (!info->isLinked())
@@ -177,6 +186,27 @@ void ContactMgr::freeCollisionInfo(CollisionInfo* info) {
     delete info;
 }
 
+void ContactMgr::registerContactLayerCollisionInfoGroup(ContactLayerCollisionInfoGroup* group) {
+    auto lock = sead::makeScopedLock(mLayerColInfoGroupMutex);
+    mLayerColInfoGroups.pushBack(group);
+}
+
+void ContactMgr::unregisterContactLayerCollisionInfoGroup(ContactLayerCollisionInfoGroup* group) {
+    auto lock = sead::makeScopedLock(mLayerColInfoGroupMutex);
+    mLayerColInfoGroups.erase(group);
+}
+
+void ContactMgr::freeContactLayerCollisionInfoGroup(ContactLayerCollisionInfoGroup* group) {
+    if (!group)
+        return;
+
+    clearCollisionEntries(group);
+    unregisterContactLayerCollisionInfoGroup(group);
+    group->finalize();
+
+    delete group;
+}
+
 // NON_MATCHING: two sub instructions reordered
 void ContactMgr::clearContactPoints() {
     auto lock = sead::makeScopedLock(mContactPointInfoMutex);
@@ -434,6 +464,20 @@ void ContactMgr::clearCollisionEntries(CollisionInfo* info) {
     }
 }
 
+void ContactMgr::clearCollisionEntries(ContactLayerCollisionInfoGroup* group) {
+    auto lock = sead::makeScopedLock(mCollidingBodiesMutex);
+
+    for (int i = 0; i < group->getCollisionInfo().size(); ++i) {
+        ContactLayerCollisionInfo* info = group->getCollisionInfo()[i];
+        auto info_lock = sead::makeScopedLock(*info);
+
+        for (auto& entry : info->getCollidingBodies().robustRange()) {
+            info->getCollidingBodies().erase(&entry);
+            freeCollidingBodiesEntry(&entry);
+        }
+    }
+}
+
 void ContactMgr::addImpulseEntry(RigidBody* body_a, RigidBody* body_b) {
     auto lock = sead::makeScopedLock(mImpulseEntriesMutex);
 
diff --git a/src/KingSystem/Physics/System/physContactMgr.h b/src/KingSystem/Physics/System/physContactMgr.h
index 4a8a8d9b..a5353f9b 100644
--- a/src/KingSystem/Physics/System/physContactMgr.h
+++ b/src/KingSystem/Physics/System/physContactMgr.h
@@ -28,6 +28,7 @@ enum class IsIndoorStage;
 struct CollidingBodies;
 class CollisionInfo;
 class ContactLayerCollisionInfo;
+class ContactLayerCollisionInfoGroup;
 class ContactPointInfoBase;
 class QueryContactPointInfo;
 class RigidBody;
@@ -100,6 +101,10 @@ public:
 
     CollisionInfo* makeCollisionInfo(sead::Heap* heap, const sead::SafeString& name);
 
+    ContactLayerCollisionInfoGroup*
+    makeContactLayerCollisionInfoGroup(sead::Heap* heap, ContactLayer layer, int capacity,
+                                       const sead::SafeString& name);
+
     // endregion
 
     void registerContactPointInfo(ContactPointInfoBase* info);
@@ -110,6 +115,10 @@ public:
     void unregisterCollisionInfo(CollisionInfo* info);
     void freeCollisionInfo(CollisionInfo* info);
 
+    void registerContactLayerCollisionInfoGroup(ContactLayerCollisionInfoGroup* group);
+    void unregisterContactLayerCollisionInfoGroup(ContactLayerCollisionInfoGroup* group);
+    void freeContactLayerCollisionInfoGroup(ContactLayerCollisionInfoGroup* group);
+
     void clearContactPoints();
     /// Remove all contact points with the specified rigid body.
     /// @note For efficiency reasons, this actually only invalidates the contact points.
@@ -138,6 +147,7 @@ public:
     void unregisterCollisionWithBody(ContactLayerCollisionInfo* info, RigidBody* body);
 
     void clearCollisionEntries(CollisionInfo* info);
+    void clearCollisionEntries(ContactLayerCollisionInfoGroup* group);
 
     bool initLayerMasks(ContactPointInfo* info, const sead::SafeString& receiver_name) const;
     bool initLayerMasks(CollisionInfo* info, const sead::SafeString& receiver_name) const;
@@ -180,13 +190,12 @@ private:
     sead::Atomic<int> mNumContactPoints = 0;
     sead::OffsetList<ContactPointInfoBase> mContactPointInfoInstances;
     sead::OffsetList<CollisionInfo> mCollisionInfoInstances;
-    sead::OffsetList<void*> mList3;
+    sead::OffsetList<ContactLayerCollisionInfoGroup> mLayerColInfoGroups;
     sead::OffsetList<ImpulseEntry> mImpulseEntriesFreeList;
     sead::OffsetList<ImpulseEntry> mImpulseEntries;
     sead::Mutex mContactPointInfoMutex;
     sead::Mutex mCollisionInfoMutex;
-    // TODO: rename mList3 and mMutex3
-    sead::Mutex mMutex3;
+    sead::Mutex mLayerColInfoGroupMutex;
     sead::Mutex mCollidingBodiesMutex;
     sead::Mutex mImpulseEntriesMutex;
     sead::SafeArray<ContactInfoTable, 2> mContactInfoTables{};