ksys/phys: Add more ContactMgr functions

data/uking_functions.csv

@@ -83750,16 +83750,16 @@ Address,Quality,Size,Name
 0x0000007100fb28d4,O,000136,_ZN4ksys4phys10ContactMgr20makeContactPointInfoEPN4sead4HeapEiRKNS2_14SafeStringBaseIcEEiii
 0x0000007100fb295c,O,000220,_ZN4ksys4phys10ContactMgr25makeLayerContactPointInfoEPN4sead4HeapEiiRKNS2_14SafeStringBaseIcEEiii
 0x0000007100fb2a38,O,000100,_ZN4ksys4phys10ContactMgr24registerContactPointInfoEPNS0_20ContactPointInfoBaseE
-0x0000007100fb2a9c,U,000056,phys::ContactInfoTable::allocCollisionInfo
+0x0000007100fb2a9c,O,000056,_ZN4ksys4phys10ContactMgr17makeCollisionInfoEPN4sead4HeapERKNS2_14SafeStringBaseIcEE
 0x0000007100fb2ad4,U,000156,phys::ContactInfoTable::x_2
 0x0000007100fb2b70,O,000148,_ZN4ksys4phys10ContactMgr20freeContactPointInfoEPNS0_20ContactPointInfoBaseE
-0x0000007100fb2c04,U,000144,phys::ContactInfoTable::x_5
+0x0000007100fb2c04,O,000144,_ZN4ksys4phys10ContactMgr17freeCollisionInfoEPNS0_13CollisionInfoE
-0x0000007100fb2c94,U,000216,phys::ContactInfoTable::x_6
+0x0000007100fb2c94,O,000216,_ZN4ksys4phys10ContactMgr21clearCollisionEntriesEPNS0_13CollisionInfoE
 0x0000007100fb2d6c,U,000136,phys::ContactInfoTable::x_7
 0x0000007100fb2df4,U,000312,phys::ContactInfoTable::x_8
-0x0000007100fb2f2c,U,000120,phys::ContactInfoTable::x_9
+0x0000007100fb2f2c,m,000120,_ZN4ksys4phys10ContactMgr18clearContactPointsEv
-0x0000007100fb2fa4,U,000152,phys::ContactInfoTable::x_10
+0x0000007100fb2fa4,O,000152,_ZN4ksys4phys10ContactMgr27removeContactPointsWithBodyEPNS0_9RigidBodyE
-0x0000007100fb303c,U,000380,phys::ContactInfoTable::x_11
+0x0000007100fb303c,O,000380,_ZN4ksys4phys10ContactMgr30removeCollisionEntriesWithBodyEPNS0_9RigidBodyE
 0x0000007100fb31b8,U,000204,phys::ContactInfoTable::x_12
 0x0000007100fb3284,O,000500,_ZN4ksys4phys10ContactMgr20registerContactPointEPNS0_16ContactPointInfoERKNS0_12ContactPointERKNS0_23RigidBodyCollisionMasksEb
 0x0000007100fb3478,O,000340,_ZN4ksys4phys10ContactMgr20registerContactPointEPNS0_21LayerContactPointInfoERKNS0_12ContactPointEb
@@ -83769,9 +83769,9 @@ Address,Quality,Size,Name
 0x0000007100fb3854,O,000228,_ZN4ksys4phys10ContactMgr19unregisterCollisionEPNS0_13CollisionInfoEPNS0_9RigidBodyES5_
 0x0000007100fb3938,O,000244,_ZN4ksys4phys10ContactMgr19unregisterCollisionEPNS0_25ContactLayerCollisionInfoEPNS0_9RigidBodyES5_
 0x0000007100fb3a2c,O,000292,_ZN4ksys4phys10ContactMgr27unregisterCollisionWithBodyEPNS0_25ContactLayerCollisionInfoEPNS0_9RigidBodyE
-0x0000007100fb3b50,U,000100,phys::ContactInfoTable::x_22
+0x0000007100fb3b50,O,000100,_ZN4ksys4phys10ContactMgr21registerCollisionInfoEPNS0_13CollisionInfoE
-0x0000007100fb3bb4,U,000652,phys::ContactInfoTable::getLayerMaskForContactPoints
+0x0000007100fb3bb4,O,000652,_ZNK4ksys4phys10ContactMgr14initLayerMasksEPNS0_16ContactPointInfoERKN4sead14SafeStringBaseIcEE
-0x0000007100fb3e40,U,000644,phys::ContactInfoTable::getLayerMaskForCollisionInfoStuff
+0x0000007100fb3e40,O,000644,_ZNK4ksys4phys10ContactMgr14initLayerMasksEPNS0_13CollisionInfoERKN4sead14SafeStringBaseIcEE
 0x0000007100fb40c4,O,000356,_ZNK4ksys4phys10ContactMgr18getSensorLayerMaskEPNS0_19SensorCollisionMaskERKN4sead14SafeStringBaseIcEE
 0x0000007100fb4228,U,000204,phys::ContactInfoTable::x_26
 0x0000007100fb42f4,U,001092,phys::ContactInfoTable::x_27
@@ -84314,7 +84314,7 @@ Address,Quality,Size,Name
 0x0000007100fcf404,O,000112,_ZN4ksys4phys15ContactListener35removeLayerPairsForContactPointInfoEPNS0_21LayerContactPointInfoE
 0x0000007100fcf474,m,000576,_ZN4ksys4phys15ContactListener34removeLayerPairForContactPointInfoEPNS0_21LayerContactPointInfoENS0_12ContactLayerES4_
 0x0000007100fcf6b4,O,000124,_ZN4ksys4phys15ContactListener14trackLayerPairENS0_12ContactLayerES2_
-0x0000007100fcf730,O,000156,_ZN4ksys4phys15ContactListener17registerRigidBodyEPNS0_9RigidBodyE
+0x0000007100fcf730,O,000156,_ZN4ksys4phys15ContactListener27unregisterCollisionWithBodyEPNS0_9RigidBodyE
 0x0000007100fcf7cc,O,000024,_ZN4ksys4phys15ContactListener24contactPointCallbackImplEjjPNS0_9RigidBodyES3_NS0_12ContactLayerES4_RK20hkpContactPointEvent
 0x0000007100fcf7e4,O,000112,_ZNK4ksys4phys15ContactListener27checkDerivedRuntimeTypeInfoEPKN4sead15RuntimeTypeInfo9InterfaceE
 0x0000007100fcf854,O,000092,_ZNK4ksys4phys15ContactListener18getRuntimeTypeInfoEv

src/KingSystem/Physics/System/physContactListener.cpp

@@ -491,7 +491,7 @@ void ContactListener::removeLayerPairForContactPointInfo(LayerContactPointInfo*
     }
 }
 
-void ContactListener::registerRigidBody(RigidBody* body) {
+void ContactListener::unregisterCollisionWithBody(RigidBody* body) {
     const u32 rlayer = body->getContactLayer() - mLayerBase;
     auto& column = mCollisionInfoPerLayerPair[int(rlayer)];
     for (u32 i = 0; i < mLayerCount; ++i) {

src/KingSystem/Physics/System/physContactListener.h

@@ -37,7 +37,7 @@ public:
     void removeLayerPairForContactPointInfo(LayerContactPointInfo* info, ContactLayer layer1,
                                             ContactLayer layer2);
 
-    void registerRigidBody(RigidBody* body);
+    void unregisterCollisionWithBody(RigidBody* body);
 
     void contactPointCallback(const hkpContactPointEvent& event) override;
     void collisionAddedCallback(const hkpCollisionEvent& event) override;

src/KingSystem/Physics/System/physContactMgr.cpp

@@ -14,7 +14,7 @@ namespace ksys::phys {
 ContactMgr::ContactMgr() {
     mContactPointInfoInstances.initOffset(ContactPointInfo::getListNodeOffset());
     // FIXME: figure out what these offsets are
-    mList2.initOffset(0x78);
+    mCollisionInfoInstances.initOffset(CollisionInfo::getListNodeOffset());
     mList3.initOffset(0x40);
     mCollidingBodiesFreeList.initOffset(CollidingBodies::getListNodeOffset());
     mList4.initOffset(0x48);
@@ -96,6 +96,10 @@ LayerContactPointInfo* ContactMgr::makeLayerContactPointInfo(sead::Heap* heap, i
     return info;
 }
 
+CollisionInfo* ContactMgr::makeCollisionInfo(sead::Heap* heap, const sead::SafeString& name) {
+    return new (heap) CollisionInfo(name);
+}
+
 void ContactMgr::registerContactPointInfo(ContactPointInfoBase* info) {
     auto lock = sead::makeScopedLock(mContactPointInfoMutex);
     if (!info->isLinked())
@@ -117,13 +121,80 @@ void ContactMgr::freeContactPointInfo(ContactPointInfoBase* info) {
     delete info;
 }
 
+void ContactMgr::registerCollisionInfo(CollisionInfo* info) {
+    auto lock = sead::makeScopedLock(mCollisionInfoMutex);
+    if (!info->isLinked())
+        mCollisionInfoInstances.pushBack(info);
+}
+
+void ContactMgr::unregisterCollisionInfo(CollisionInfo* info) {
+    auto lock = sead::makeScopedLock(mCollisionInfoMutex);
+    if (info->isLinked())
+        mCollisionInfoInstances.erase(info);
+}
+
+void ContactMgr::freeCollisionInfo(CollisionInfo* info) {
+    if (!info)
+        return;
+
+    clearCollisionEntries(info);
+    unregisterCollisionInfo(info);
+    delete info;
+}
+
+// NON_MATCHING: two sub instructions reordered
+void ContactMgr::clearContactPoints() {
+    auto lock = sead::makeScopedLock(mContactPointInfoMutex);
+
+    if (mNumContactPoints != 0)
+        mNumContactPoints = 0;
+
+    for (auto& info : mContactPointInfoInstances) {
+        if (info.mNumContactPoints != 0)
+            info.mNumContactPoints = 0;
+    }
+}
+
+void ContactMgr::removeContactPointsWithBody(RigidBody* body) {
+    auto lock = sead::makeScopedLock(mContactPointInfoMutex);
+    for (int i = 0; i < mNumContactPoints; ++i) {
+        ContactPoint& point = mContactPointPool[i];
+        if (point.body_a == body || point.body_b == body)
+            point.flags.set(ContactPoint::Flag::Invalid);
+    }
+}
+
+void ContactMgr::removeCollisionEntriesWithBody(RigidBody* body) {
+    auto lock_all_ci = sead::makeScopedLock(mCollisionInfoMutex);
+    auto lock_all_cb = sead::makeScopedLock(mCollidingBodiesMutex);
+
+    const auto body_layer = body->getContactLayer();
+
+    for (auto& collision_info : mCollisionInfoInstances) {
+        auto lock_ci = sead::makeScopedLock(collision_info);
+
+        // Small optimisation: there's no need to check all colliding bodies in this CI
+        // if we know we will never be able to find the body in this CI because the layers mismatch.
+        if (!collision_info.isLayerEnabled(body_layer))
+            continue;
+
+        for (auto& colliding_pair : collision_info.getCollidingBodies().robustRange()) {
+            if (colliding_pair.body_b != body)
+                continue;
+
+            collision_info.getCollidingBodies().erase(&colliding_pair);
+            mCollidingBodiesFreeList.pushBack(&colliding_pair);
+        }
+    }
+}
+
 int ContactMgr::allocateContactPoint() {
-    if (mContactPointIndex >= mContactPointPool.size() - 1) {
+    if (mNumContactPoints >= mContactPointPool.size() - 1) {
-        util::PrintDebugFmt("contact point pool exhausted (current index: %d)",
+        util::PrintDebugFmt("contact point pool exhausted (current number of points: %d)",
-                            mContactPointIndex.load());
+                            mNumContactPoints.load());
         return -1;
     }
-    return mContactPointIndex.increment();
+    return mNumContactPoints.increment();
 }
 
 bool ContactMgr::registerContactPoint(ContactPointInfo* info, const ContactPoint& point,
@@ -177,6 +248,49 @@ void ContactMgr::registerContactPoint(LayerContactPointInfo* info, const Contact
     }
 }
 
+bool ContactMgr::initLayerMasks(ContactPointInfo* info,
+                                const sead::SafeString& receiver_name) const {
+    for (int type = 0; type < mContactInfoTables.size(); ++type) {
+        const auto& receivers = mContactInfoTables[type].receivers;
+        for (int i = 0; i < receivers.size(); ++i) {
+            const auto& receiver = receivers[i];
+            if (receiver_name == receiver.name) {
+                info->mSubscribedLayers[type] = receiver.layer_mask;
+                info->mLayerMask2[type] = receiver.layer_mask2;
+                return true;
+            }
+        }
+    }
+    return false;
+}
+
+bool ContactMgr::initLayerMasks(CollisionInfo* info, const sead::SafeString& receiver_name) const {
+    for (int type = 0; type < mContactInfoTables.size(); ++type) {
+        const auto& receivers = mContactInfoTables[type].receivers;
+        for (int i = 0; i < receivers.size(); ++i) {
+            const auto& receiver = receivers[i];
+            if (receiver_name == receiver.name) {
+                info->getLayerMask(ContactLayerType(type)) = receiver.layer_mask;
+                return true;
+            }
+        }
+    }
+    return false;
+}
+
+bool ContactMgr::getSensorLayerMask(SensorCollisionMask* mask,
+                                    const sead::SafeString& receiver_name) const {
+    const auto& receivers = mContactInfoTables[int(ContactLayerType::Sensor)].receivers;
+    for (int i = 0; i < receivers.size(); ++i) {
+        const auto& receiver = receivers[i];
+        if (receiver_name == receiver.name) {
+            receiverMaskSetSensorLayerMask(mask, receiver.layer_mask);
+            return true;
+        }
+    }
+    return false;
+}
+
 void ContactMgr::registerCollision(CollisionInfo* info, RigidBody* body_a, RigidBody* body_b) {
     auto lock = sead::makeScopedLock(mCollidingBodiesMutex);
     if (!info->isLinked())
@@ -245,17 +359,14 @@ void ContactMgr::unregisterCollisionWithBody(ContactLayerCollisionInfo* info, Ri
     }
 }
 
-bool ContactMgr::getSensorLayerMask(SensorCollisionMask* mask,
+void ContactMgr::clearCollisionEntries(CollisionInfo* info) {
-                                    const sead::SafeString& receiver_type) const {
+    auto lock = sead::makeScopedLock(mCollidingBodiesMutex);
-    const auto& receivers = mContactInfoTables[int(ContactLayerType::Sensor)].receivers;
+    auto info_lock = sead::makeScopedLock(*info);
-    for (int i = 0; i < receivers.size(); ++i) {
+
-        const auto& receiver = receivers[i];
+    for (auto& entry : info->getCollidingBodies().robustRange()) {
-        if (receiver_type == receiver.name) {
+        info->getCollidingBodies().erase(&entry);
-            receiverMaskSetSensorLayerMask(mask, receiver.layer_mask);
+        mCollidingBodiesFreeList.pushBack(&entry);
-            return true;
-        }
     }
-    return false;
 }
 
 void ContactInfoTable::Receiver::postRead_() {

src/KingSystem/Physics/System/physContactMgr.h

@@ -54,7 +54,9 @@ struct ContactInfoTable {
 
 struct ContactPoint {
     enum class Flag {
-        _1 = 1 << 0,
+        /// Whether this contact point should be skipped when iterating over contact points.
+        Invalid = 1 << 0,
+        /// Whether the two bodies are penetrating.
         Penetrating = 1 << 1,
     };
 
@@ -84,7 +86,7 @@ public:
     void loadContactInfoTable(sead::Heap* heap, agl::utl::ResParameterArchive archive,
                               ContactLayerType type);
 
-    bool getSensorLayerMask(SensorCollisionMask* mask, const sead::SafeString& receiver_type) const;
+    // region Factories for collision tracking structures
 
     ContactPointInfo* makeContactPointInfo(sead::Heap* heap, int num, const sead::SafeString& name,
                                            int a, int b, int c);
@@ -93,16 +95,37 @@ public:
                                                      const sead::SafeString& name, int a, int b,
                                                      int c);
 
+    CollisionInfo* makeCollisionInfo(sead::Heap* heap, const sead::SafeString& name);
+
+    // endregion
+
     void registerContactPointInfo(ContactPointInfoBase* info);
     void unregisterContactPointInfo(ContactPointInfoBase* info);
     void freeContactPointInfo(ContactPointInfoBase* info);
 
+    void registerCollisionInfo(CollisionInfo* info);
+    void unregisterCollisionInfo(CollisionInfo* info);
+    void freeCollisionInfo(CollisionInfo* info);
+
+    void clearContactPoints();
+    /// Remove all contact points with the specified rigid body.
+    /// @note For efficiency reasons, this actually only invalidates the contact points.
+    /// They will be removed at a later stage.
+    void removeContactPointsWithBody(RigidBody* body);
+    void removeCollisionEntriesWithBody(RigidBody* body);
+    // TODO: figure out what this removes
+    // 0x0000007100fb31b8
+    void removeUnkWithBody(RigidBody* body);
+
     /// @param colliding_body_masks The collision masks of the colliding rigid bodies.
     /// @returns whether contact should be disabled.
     bool registerContactPoint(ContactPointInfo* info, const ContactPoint& point,
                               const RigidBodyCollisionMasks& colliding_body_masks,
                               bool penetrating);
 
+    // 0x0000007100fb35cc
+    void registerContactPoint(void* unk_info, const ContactPoint& point, bool penetrating);
+
     void registerContactPoint(LayerContactPointInfo* info, const ContactPoint& point,
                               bool penetrating);
 
@@ -113,6 +136,24 @@ public:
     /// Unregister all collision pairs involving the specified rigid body.
     void unregisterCollisionWithBody(ContactLayerCollisionInfo* info, RigidBody* body);
 
+    void clearCollisionEntries(CollisionInfo* info);
+
+    bool initLayerMasks(ContactPointInfo* info, const sead::SafeString& receiver_name) const;
+    bool initLayerMasks(CollisionInfo* info, const sead::SafeString& receiver_name) const;
+    bool getSensorLayerMask(SensorCollisionMask* mask, const sead::SafeString& receiver_name) const;
+
+    // 0x0000007100fb4228
+    void x_26(RigidBody* body_a, RigidBody* body_b);
+    // 0x0000007100fb42f4
+    void x_27(RigidBody* body_a, RigidBody* body_b, const sead::Vector3f&, const sead::Vector3f&,
+              u32 material_a, u32 material_b);
+    // 0x0000007100fb4738
+    void x_28(void* unk);
+    // 0x0000007100fb49d4
+    void x_14();
+    // 0x0000007100fb4a90
+    void x_29();
+
 private:
     void doLoadContactInfoTable(agl::utl::ResParameterArchive archive, ContactLayerType type,
                                 bool skip_params);
@@ -129,16 +170,16 @@ private:
     sead::Buffer<ContactPoint> mContactPointPool;
     sead::OffsetList<CollidingBodies> mCollidingBodiesFreeList;
     int mList0Size = 0;
-    /// The index of the next free ContactPoint in the pool.
+    /// The number of contact points. Also the iindex of the next free ContactPoint in the pool.
-    sead::Atomic<int> mContactPointIndex = 0;
+    sead::Atomic<int> mNumContactPoints = 0;
     sead::OffsetList<ContactPointInfoBase> mContactPointInfoInstances;
-    sead::OffsetList<void*> mList2;
+    sead::OffsetList<CollisionInfo> mCollisionInfoInstances;
     sead::OffsetList<void*> mList3;
     sead::OffsetList<void*> mList4;
     sead::OffsetList<void*> mList5;
     // TODO: rename these members
     sead::Mutex mContactPointInfoMutex;
-    sead::Mutex mMutex2;
+    sead::Mutex mCollisionInfoMutex;
     sead::Mutex mMutex3;
     sead::Mutex mCollidingBodiesMutex;
     sead::Mutex mMutex5;

src/KingSystem/Physics/System/physContactPointInfo.cpp

@@ -29,7 +29,7 @@ void ContactPointInfo::freePoints() {
 ContactPointInfo::Iterator::Iterator(const Points& points, int count)
     : mPoints(points.getBufferPtr()), mPointsNum(count), mPointsStart(points.getBufferPtr()) {
     for (int i = 0; i != count; ++i) {
-        if (!mPoints[i]->flags.isOn(ContactPoint::Flag::_1))
+        if (!mPoints[i]->flags.isOn(ContactPoint::Flag::Invalid))
             break;
         ++mIdx;
     }

src/KingSystem/Physics/System/physLayerContactPointInfo.cpp

@@ -96,7 +96,7 @@ sead::Vector3f LayerContactPointInfo::Iterator::getPointPosition(Point point) co
 LayerContactPointInfo::Iterator::Iterator(const LayerContactPointInfo::Points& points, int count)
     : mPoints(points.getBufferPtr()), mPointsNum(count), mPointsStart(points.getBufferPtr()) {
     for (int i = 0; i != count; ++i) {
-        if (!mPoints[i]->flags.isOn(ContactPoint::Flag::_1))
+        if (!mPoints[i]->flags.isOn(ContactPoint::Flag::Invalid))
             break;
         ++mIdx;
     }