From 05abdf7e77c553f95e61dba8d2b395014711ec12 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?L=C3=A9o=20Lam?= <leo@leolam.fr>
Date: Mon, 17 Jan 2022 01:41:49 +0100
Subject: ksys/phys: Add easy RigidBodyRequestMgr functions

---
 src/KingSystem/Physics/RigidBody/physRigidBody.cpp |  2 +-
 .../Physics/RigidBody/physRigidBodyRequestMgr.cpp  | 28 ++++++++++++++++++++--
 .../Physics/RigidBody/physRigidBodyRequestMgr.h    |  8 +++++--
 3 files changed, 33 insertions(+), 5 deletions(-)

(limited to 'src/KingSystem/Physics')

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;
-- 
cgit v1.2.3