ksys/phys: Add RigidBodyRequestMgr::calc1

6 files changed, 59 insertions, 19 deletions

diff --git a/src/KingSystem/Physics/RigidBody/physRigidBody.cpp b/src/KingSystem/Physics/RigidBody/physRigidBody.cpp
index f82f0aaa..f0710bca 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBody.cpp
+++ b/src/KingSystem/Physics/RigidBody/physRigidBody.cpp
@@ -1766,7 +1766,7 @@ const char* RigidBody::getName() {
     return mUserTag ? mUserTag->getName().cstr() : getHkBodyName().cstr();
 }
 
-void RigidBody::logPosition() const {
+void RigidBody::onImpulse(RigidBody* body_b, float impulse) const {
     sead::Vector3f position;
     getPosition(&position);
     // debug logging?
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBody.h b/src/KingSystem/Physics/RigidBody/physRigidBody.h
index 34f9fd71..7368d178 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBody.h
+++ b/src/KingSystem/Physics/RigidBody/physRigidBody.h
@@ -283,7 +283,7 @@ public:
     void setPosition(const sead::Vector3f& position, bool propagate_to_linked_motions);
     void getPosition(sead::Vector3f* position) const;
     sead::Vector3f getPosition() const;
-    virtual void logPosition() const;
+    virtual void onImpulse(RigidBody* body_b, float impulse) const;
     void getAabbInLocal(sead::BoundBox3f* aabb) const;
     void getAabbInWorld(sead::BoundBox3f* aabb) const;
 
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.cpp b/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.cpp
index 77ddadbf..957454a2 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.cpp
+++ b/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.cpp
@@ -1,5 +1,8 @@
 #include "KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h"
 #include <prim/seadScopedLock.h>
+#include "KingSystem/Physics/RigidBody/physRigidBody.h"
+#include "KingSystem/Physics/System/physSystem.h"
+#include "KingSystem/Physics/System/physUserTag.h"
 #include "KingSystem/Physics/physMaterialMask.h"
 
 namespace ksys::phys {
@@ -12,18 +15,18 @@ RigidBodyRequestMgr::RigidBodyRequestMgr() = default;
 RigidBodyRequestMgr::~RigidBodyRequestMgr() {
     for (int i = 0; i < NumRigidBodyBuffers; ++i) {
         mRigidBodies1[i].freeBuffer();
-        mRigidBodies2[i].freeBuffer();
+        mOobRigidBodies[i].freeBuffer();
     }
 
     _38.freeBuffer();
     _50.freeBuffer();
-    _98.freeBuffer();
+    mImpulseEntries.freeBuffer();
     _b0.freeBuffer();
     _c8.freeBuffer();
     _e0.freeBuffer();
     _120.freeBuffer();
     _138.freeBuffer();
-    _108.freeBuffer();
+    mImpulseEntriesPool.freeBuffer();
     mMotionAccessors.freeBuffer();
 
     if (mContactPoints) {
@@ -35,7 +38,7 @@ RigidBodyRequestMgr::~RigidBodyRequestMgr() {
 void RigidBodyRequestMgr::init(sead::Heap* heap) {
     constexpr int Buffer138Size = 0x800;
 
-    _108.allocBufferAssert(0x100, heap);
+    mImpulseEntriesPool.allocBufferAssert(0x100, heap);
     _118 = 0;
 
     _120.allocBufferAssert(0x100, heap);
@@ -43,12 +46,12 @@ void RigidBodyRequestMgr::init(sead::Heap* heap) {
 
     for (int i = 0; i < NumRigidBodyBuffers; ++i) {
         mRigidBodies1[i].alloc(0x2000, heap);
-        mRigidBodies2[i].alloc(0x100, heap);
+        mOobRigidBodies[i].alloc(0x100, heap);
     }
 
     _38.alloc(0x800, heap);
     _50.alloc(0x800, heap);
-    _98.alloc(0x100, heap);
+    mImpulseEntries.alloc(0x100, heap);
     _b0.alloc(0x100, heap);
     mMotionAccessors.allocBuffer(0x400, heap);
     _138.allocBufferAssert(Buffer138Size, heap);
@@ -83,6 +86,35 @@ void RigidBodyRequestMgr::init(sead::Heap* heap) {
     mWaterPoisonSubmatIdx = MaterialMask::getSubMaterialIdx(Material::Water, "Water_Poison");
 }
 
+void RigidBodyRequestMgr::calc1(ContactLayerType layer_type, bool paused) {
+    auto lock = sead::makeScopedLock(mCriticalSections[int(layer_type)]);
+    ScopedWorldLock world_lock{layer_type};
+
+    if (!paused && layer_type == ContactLayerType::Entity)
+        processImpulseEntries();
+
+    processOobRigidBodyEntries(layer_type);
+}
+
+void RigidBodyRequestMgr::processImpulseEntries() {
+    while (auto* impulse = mImpulseEntries.pop()) {
+        if (auto* tag = impulse->body_a->getUserTag())
+            tag->onImpulse(impulse->body_a, impulse->body_b, impulse->impulse_a);
+        else
+            impulse->body_a->onImpulse(impulse->body_b, impulse->impulse_a);
+    }
+    _118 = 0;
+}
+
+void RigidBodyRequestMgr::processOobRigidBodyEntries(ContactLayerType layer_type) {
+    while (auto* body = mOobRigidBodies[int(layer_type)].pop()) {
+        if (auto* tag = body->getUserTag())
+            tag->onMaxPositionExceeded(body);
+        else
+            body->onMaxPositionExceeded();
+    }
+}
+
 bool RigidBodyRequestMgr::pushRigidBody(ContactLayerType type, RigidBody* body) {
     static_cast<void>(mRigidBodies1[int(type)].getSize());
     return mRigidBodies1[int(type)].push(body);
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h b/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h
index e29db07b..56afd4d2 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h
+++ b/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h
@@ -46,6 +46,10 @@ public:
 
     void init(sead::Heap* heap);
 
+    // 0x0000007100fa6ac4
+    void calc(ContactLayerType layer_type);
+    void calc1(ContactLayerType layer_type, bool paused);
+
     bool pushRigidBody(ContactLayerType type, RigidBody* body);
     // 0x0000007100fa6d48
     void addEntityToWorld(ContactLayerType type, hkpEntity* entity);
@@ -65,7 +69,6 @@ public:
 
 private:
     struct Unk1;
-    struct Unk2;
     struct Unk3;
 
     struct Unk4 {
@@ -73,10 +76,12 @@ private:
     };
     KSYS_CHECK_SIZE_NX150(Unk4, 0x10);
 
-    struct Unk5 {
-        u8 _0[0x18];
+    struct ImpulseEntry {
+        RigidBody* body_a;
+        RigidBody* body_b;
+        float impulse_a;
     };
-    KSYS_CHECK_SIZE_NX150(Unk5, 0x18);
+    KSYS_CHECK_SIZE_NX150(ImpulseEntry, 0x18);
 
     struct Unk6 {
         sead::Vector3f _0 = sead::Vector3f::zero;
@@ -104,18 +109,22 @@ private:
     bool someFunction2(const LayerContactPointInfo::ContactEvent& event);
     static void someFunction(void* arg);
 
+    void processImpulseEntries();
+    void processOobRigidBodyEntries(ContactLayerType layer_type);
+
     static constexpr int NumRigidBodyBuffers = 2;
 
     sead::SafeArray<util::LockFreeQueue<RigidBody>, NumRigidBodyBuffers> mRigidBodies1;
     util::LockFreeQueue<Unk1> _38;
     util::LockFreeQueue<Unk1> _50;
-    sead::SafeArray<util::LockFreeQueue<RigidBody>, NumRigidBodyBuffers> mRigidBodies2;
-    util::LockFreeQueue<Unk2> _98;
+    /// Rigid bodies that are out of bounds.
+    sead::SafeArray<util::LockFreeQueue<RigidBody>, NumRigidBodyBuffers> mOobRigidBodies;
+    util::LockFreeQueue<ImpulseEntry> mImpulseEntries;
     util::LockFreeQueue<Unk3> _b0;
     util::LockFreeQueue<Unk4> _c8;
     util::LockFreeQueue<Unk4> _e0;
     sead::PtrArray<MotionAccessor> mMotionAccessors;
-    sead::Buffer<Unk5> _108;
+    sead::Buffer<ImpulseEntry> mImpulseEntriesPool;
     sead::Atomic<u32> _118;
     sead::Buffer<Unk6> _120;
     sead::Atomic<u32> _130;
diff --git a/src/KingSystem/Physics/System/physUserTag.cpp b/src/KingSystem/Physics/System/physUserTag.cpp
index 99a05991..9ac62117 100644
--- a/src/KingSystem/Physics/System/physUserTag.cpp
+++ b/src/KingSystem/Physics/System/physUserTag.cpp
@@ -7,8 +7,8 @@ void UserTag::onMaxPositionExceeded(RigidBody* body) {
     body->onMaxPositionExceeded();
 }
 
-void UserTag::m3(void* a, void* b, float c) {
-    // FIXME
+void UserTag::onImpulse(RigidBody* body_a, RigidBody* body_b, float impulse_a) {
+    body_a->onImpulse(body_b, impulse_a);
 }
 
 void UserTag::onBodyShapeChanged(RigidBody* body) {}
diff --git a/src/KingSystem/Physics/System/physUserTag.h b/src/KingSystem/Physics/System/physUserTag.h
index 5da68e5b..ebf5815c 100644
--- a/src/KingSystem/Physics/System/physUserTag.h
+++ b/src/KingSystem/Physics/System/physUserTag.h
@@ -16,8 +16,7 @@ public:
     /// Called when a rigid body goes beyond the broadphase border.
     /// The default implementation just notifies the rigid body of this callback.
     virtual void onMaxPositionExceeded(RigidBody* body);
-    // a and b are probably physics bodies?
-    virtual void m3(void* a, void* b, float c);
+    virtual void onImpulse(RigidBody* body_a, RigidBody* body_b, float impulse_a);
     virtual void onBodyShapeChanged(RigidBody* body);
     virtual void m5();
     virtual const sead::SafeString& getName() const { return sead::SafeString::cEmptyString; }