ksys/phys: Add more ContactListener functions and Havok prereqs

src/KingSystem/Physics/System/physCollisionInfo.h

@@ -18,6 +18,8 @@ public:
     sead::BitFlag32& getLayerMask(ContactLayerType layer_type);
     const sead::BitFlag32& getLayerMask(ContactLayerType layer_type) const;
 
+    bool isLayerEnabled(ContactLayer layer) const;
+
     void lock();
     void unlock();
 
@@ -53,6 +55,11 @@ inline const sead::BitFlag32& CollisionInfoBase::getLayerMask(ContactLayerType l
     return mLayerMasks[int(layer_type)];
 }
 
+inline bool CollisionInfoBase::isLayerEnabled(ContactLayer layer) const {
+    const auto& mask = getLayerMask(getContactLayerType(layer));
+    return mask.isOnBit(int(getContactLayerBaseRelativeValue(layer)));
+}
+
 inline void CollisionInfoBase::lock() {
     mMutex.lock();
 }

src/KingSystem/Physics/System/physContactListener.cpp

@@ -1,14 +1,20 @@
 #include "KingSystem/Physics/System/physContactListener.h"
 #include <Havok/Common/Base/Types/Physics/ContactPoint/hkContactPoint.h>
+#include <Havok/Physics2012/Collide/Agent/ContactMgr/hkpContactMgr.h>
 #include <Havok/Physics2012/Dynamics/Collide/ContactListener/hkpCollisionEvent.h>
 #include <Havok/Physics2012/Dynamics/Collide/ContactListener/hkpContactPointEvent.h>
+#include <Havok/Physics2012/Dynamics/Constraint/Contact/hkpContactPointProperties.h>
 #include <Havok/Physics2012/Utilities/CharacterControl/CharacterRigidBody/hkpCharacterRigidBody.h>
 #include <math/seadMathCalcCommon.h>
 #include <prim/seadMemUtil.h>
+#include <prim/seadScopedLock.h>
 #include "KingSystem/Physics/RigidBody/physRigidBody.h"
 #include "KingSystem/Physics/System/physContactLayerCollisionInfo.h"
 #include "KingSystem/Physics/System/physContactMgr.h"
+#include "KingSystem/Physics/System/physGroupFilter.h"
+#include "KingSystem/Physics/System/physLayerContactPointInfo.h"
 #include "KingSystem/Physics/System/physSystem.h"
+#include "KingSystem/Physics/physConversions.h"
 
 namespace ksys::phys {
 
@@ -25,9 +31,9 @@ ContactListener::~ContactListener() = default;
 
 void ContactListener::init(sead::Heap* heap) {
     // NOLINTBEGIN(cppcoreguidelines-narrowing-conversions)
-    _20.allocBufferAssert(mLayerCount, nullptr);
+    mTrackedContactPointLayers.allocBufferAssert(mLayerCount, nullptr);
     for (u32 i = 0; i < mLayerCount; ++i) {
-        _20[i].allocBufferAssert(mLayerCount, nullptr);
+        mTrackedContactPointLayers[i].allocBufferAssert(mLayerCount, nullptr);
     }
 
     mCollisionInfoPerLayerPair.allocBufferAssert(mLayerCount, nullptr);
@@ -45,13 +51,13 @@ void ContactListener::init(sead::Heap* heap) {
     }
     // NOLINTEND(cppcoreguidelines-narrowing-conversions)
 
-    _48 = sead::Mathu::roundUpPow2(mLayerCount * mLayerCount, 0x20);
-    _40 = ::operator new[](_48, heap, 0x20);
+    mTrackedLayersBufferSize = sead::Mathu::roundUpPow2(mLayerCount * mLayerCount, 0x20);
+    mTrackedLayers = new (heap, 0x20) u8[mTrackedLayersBufferSize];
     clearTable();
 }
 
 void ContactListener::clearTable() {
-    sead::MemUtil::fillZero(_40, _48);
+    sead::MemUtil::fillZero(mTrackedLayers, mTrackedLayersBufferSize);
 }
 
 void ContactListener::collisionAddedCallback(const hkpCollisionEvent& event) {
@@ -88,7 +94,98 @@ void ContactListener::contactPointCallback(const hkpContactPointEvent& event) {
     } else if (event.m_type == hkpContactPointEvent::TYPE_MANIFOLD) {
         manifoldContactPointCallback(event, body_a, body_b);
     } else {
-        regularContactPointCallback(event, body_a, body_b);
+        regularContactPointCallback(event, body_a, body_b, nullptr);
+    }
+}
+
+bool ContactListener::manifoldContactPointCallback(const hkpContactPointEvent& event,
+                                                   RigidBody* body_a, RigidBody* body_b) {
+    auto* filter = System::instance()->getGroupFilter(mLayerType);
+
+    RigidBody::CollisionMasks masks_a, masks_b;
+    sead::Vector3f contact_point_pos, position, normal;
+    ContactPointInfo::Event my_event;
+
+    storeToVec3(&contact_point_pos, event.m_contactPoint->getPosition());
+
+    body_a->getCollisionMasks(&masks_a, event.getShapeKeys(0), contact_point_pos);
+    body_b->getCollisionMasks(&masks_b, event.getShapeKeys(1), contact_point_pos);
+
+    const auto layer_a = filter->getCollisionFilterInfoLayer(masks_a.collision_filter_info);
+    const auto layer_b = filter->getCollisionFilterInfoLayer(masks_b.collision_filter_info);
+
+    if (!characterControlContactPointCallback(masks_a.ignored_layers, masks_b.ignored_layers,
+                                              body_a, body_b, layer_a, layer_b, event)) {
+        return false;
+    }
+
+    if (auto* info = body_a->getContactPointInfo(); info && info->getContactCallback()) {
+        storeToVec3(&position, event.m_contactPoint->getPosition());
+        storeToVec3(&normal, event.m_contactPoint->getSeparatingNormal());
+        normal *= -1;
+        my_event.body = body_b;
+        my_event.position = &position;
+        my_event.separating_normal = &normal;
+        my_event.collision_masks = &masks_b;
+
+        auto disable = ContactPointInfo::ShouldDisableContact::No;
+
+        info->getContactCallback()->invoke(&disable, my_event);
+
+        if (disable == ContactPointInfo::ShouldDisableContact::Yes) {
+            event.m_contactPointProperties->m_flags |=
+                hkpContactPointProperties::CONTACT_IS_DISABLED;
+            return false;
+        }
+    }
+
+    if (auto* info = body_b->getContactPointInfo(); info && info->getContactCallback()) {
+        storeToVec3(&position, event.m_contactPoint->getPosition());
+        storeToVec3(&normal, event.m_contactPoint->getSeparatingNormal());
+        my_event.body = body_a;
+        my_event.position = &position;
+        my_event.separating_normal = &normal;
+        my_event.collision_masks = &masks_a;
+
+        auto disable = ContactPointInfo::ShouldDisableContact::No;
+
+        info->getContactCallback()->invoke(&disable, my_event);
+
+        if (disable == ContactPointInfo::ShouldDisableContact::Yes) {
+            event.m_contactPointProperties->m_flags |=
+                hkpContactPointProperties::CONTACT_IS_DISABLED;
+            return false;
+        }
+    }
+
+    return true;
+}
+
+void ContactListener::handleCollisionAdded(const hkpCollisionEvent& event, RigidBody* body_a,
+                                           RigidBody* body_b) {
+    registerForEndOfStepContactPointCallbacks(event);
+
+    const auto layer_a = body_a->getContactLayer();
+    const auto layer_b = body_b->getContactLayer();
+
+    if (body_a->getCollisionInfo() && body_a->getCollisionInfo()->isLayerEnabled(layer_b))
+        mMgr->x_17(body_a->getCollisionInfo(), body_a, body_b);
+
+    if (body_b->getCollisionInfo() && body_b->getCollisionInfo()->isLayerEnabled(layer_a))
+        mMgr->x_17(body_b->getCollisionInfo(), body_b, body_a);
+
+    const auto i = int(layer_a - mLayerBase);
+    const auto j = int(layer_b - mLayerBase);
+    if (areContactsTrackedForLayerPair(i, j)) {
+        auto* info = getContactLayerCollisionInfo(i, j);
+        if (body_a->isFlag8Set() && body_b->isFlag8Set()) {
+            const auto layer_a_ = int(layer_a);
+            const auto tracked_layer = info->getLayer();
+            const bool body_a_first = layer_a_ == tracked_layer;
+            auto* body1 = body_a_first ? body_a : body_b;
+            auto* body2 = body_a_first ? body_b : body_a;
+            mMgr->x_18(info, body1, body2);
+        }
     }
 }
 
@@ -99,15 +196,15 @@ void ContactListener::handleCollisionRemoved(const hkpCollisionEvent& event, Rig
     const auto layer_a = body_a->getContactLayer();
     const auto layer_b = body_b->getContactLayer();
 
-    if (auto* unk = body_a->getCollisionInfo())
-        mMgr->x_19(unk, body_a, body_b);
+    if (auto* info = body_a->getCollisionInfo())
+        mMgr->x_19(info, body_a, body_b);
 
-    if (auto* unk = body_b->getCollisionInfo())
-        mMgr->x_19(unk, body_b, body_a);
+    if (auto* info = body_b->getCollisionInfo())
+        mMgr->x_19(info, body_b, body_a);
 
     const auto i = int(layer_a - mLayerBase);
     const auto j = int(layer_b - mLayerBase);
-    ContactLayerCollisionInfo* info = mCollisionInfoPerLayerPair[i][j];
+    auto* info = getContactLayerCollisionInfo(i, j);
     if (!info->getList().isEmpty()) {
         const auto layer_a_ = int(layer_a);
         const auto tracked_layer = info->getLayer();
@@ -118,4 +215,91 @@ void ContactListener::handleCollisionRemoved(const hkpCollisionEvent& event, Rig
     }
 }
 
+bool ContactListener::areContactsTrackedForLayerPair(u32 rlayer_a, u32 rlayer_b) const {
+    return mTrackedLayers[mLayerCount * rlayer_a + rlayer_b];
+}
+
+ContactLayerCollisionInfo* ContactListener::getContactLayerCollisionInfo(u32 rlayer_a,
+                                                                         u32 rlayer_b) {
+    return mCollisionInfoPerLayerPair[int(rlayer_a)][int(rlayer_b)];
+}
+
+ContactLayerCollisionInfo* ContactListener::trackLayerPair(ContactLayer layer_a,
+                                                           ContactLayer layer_b) {
+    const auto [i, j] = convertToRelativeLayer(layer_a, layer_b);
+    (&mTrackedLayers[mLayerCount * i])[j] = true;
+    (&mTrackedLayers[mLayerCount * j])[i] = true;
+    return mCollisionInfoPerLayerPair[i][j];
+}
+
+void ContactListener::addLayerPairForContactPointInfo(LayerContactPointInfo* info,
+                                                      ContactLayer layer1, ContactLayer layer2,
+                                                      bool enabled) {
+    auto lock = sead::makeScopedLock(mCS);
+    const auto [i, j] = convertToRelativeLayer(layer1, layer2);
+
+    const auto set = [&](TrackedContactPointLayer* entry) {
+        if (!entry)
+            return;
+        entry->info = info;
+        entry->layer = layer1;
+        entry->enabled = enabled;
+    };
+
+    auto& row_i = mTrackedContactPointLayers[i];
+    auto& row_j = mTrackedContactPointLayers[j];
+    set(row_i[j].emplaceBack());
+    set(row_j[i].emplaceBack());
+}
+
+void ContactListener::removeLayerPairsForContactPointInfo(LayerContactPointInfo* info) {
+    auto lock = sead::makeScopedLock(mCS);
+    for (int i = 0; i < info->getLayerEntries().size(); ++i) {
+        auto* entry = info->getLayerEntries()[i];
+        removeLayerPairForContactPointInfo(info, entry->layer1, entry->layer2);
+    }
+}
+
+// NON_MATCHING: enum-to-int conversion (stp should be 2 str), reordering, branching
+void ContactListener::removeLayerPairForContactPointInfo(LayerContactPointInfo* info,
+                                                         ContactLayer layer1, ContactLayer layer2) {
+    const auto [i, j] = convertToRelativeLayer(layer1, layer2);
+    auto& row_i = mTrackedContactPointLayers[i];
+    auto& row_j = mTrackedContactPointLayers[j];
+    auto& ij = row_i[j];
+    auto& ji = row_j[i];
+
+    for (int idx = 0; idx < ij.size(); ++idx) {
+        if (ij[idx]->info == info) {
+            ij.erase(idx);
+            break;
+        }
+    }
+
+    for (int idx = 0; idx < ji.size(); ++idx) {
+        if (ji[idx]->info == info) {
+            ji.erase(idx);
+            break;
+        }
+    }
+}
+
+void ContactListener::registerRigidBody(RigidBody* body) {
+    const u32 rlayer = body->getContactLayer() - mLayerBase;
+    auto& column = mCollisionInfoPerLayerPair[int(rlayer)];
+    for (u32 i = 0; i < mLayerCount; ++i) {
+        ContactLayerCollisionInfo* info = column[int(i)];
+        mMgr->x_21(info, body);
+    }
+}
+
+bool ContactListener::characterControlContactPointCallback(u32 ignored_layers_a,
+                                                           u32 ignored_layers_b, RigidBody* body_a,
+                                                           RigidBody* body_b, ContactLayer layer_a,
+                                                           ContactLayer layer_b,
+                                                           const hkpContactPointEvent& event) {
+    event.m_contactPointProperties->m_flags |= hkpContactPointProperties::CONTACT_IS_DISABLED;
+    return false;
+}
+
 }  // namespace ksys::phys

src/KingSystem/Physics/System/physContactListener.h

@@ -6,12 +6,15 @@
 #include <hostio/seadHostIONode.h>
 #include <prim/seadRuntimeTypeInfo.h>
 #include <thread/seadCriticalSection.h>
+#include <utility>
 #include "KingSystem/Physics/physDefines.h"
+#include "KingSystem/Physics/physMaterialMask.h"
 
 namespace ksys::phys {
 
 class ContactLayerCollisionInfo;
 class ContactMgr;
+class LayerContactPointInfo;
 class RigidBody;
 
 class ContactListener : public hkpContactListener, public sead::hostio::Node {
@@ -23,6 +26,18 @@ public:
     void init(sead::Heap* heap);
     void clearTable();
 
+    bool areContactsTrackedForLayerPair(u32 rlayer_a, u32 rlayer_b) const;
+    ContactLayerCollisionInfo* trackLayerPair(ContactLayer layer_a, ContactLayer layer_b);
+    ContactLayerCollisionInfo* getContactLayerCollisionInfo(u32 rlayer_a, u32 rlayer_b);
+
+    void addLayerPairForContactPointInfo(LayerContactPointInfo* info, ContactLayer layer1,
+                                         ContactLayer layer2, bool enabled);
+    void removeLayerPairsForContactPointInfo(LayerContactPointInfo* info);
+    void removeLayerPairForContactPointInfo(LayerContactPointInfo* info, ContactLayer layer1,
+                                            ContactLayer layer2);
+
+    void registerRigidBody(RigidBody* body);
+
     void contactPointCallback(const hkpContactPointEvent& event) override;
     void collisionAddedCallback(const hkpCollisionEvent& event) override;
     void collisionRemovedCallback(const hkpCollisionEvent& event) override;
@@ -31,7 +46,7 @@ public:
     void contactProcessCallback(hkpContactProcessEvent& event) override {}
 
 protected:
-    virtual void characterControlContactPointCallback(u32 ignored_layers_a, u32 ignored_layers_b,
+    virtual bool characterControlContactPointCallback(u32 ignored_layers_a, u32 ignored_layers_b,
                                                       RigidBody* body_a, RigidBody* body_b,
                                                       ContactLayer layer_a, ContactLayer layer_b,
                                                       const hkpContactPointEvent& event);
@@ -44,27 +59,35 @@ protected:
     void handleCollisionRemoved(const hkpCollisionEvent& event, RigidBody* body_a,
                                 RigidBody* body_b);
 
-    virtual void manifoldContactPointCallback(const hkpContactPointEvent& event, RigidBody* body_a,
+    virtual bool manifoldContactPointCallback(const hkpContactPointEvent& event, RigidBody* body_a,
                                               RigidBody* body_b);
 
-    virtual void regularContactPointCallback(const hkpContactPointEvent& event, RigidBody* body_a,
-                                             RigidBody* body_b, void* unk = nullptr);
+    /// @param out_material_masks [Optional] Pass this to get the materials of the rigid bodies.
+    virtual bool regularContactPointCallback(const hkpContactPointEvent& event, RigidBody* body_a,
+                                             RigidBody* body_b,
+                                             sead::SafeArray<MaterialMask, 2>* out_material_masks);
 
     virtual u32 m15() { return 0; }
 
+    std::pair<int, int> convertToRelativeLayer(ContactLayer layer1, ContactLayer layer2) const {
+        return {layer1 - int(mLayerBase), layer2 - int(mLayerBase)};
+    }
+
 private:
-    struct Unk1 {
-        void* _0;
-        void* _8;
+    struct TrackedContactPointLayer {
+        LayerContactPointInfo* info;
+        ContactLayer layer;
+        bool enabled;
     };
 
     ContactMgr* mMgr{};
     ContactLayerType mLayerType{};
     u32 mLayerBase{};
-    sead::Buffer<sead::Buffer<sead::FixedObjArray<Unk1, 8>>> _20;
+    sead::Buffer<sead::Buffer<sead::FixedObjArray<TrackedContactPointLayer, 8>>>
+        mTrackedContactPointLayers;
     sead::Buffer<sead::Buffer<ContactLayerCollisionInfo*>> mCollisionInfoPerLayerPair;
-    void* _40{};
-    u32 _48{};
+    u8* mTrackedLayers{};
+    u32 mTrackedLayersBufferSize{};
     u32 mLayerCount{};
     sead::CriticalSection mCS;
     u16 _90{};

src/KingSystem/Physics/System/physContactMgr.h

@@ -22,8 +22,10 @@ class Heap;
 
 namespace ksys::phys {
 
-class ContactLayerCollisionInfo;
 enum class IsIndoorStage;
+
+class CollisionInfo;
+class ContactLayerCollisionInfo;
 class ContactPointInfoBase;
 class RigidBody;
 class ContactPointInfo;
@@ -89,13 +91,15 @@ public:
     void freeContactPointInfo(ContactPointInfoBase* info);
 
     // 0x0000007100fb3744
-    void x_17(void* unk, RigidBody* body_a, RigidBody* body_b);
+    void x_17(CollisionInfo* info, RigidBody* body_a, RigidBody* body_b);
     // 0x0000007100fb37d4
     void x_18(ContactLayerCollisionInfo* info, RigidBody* body_a, RigidBody* body_b);
     // 0x0000007100fb3854
-    void x_19(void* unk, RigidBody* body_a, RigidBody* body_b);
+    void x_19(CollisionInfo* info, RigidBody* body_a, RigidBody* body_b);
     // 0x0000007100fb3938
     void x_20(ContactLayerCollisionInfo* info, RigidBody* body_a, RigidBody* body_b);
+    // 0x0000007100fb3a2c
+    void x_21(ContactLayerCollisionInfo* info, RigidBody* body);
 
 private:
     void doLoadContactInfoTable(agl::utl::ResParameterArchive archive, ContactLayerType type,

src/KingSystem/Physics/System/physContactPointInfo.h

@@ -5,10 +5,13 @@
 #include <container/seadListImpl.h>
 #include <container/seadSafeArray.h>
 #include <cstddef>
+#include <math/seadVector.h>
 #include <prim/seadBitFlag.h>
+#include <prim/seadDelegate.h>
 #include <prim/seadNamable.h>
 #include <prim/seadSafeString.h>
 #include <thread/seadAtomic.h>
+#include "KingSystem/Physics/RigidBody/physRigidBody.h"
 #include "KingSystem/Utils/Types.h"
 
 namespace ksys::phys {
@@ -40,6 +43,20 @@ protected:
 
 class ContactPointInfo : public ContactPointInfoBase {
 public:
+    enum class ShouldDisableContact : bool {
+        Yes = true,
+        No = false,
+    };
+
+    struct Event {
+        RigidBody* body;
+        const sead::Vector3f* position;
+        const sead::Vector3f* separating_normal;
+        const RigidBody::CollisionMasks* collision_masks;
+    };
+
+    using ContactCallback = sead::IDelegate2R<ShouldDisableContact*, const Event&, bool>;
+
     static ContactPointInfo* make(sead::Heap* heap, int num, const sead::SafeString& name, int a,
                                   int b, int c);
     static void free(ContactPointInfo* instance);
@@ -49,9 +66,12 @@ public:
     void freePoints() override;
     virtual void allocPoints(sead::Heap* heap, int num);
 
+    ContactCallback* getContactCallback() const { return mContactCallback; }
+    void setContactCallback(ContactCallback* cb) { mContactCallback = cb; }
+
 private:
     sead::Buffer<void*> mPoints{};
-    void* _58{};
+    ContactCallback* mContactCallback{};
 };
 KSYS_CHECK_SIZE_NX150(ContactPointInfo, 0x60);
 

src/KingSystem/Physics/System/physLayerContactPointInfo.h

@@ -16,6 +16,12 @@ class LayerContactPointInfo : public ContactPointInfoBase {
 public:
     using Points = sead::Buffer<ContactPoint*>;
 
+    struct LayerEntry {
+        ContactLayer layer1;
+        ContactLayer layer2;
+        bool enabled;
+    };
+
     class IteratorEnd;
 
     class Iterator {
@@ -100,13 +106,10 @@ public:
     auto begin() const { return Iterator(mPoints, _18); }
     auto end() const { return IteratorEnd(mPoints, _18); }
 
-private:
-    struct LayerEntry {
-        ContactLayer layer1;
-        ContactLayer layer2;
-        bool enabled;
-    };
+    sead::ObjArray<LayerEntry>& getLayerEntries() { return mLayerEntries; }
+    const sead::ObjArray<LayerEntry>& getLayerEntries() const { return mLayerEntries; }
 
+private:
     Points mPoints{};
     sead::ObjArray<LayerEntry> mLayerEntries;
     ContactLayerType mLayerType = ContactLayerType::Invalid;

src/KingSystem/Physics/System/physSystem.cpp

@@ -58,8 +58,8 @@ void System::freeContactPointInfo(ContactPointInfo* info) const {
 }
 
 LayerContactPointInfo* System::allocLayerContactPointInfo(sead::Heap* heap, int num, int num2,
-                                                    const sead::SafeString& name, int a, int b,
-                                                    int c) const {
+                                                          const sead::SafeString& name, int a,
+                                                          int b, int c) const {
     return mContactMgr->allocContactPointsEx(heap, num, num2, name, a, b, c);
 }