ksys/phys: Add easy RigidBodyRequestMgr functions

5 files changed, 37 insertions, 8 deletions

diff --git a/data/uking_functions.csv b/data/uking_functions.csv
index 26bb3f92..f868573d 100644
--- a/data/uking_functions.csv
+++ b/data/uking_functions.csv
@@ -83454,7 +83454,7 @@ Address,Quality,Size,Name
 0x0000007100fa6a9c,U,000020,
 0x0000007100fa6ab0,U,000020,
 0x0000007100fa6ac4,U,000456,phys::RigidBodyRequestMgr::x_1
-0x0000007100fa6c8c,U,000188,phys::RigidBodyRequestMgr::x3_requestMaybe
+0x0000007100fa6c8c,O,000188,_ZN4ksys4phys19RigidBodyRequestMgr13pushRigidBodyEiPNS0_9RigidBodyE
 0x0000007100fa6d48,U,000100,
 0x0000007100fa6dac,U,000100,
 0x0000007100fa6e10,U,000084,
@@ -83465,8 +83465,8 @@ Address,Quality,Size,Name
 0x0000007100fa728c,U,000180,
 0x0000007100fa7340,U,000236,
 0x0000007100fa742c,U,000376,
-0x0000007100fa75a4,U,000108,
-0x0000007100fa7610,U,000140,
+0x0000007100fa75a4,O,000108,_ZN4ksys4phys19RigidBodyRequestMgr22registerMotionAccessorEPNS0_14MotionAccessorE
+0x0000007100fa7610,O,000140,_ZN4ksys4phys19RigidBodyRequestMgr24deregisterMotionAccessorEPNS0_14MotionAccessorE
 0x0000007100fa769c,U,000060,
 0x0000007100fa76d8,U,000384,
 0x0000007100fa7858,U,000352,
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBody.cpp b/src/KingSystem/Physics/RigidBody/physRigidBody.cpp
index 12d8e37a..59350e90 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBody.cpp
+++ b/src/KingSystem/Physics/RigidBody/physRigidBody.cpp
@@ -169,7 +169,7 @@ void RigidBody::setMotionFlag(MotionFlag flag) {
 
     if (mFlags.isOff(Flag::_20) && mFlags.isOff(Flag::_2)) {
         mFlags.set(Flag::_2);
-        MemSystem::instance()->getRigidBodyRequestMgr()->sub_7100FA6C8C(
+        MemSystem::instance()->getRigidBodyRequestMgr()->pushRigidBody(
             mFlags.isOn(Flag::MassScaling), this);
     }
 }
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.cpp b/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.cpp
index d603d3a3..171d72ed 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.cpp
+++ b/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.cpp
@@ -1,4 +1,5 @@
 #include "KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h"
+#include <prim/seadScopedLock.h>
 #include "KingSystem/Physics/System/physMaterialMask.h"
 
 namespace ksys::phys {
@@ -20,7 +21,7 @@ RigidBodyRequestMgr::~RigidBodyRequestMgr() {
     _120.freeBuffer();
     _138.freeBuffer();
     _108.freeBuffer();
-    _f8.freeBuffer();
+    mMotionAccessors.freeBuffer();
 
     if (mContactPoints) {
         ksys::phys::RigidContactPointsEx::free(mContactPoints);
@@ -46,7 +47,7 @@ void RigidBodyRequestMgr::init(sead::Heap* heap) {
     _50.alloc(0x800, heap);
     _98.alloc(0x100, heap);
     _b0.alloc(0x100, heap);
-    _f8.allocBuffer(0x400, heap);
+    mMotionAccessors.allocBuffer(0x400, heap);
     _138.allocBufferAssert(Buffer138Size, heap);
     _c8.alloc(0x800, heap);
     _e0.alloc(Buffer138Size, heap);
@@ -79,4 +80,27 @@ void RigidBodyRequestMgr::init(sead::Heap* heap) {
     mWaterPoisonSubmatIdx = MaterialMask::getSubMaterialIdx(Material::Water, "Water_Poison");
 }
 
+bool RigidBodyRequestMgr::pushRigidBody(int type, RigidBody* body) {
+    static_cast<void>(mRigidBodies1[type].getSize());
+    return mRigidBodies1[type].push(body);
+}
+
+bool RigidBodyRequestMgr::registerMotionAccessor(MotionAccessor* accessor) {
+    auto lock = sead::makeScopedLock(mCS);
+    bool ok = !mMotionAccessors.isFull();
+    mMotionAccessors.pushBack(accessor);
+    return ok;
+}
+
+bool RigidBodyRequestMgr::deregisterMotionAccessor(MotionAccessor* accessor) {
+    auto lock = sead::makeScopedLock(mCS);
+
+    int idx = mMotionAccessors.indexOf(accessor);
+    if (idx < 0)
+        return false;
+
+    mMotionAccessors.erase(idx);
+    return true;
+}
+
 }  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h b/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h
index 190c234d..c7ba6f9b 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h
+++ b/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h
@@ -15,6 +15,7 @@
 
 namespace ksys::phys {
 
+class MotionAccessor;
 class RigidBody;
 
 class RigidBodyRequestMgr : public sead::hostio::Node {
@@ -24,7 +25,10 @@ public:
 
     void init(sead::Heap* heap);
 
-    void sub_7100FA6C8C(bool, RigidBody*);
+    bool pushRigidBody(int type, RigidBody* body);
+
+    bool registerMotionAccessor(MotionAccessor* accessor);
+    bool deregisterMotionAccessor(MotionAccessor* accessor);
 
 private:
     struct Unk1;
@@ -74,7 +78,7 @@ private:
     util::LockFreeQueue<Unk3> _b0;
     util::LockFreeQueue<Unk4> _c8;
     util::LockFreeQueue<Unk4> _e0;
-    sead::PtrArray<void*> _f8;
+    sead::PtrArray<MotionAccessor> mMotionAccessors;
     sead::Buffer<Unk5> _108;
     sead::Atomic<u32> _118;
     sead::Buffer<Unk6> _120;
diff --git a/src/KingSystem/Utils/Container/LockFreeQueue.h b/src/KingSystem/Utils/Container/LockFreeQueue.h
index 8f9be9b7..1f6d5817 100644
--- a/src/KingSystem/Utils/Container/LockFreeQueue.h
+++ b/src/KingSystem/Utils/Container/LockFreeQueue.h
@@ -29,6 +29,7 @@ public:
 
     void freeBuffer() { mBuffer.freeBuffer(); }
 
+    int getSize() const { return mWriteIdx - mReadIdx; }
     int getCapacity() const { return mBuffer.size(); }
 
     /// Pop an element from the front of the queue. Non-blocking.