ksys/phys: Add impulse related ContactMgr functions

8 files changed, 276 insertions, 43 deletions

diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodyFromResource.cpp b/src/KingSystem/Physics/RigidBody/physRigidBodyFromResource.cpp
index bb4af731..e828d98f 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBodyFromResource.cpp
+++ b/src/KingSystem/Physics/RigidBody/physRigidBodyFromResource.cpp
@@ -66,14 +66,14 @@ bool RigidBodyFromResource::isMaterial(Material material) const {
             if (!shape)
                 return false;
 
-            MaterialMask shape_material{MaterialMaskData(shape->getUserData())};
+            MaterialMask shape_material{shape->getUserData()};
             if (int(shape_material.getMaterial()) == material) {
                 found_child_shape_with_material = true;
                 break;
             }
         }
     } else {
-        MaterialMask shape_material{MaterialMaskData(mShape->getUserData())};
+        MaterialMask shape_material{mShape->getUserData()};
         if (int(shape_material.getMaterial()) == material)
             return true;
     }
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h b/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h
index 1f895de2..5ee9bce0 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h
+++ b/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h
@@ -54,6 +54,9 @@ public:
     // 0x0000007100fa6ebc
     void removeRigidBody(ContactLayerType type, RigidBody* body);
 
+    // 0x0000007100fa7340
+    void addImpulse(RigidBody* body_a, RigidBody* body_b, float impulse);
+
     bool registerMotionAccessor(MotionAccessor* accessor);
     bool deregisterMotionAccessor(MotionAccessor* accessor);
 
diff --git a/src/KingSystem/Physics/System/physContactMgr.cpp b/src/KingSystem/Physics/System/physContactMgr.cpp
index c7ea0aab..4a3c8de1 100644
--- a/src/KingSystem/Physics/System/physContactMgr.cpp
+++ b/src/KingSystem/Physics/System/physContactMgr.cpp
@@ -1,5 +1,8 @@
 #include "KingSystem/Physics/System/physContactMgr.h"
+#include <memory>
 #include <prim/seadScopedLock.h>
+#include "KingSystem/Physics/RigidBody/physRigidBody.h"
+#include "KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h"
 #include "KingSystem/Physics/System/physCollisionInfo.h"
 #include "KingSystem/Physics/System/physContactLayerCollisionInfo.h"
 #include "KingSystem/Physics/System/physContactPointInfo.h"
@@ -9,17 +12,48 @@
 #include "KingSystem/Physics/System/physQueryContactPointInfo.h"
 #include "KingSystem/Physics/System/physSystem.h"
 #include "KingSystem/Utils/Debug.h"
+#include "KingSystem/Utils/MathUtil.h"
 
 namespace ksys::phys {
 
+struct ContactMgr::ImpulseEntry {
+    static constexpr size_t getListNodeOffset() { return offsetof(ImpulseEntry, list_node); }
+
+    RigidBody* bodies[2];
+    sead::Vector3f linear_vels[2];
+    bool is_dynamic_motion[2];
+    float magnitude;
+    float impulses[2];
+    sead::Vector3f contact_point_pos;
+    bool is_entity_motion_flag_20_off;
+    bool valid;
+    sead::ListNode list_node;
+};
+
 ContactMgr::ContactMgr() {
     mContactPointInfoInstances.initOffset(ContactPointInfo::getListNodeOffset());
-    // FIXME: figure out what these offsets are
     mCollisionInfoInstances.initOffset(CollisionInfo::getListNodeOffset());
+    // FIXME: figure out what this offset is
     mList3.initOffset(0x40);
     mCollidingBodiesFreeList.initOffset(CollidingBodies::getListNodeOffset());
-    mList4.initOffset(0x48);
-    mList5.initOffset(0x48);
+    mImpulseEntriesFreeList.initOffset(ImpulseEntry::getListNodeOffset());
+    mImpulseEntries.initOffset(ImpulseEntry::getListNodeOffset());
+}
+
+ContactMgr::~ContactMgr() {
+    for (int i = 0; i < mCollidingBodiesCapacity; ++i)
+        delete mCollidingBodiesFreeList.popFront();
+}
+
+void ContactMgr::init(sead::Heap* heap) {
+    mCollidingBodiesCapacity = 0x2000;
+    for (int i = 0; i < mCollidingBodiesCapacity; ++i) {
+        mCollidingBodiesFreeList.pushBack(new (heap) CollidingBodies());
+    }
+
+    for (int i = 0; i < 0x100; ++i) {
+        mImpulseEntriesFreeList.pushBack(new (heap) ImpulseEntry());
+    }
 }
 
 void ContactMgr::initContactPointPool(sead::Heap* heap, IsIndoorStage indoor) {
@@ -189,6 +223,17 @@ void ContactMgr::removeCollisionEntriesWithBody(RigidBody* body) {
     }
 }
 
+void ContactMgr::removeImpulseEntriesWithBody(RigidBody* body) {
+    auto lock = sead::makeScopedLock(mImpulseEntriesMutex);
+
+    for (auto& entry : mImpulseEntries.robustRange()) {
+        if (entry.bodies[0] == body || entry.bodies[1] == body) {
+            mImpulseEntries.erase(&entry);
+            mImpulseEntriesFreeList.pushBack(&entry);
+        }
+    }
+}
+
 int ContactMgr::allocateContactPoint() {
     if (mNumContactPoints >= mContactPointPool.size() - 1) {
         util::PrintDebugFmt("contact point pool exhausted (current number of points: %d)",
@@ -389,6 +434,171 @@ void ContactMgr::clearCollisionEntries(CollisionInfo* info) {
     }
 }
 
+void ContactMgr::addImpulseEntry(RigidBody* body_a, RigidBody* body_b) {
+    auto lock = sead::makeScopedLock(mImpulseEntriesMutex);
+
+    auto* entry = mImpulseEntriesFreeList.popFront();
+    if (!entry)
+        return;
+
+    entry->bodies[0] = body_a;
+    entry->bodies[1] = body_b;
+    body_a->getLinearVelocity(&entry->linear_vels[0]);
+    body_b->getLinearVelocity(&entry->linear_vels[1]);
+    entry->is_dynamic_motion[0] = body_a->getMotionType() == MotionType::Dynamic;
+    entry->is_dynamic_motion[1] = body_b->getMotionType() == MotionType::Dynamic;
+    entry->magnitude = 0;
+    entry->impulses[0] = 0;
+    entry->impulses[1] = 0;
+    entry->is_entity_motion_flag_20_off = true;
+    entry->valid = false;
+    mImpulseEntries.pushBack(entry);
+}
+
+static sead::Vector3f computeLinearVelocity(const RigidBody& body, const sead::Vector3f& linear_vel,
+                                            const sead::Vector3f& pos) {
+    const auto center = body.getCenterOfMassInWorld();
+    const auto rel_pos = pos - center;
+    sead::Vector3f correction;
+    correction.setCross(body.getAngularVelocity(), rel_pos);
+    return linear_vel + correction;
+}
+
+void ContactMgr::setImpulseEntryContactInfo(RigidBody* body_a, RigidBody* body_b,
+                                            const sead::Vector3f& contact_point_pos,
+                                            const sead::Vector3f& contact_point_normal,
+                                            u32 material_a, u32 material_b) {
+    auto lock = sead::makeScopedLock(mImpulseEntriesMutex);
+
+    ImpulseEntry* entry = nullptr;
+    for (auto& impulse_entry : mImpulseEntries) {
+        if ((impulse_entry.bodies[0] == body_a && impulse_entry.bodies[1] == body_b) ||
+            (impulse_entry.bodies[1] == body_a && impulse_entry.bodies[0] == body_b)) {
+            entry = std::addressof(impulse_entry);
+            break;
+        }
+    }
+
+    if (!entry)
+        return;
+
+    const MaterialMask mat_mask_a{material_a};
+    const MaterialMask mat_mask_b{material_b};
+
+    if (mat_mask_a.getData().flag30 || mat_mask_b.getData().flag30)
+        return;
+
+    if (mat_mask_a.getFloorCode() == FloorCode::NoImpulseUpperMove && entry->linear_vels[0].y > 0)
+        return;
+
+    if (mat_mask_b.getFloorCode() == FloorCode::NoImpulseUpperMove && entry->linear_vels[1].y > 0)
+        return;
+
+    /// The velocities of the rigid bodies at the contact point.
+    const auto linvel_a =
+        computeLinearVelocity(*entry->bodies[0], entry->linear_vels[0], contact_point_pos);
+    const auto linvel_b =
+        computeLinearVelocity(*entry->bodies[1], entry->linear_vels[1], contact_point_pos);
+
+    const bool is_flag_off =
+        entry->bodies[0]->isEntityMotionFlag20Off() && entry->bodies[1]->isEntityMotionFlag20Off();
+
+    /// The pre-collision relative velocity.
+    const auto relative_vel = linvel_a - linvel_b;
+    const auto dot_neg = [&](const auto& vec) { return vec.dot(-contact_point_normal); };
+
+    float magnitude = is_flag_off ? sead::Mathf::max(0.0, relative_vel.dot(-contact_point_normal)) :
+                                    sead::Mathf::max(0.0, relative_vel.length());
+
+    if (magnitude >= entry->magnitude) {
+        float i1, i2;
+        if (is_flag_off) {
+            i1 = sead::Mathf::min(sead::Mathf::max(0.0, dot_neg(linvel_a)), magnitude);
+            i2 = sead::Mathf::min(sead::Mathf::max(0.0, linvel_b.dot(contact_point_normal)),
+                                  sead::Mathf::max(0.0, dot_neg(relative_vel)));
+        } else {
+            i1 = sead::Mathf::min(sead::Mathf::max(0.0, linvel_a.length()), magnitude);
+            i2 = sead::Mathf::min(sead::Mathf::max(0.0, linvel_b.length()),
+                                  sead::Mathf::max(0.0, relative_vel.length()));
+        }
+
+        entry->magnitude = magnitude;
+        entry->impulses[0] = i1;
+        entry->impulses[1] = i2;
+        entry->contact_point_pos = contact_point_pos;
+        entry->is_entity_motion_flag_20_off = is_flag_off;
+        entry->valid = true;
+    }
+}
+
+void ContactMgr::processImpulseEntry(const ImpulseEntry& entry) {
+    if (!entry.valid)
+        return;
+
+    RigidBody* body_a = entry.bodies[0];
+    RigidBody* body_b = entry.bodies[1];
+
+    if (body_a->isEntityMotionFlag40On())
+        body_a->setEntityMotionFlag40(false);
+
+    if (body_b->isEntityMotionFlag40On())
+        body_b->setEntityMotionFlag40(false);
+
+    float impulse_a = 0.0f;
+    float impulse_b = 0.0f;
+
+    const auto compute_impulse = [&entry](int index, RigidBody* body) {
+        if (!entry.is_dynamic_motion[index])
+            return 0.0f;
+
+        const auto impulse = entry.impulses[index];
+        if (!(impulse > 5.0f || !entry.is_entity_motion_flag_20_off))
+            return 0.0f;
+
+        const float scaled_impulse = impulse * body->getMass() * body->getColImpulseScale();
+
+        float volume_factor = 1.0f;
+        if (body->getMotionType() == MotionType::Dynamic)
+            volume_factor += util::clampAndRemapRange(body->getVolume(), 0.2, 1.0, 1.0, 0.0);
+        return scaled_impulse * volume_factor;
+    };
+
+    impulse_b = compute_impulse(0, body_a);
+    impulse_a = compute_impulse(1, body_b);
+
+    const auto add_impulse = [impulse_a, impulse_b](RigidBody* body1, RigidBody* body2,
+                                                    float impulse) {
+        if (body1->isEntityMotionFlag10Off())
+            impulse = impulse_a + impulse_b;
+
+        if (body2->isEntityMotionFlag8On() ||
+            (body1->getMaxImpulse() >= 0 && impulse > body1->getMaxImpulse())) {
+            System::instance()->getRigidBodyRequestMgr()->addImpulse(body1, body2, impulse);
+        }
+    };
+    add_impulse(body_a, body_b, impulse_a);
+    add_impulse(body_b, body_a, impulse_b);
+}
+
+void ContactMgr::processImpulseEntries() {
+    auto lock = sead::makeScopedLock(mImpulseEntriesMutex);
+
+    for (auto& entry : mImpulseEntries.robustRange()) {
+        processImpulseEntry(entry);
+        mImpulseEntries.erase(&entry);
+        mImpulseEntriesFreeList.pushBack(&entry);
+    }
+}
+
+void ContactMgr::clearImpulseEntries() {
+    auto lock = sead::makeScopedLock(mImpulseEntriesMutex);
+
+    for (auto& entry : mImpulseEntries.robustRange()) {
+        mImpulseEntries.erase(&entry);
+        mImpulseEntriesFreeList.pushBack(&entry);
+    }
+}
+
 void ContactInfoTable::Receiver::postRead_() {
     layer_mask = 0;
     layer_mask2 = 0;
diff --git a/src/KingSystem/Physics/System/physContactMgr.h b/src/KingSystem/Physics/System/physContactMgr.h
index ac5b5f79..4a8a8d9b 100644
--- a/src/KingSystem/Physics/System/physContactMgr.h
+++ b/src/KingSystem/Physics/System/physContactMgr.h
@@ -76,6 +76,8 @@ KSYS_CHECK_SIZE_NX150(ContactPoint, 0x70);
 
 class ContactMgr : public sead::hostio::Node {
 public:
+    struct ImpulseEntry;
+
     ContactMgr();
     virtual ~ContactMgr();
 
@@ -114,9 +116,7 @@ public:
     /// 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);
+    void removeImpulseEntriesWithBody(RigidBody* body);
 
     /// @param colliding_body_masks The collision masks of the colliding rigid bodies.
     /// @returns whether contact should be disabled.
@@ -143,19 +143,24 @@ public:
     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& point_position,
-              const sead::Vector3f& point_separating_normal, u32 material_a, u32 material_b);
-    // 0x0000007100fb4738
-    void x_28(void* unk);
-    // 0x0000007100fb49d4
-    void x_14();
-    // 0x0000007100fb4a90
-    void x_29();
+    /// Add a new impulse entry based on contact between the two specified rigid bodies.
+    /// After this is called, contact point information must then be specified using
+    /// setImpulseEntryContactInfo.
+    void addImpulseEntry(RigidBody* body_a, RigidBody* body_b);
+
+    /// Fill in an existing impulse entry with contact information.
+    /// This does nothing if there is no impulse entry for the specified rigid bodies.
+    void setImpulseEntryContactInfo(RigidBody* body_a, RigidBody* body_b,
+                                    const sead::Vector3f& contact_point_pos,
+                                    const sead::Vector3f& contact_point_normal, u32 material_a,
+                                    u32 material_b);
+
+    void processImpulseEntries();
+    void clearImpulseEntries();
 
 private:
+    void processImpulseEntry(const ImpulseEntry& index);
+
     void doLoadContactInfoTable(agl::utl::ResParameterArchive archive, ContactLayerType type,
                                 bool skip_params);
 
@@ -170,20 +175,20 @@ private:
     /// Used to optimise ContactPoint allocations.
     sead::Buffer<ContactPoint> mContactPointPool;
     sead::OffsetList<CollidingBodies> mCollidingBodiesFreeList;
-    int mList0Size = 0;
+    int mCollidingBodiesCapacity = 0;
     /// The number of contact points. Also the iindex of the next free ContactPoint in the pool.
     sead::Atomic<int> mNumContactPoints = 0;
     sead::OffsetList<ContactPointInfoBase> mContactPointInfoInstances;
     sead::OffsetList<CollisionInfo> mCollisionInfoInstances;
     sead::OffsetList<void*> mList3;
-    sead::OffsetList<void*> mList4;
-    sead::OffsetList<void*> mList5;
-    // TODO: rename these members
+    sead::OffsetList<ImpulseEntry> mImpulseEntriesFreeList;
+    sead::OffsetList<ImpulseEntry> mImpulseEntries;
     sead::Mutex mContactPointInfoMutex;
     sead::Mutex mCollisionInfoMutex;
+    // TODO: rename mList3 and mMutex3
     sead::Mutex mMutex3;
     sead::Mutex mCollidingBodiesMutex;
-    sead::Mutex mMutex5;
+    sead::Mutex mImpulseEntriesMutex;
     sead::SafeArray<ContactInfoTable, 2> mContactInfoTables{};
 };
 
diff --git a/src/KingSystem/Physics/System/physEntityContactListener.cpp b/src/KingSystem/Physics/System/physEntityContactListener.cpp
index d0864888..c7986c87 100644
--- a/src/KingSystem/Physics/System/physEntityContactListener.cpp
+++ b/src/KingSystem/Physics/System/physEntityContactListener.cpp
@@ -101,7 +101,7 @@ void EntityContactListener::collisionAddedCallback(const hkpCollisionEvent& even
     }
 
     if (!should_process && m15(body_a, body_b)) {
-        mMgr->x_26(body_a, body_b);
+        mMgr->addImpulseEntry(body_a, body_b);
     }
 }
 
@@ -221,8 +221,8 @@ void EntityContactListener::m11(const hkpContactPointEvent& event,
                                 RigidBody* body_b) {
     auto* hk_point_properties = event.m_contactPointProperties;
 
-    const MaterialMask mat_mask_a = MaterialMaskData(masks_a.material_mask);
-    const MaterialMask mat_mask_b = MaterialMaskData(masks_b.material_mask);
+    const MaterialMask mat_mask_a{masks_a.material_mask};
+    const MaterialMask mat_mask_b{masks_b.material_mask};
     const auto* material_table = System::instance()->getMaterialTable();
 
     const auto mat_a = mat_mask_a.getMaterial();
@@ -519,10 +519,11 @@ bool EntityContactListener::regularContactPointCallback(const hkpContactPointEve
         storeToVec3(&position, event.m_contactPoint->getPosition());
         storeToVec3(&normal, event.m_contactPoint->getSeparatingNormal());
         if (body_a->isEntityMotionFlag40On() || body_b->isEntityMotionFlag40On()) {
-            mMgr->x_26(body_a, body_b);
+            mMgr->addImpulseEntry(body_a, body_b);
         }
 
-        mMgr->x_27(body_a, body_b, position, normal, material_masks[0], material_masks[1]);
+        mMgr->setImpulseEntryContactInfo(body_a, body_b, position, normal, material_masks[0],
+                                         material_masks[1]);
     }
 
     return result;
diff --git a/src/KingSystem/Physics/physMaterialMask.cpp b/src/KingSystem/Physics/physMaterialMask.cpp
index 365248fa..04345c51 100644
--- a/src/KingSystem/Physics/physMaterialMask.cpp
+++ b/src/KingSystem/Physics/physMaterialMask.cpp
@@ -6,13 +6,13 @@ namespace ksys::phys {
 
 MaterialMask::MaterialMask() = default;
 
-MaterialMask::MaterialMask(MaterialMaskData data) : mData(data) {}
+MaterialMask::MaterialMask(u32 data) : mData(data) {}
 
 MaterialMask::MaterialMask(Material mat, FloorCode floor, WallCode wall, bool flag) {
     mData.material.Init(mat);
     mData.floor.Init(floor);
     mData.wall.Init(wall);
-    mData.setFlag(flag);
+    mData.setFlag31(flag);
 }
 
 MaterialMask::MaterialMask(Material mat, const char* submat_name, FloorCode floor, WallCode wall,
@@ -26,7 +26,7 @@ MaterialMask::MaterialMask(Material mat, const char* submat_name, FloorCode floo
     mData.sub_material.Init(submat_idx);
     mData.floor.Init(floor);
     mData.wall.Init(wall);
-    mData.setFlag(flag);
+    mData.setFlag31(flag);
 }
 
 MaterialMask::MaterialMask(Material mat, int submat_idx, FloorCode floor, WallCode wall, bool flag)
@@ -36,7 +36,7 @@ MaterialMask::MaterialMask(Material mat, int submat_idx, FloorCode floor, WallCo
     mData.sub_material.Init(submat_idx);
     mData.floor.Init(floor);
     mData.wall.Init(wall);
-    mData.setFlag(flag);
+    mData.setFlag31(flag);
 }
 
 MaterialMask::~MaterialMask() = default;
diff --git a/src/KingSystem/Physics/physMaterialMask.h b/src/KingSystem/Physics/physMaterialMask.h
index f4482325..69e72c11 100644
--- a/src/KingSystem/Physics/physMaterialMask.h
+++ b/src/KingSystem/Physics/physMaterialMask.h
@@ -28,14 +28,14 @@ union MaterialMaskData {
     FloorCode getFloorCode() const { return int(floor.Value()); }
     WallCode getWallCode() const { return int(wall.Value()); }
 
-    void setFlag(bool b) {
+    void setFlag31(bool b) {
         if (!b)
-            clearFlag();
+            clearFlag31();
         else
-            setFlag();
+            setFlag31();
     }
-    void setFlag() { flag = 1; }
-    void clearFlag() { flag = 0; }
+    void setFlag31() { flag31 = 1; }
+    void clearFlag31() { flag31 = 0; }
 
     void setCustomFlag(CustomFlag custom_flag) {
         raw |= 1 << (decltype(custom_flags)::StartBit() + custom_flag);
@@ -46,16 +46,18 @@ union MaterialMaskData {
     util::BitField<6, 4, int, u32> sub_material;
     util::BitField<10, 5, u32> floor;
     util::BitField<15, 5, u32> wall;
-    util::BitField<23, 8, u32> custom_flags;
-    // TODO: rename once we figure out what this flag is
-    util::BitField<31, 1, u32> flag;
+    // TODO: rename once we figure out what these flags are
+    util::BitField<23, 7, u32> custom_flags;
+    util::BitField<30, 1, u32> flag30;
+    util::BitField<31, 1, u32> flag31;
 };
 static_assert(sizeof(MaterialMaskData) == sizeof(u32));
 
 class MaterialMask {
 public:
     MaterialMask();
-    MaterialMask(MaterialMaskData data);  // NOLINT(google-explicit-constructor)
+    explicit MaterialMask(u32 data);
+    explicit MaterialMask(u64 havok_user_data) : MaterialMask(static_cast<u32>(havok_user_data)) {}
     MaterialMask(Material mat, FloorCode floor, WallCode wall, bool flag = false);
     MaterialMask(Material mat, const char* submat_name, FloorCode floor, WallCode wall,
                  bool flag = false);
diff --git a/src/KingSystem/Utils/MathUtil.h b/src/KingSystem/Utils/MathUtil.h
index 6ceb07f5..182bbe2c 100644
--- a/src/KingSystem/Utils/MathUtil.h
+++ b/src/KingSystem/Utils/MathUtil.h
@@ -1,12 +1,24 @@
 #pragma once
 
+#include <math/seadMathCalcCommon.h>
 #include <math/seadVector.h>
 
 namespace ksys::util {
 
+/// Remap `value` from a [low, high] range to [new_low, new_high].
+inline float remapRange(float value, float low, float high, float new_low, float new_high) {
+    return new_low + (value - low) * (new_high - new_low) / (high - low);
+}
+
+/// Clamp `value` to [low, high] and then remap the value to the range [new_low, new_high].
+inline float clampAndRemapRange(float value, float low, float high, float new_low, float new_high) {
+    value = sead::Mathf::clamp(value, low, high);
+    return remapRange(value, low, high, new_low, new_high);
+}
+
 // too specific for sead
 inline float sqXZDistance(const sead::Vector3f& a, const sead::Vector3f& b) {
     return sead::Mathf::square(a.x - b.x) + sead::Mathf::square(a.z - b.z);
 }
 
-}  // namespace ksys::util
\ No newline at end of file
+}  // namespace ksys::util