From 98c69ebaef70dd81d8479f227c33559e0100ff58 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?L=C3=A9o=20Lam?= <leo@leolam.fr>
Date: Wed, 19 Jan 2022 15:50:11 +0100
Subject: [PATCH] ksys/phys: Add one more RigidBody function

---
 data/uking_functions.csv                      |  2 +-
 .../Physics/RigidBody/physRigidBody.cpp       | 46 ++++++++++++++++++-
 .../RigidBody/physRigidBodyRequestMgr.cpp     |  2 +-
 .../RigidBody/physRigidBodyRequestMgr.h       | 10 +++-
 src/KingSystem/Physics/System/physMemSystem.h |  3 ++
 5 files changed, 58 insertions(+), 5 deletions(-)

diff --git a/data/uking_functions.csv b/data/uking_functions.csv
index fb3b163a..09dcd94b 100644
--- a/data/uking_functions.csv
+++ b/data/uking_functions.csv
@@ -82976,7 +82976,7 @@ Address,Quality,Size,Name
 0x0000007100f8d204,O,000012,_ZNK4ksys4phys9RigidBody16isMotionFlag1SetEv
 0x0000007100f8d210,O,000012,_ZNK4ksys4phys9RigidBody16isMotionFlag2SetEv
 0x0000007100f8d21c,O,000236,_ZN4ksys4phys9RigidBody14sub_7100F8D21CEv
-0x0000007100f8d308,U,000888,phys::RigidBody::x_6
+0x0000007100f8d308,O,000888,_ZN4ksys4phys9RigidBody3x_6Ev
 0x0000007100f8d680,O,000140,_ZNK4ksys4phys9RigidBody23getEntityMotionAccessorEv
 0x0000007100f8d70c,O,000156,_ZNK4ksys4phys9RigidBody18getLinkedRigidBodyEv
 0x0000007100f8d7a8,O,000152,_ZNK4ksys4phys9RigidBody20resetLinkedRigidBodyEv
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBody.cpp b/src/KingSystem/Physics/RigidBody/physRigidBody.cpp
index 4dad0822..39cc772d 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBody.cpp
+++ b/src/KingSystem/Physics/RigidBody/physRigidBody.cpp
@@ -231,18 +231,60 @@ bool RigidBody::isMotionFlag2Set() const {
 
 void RigidBody::sub_7100F8D21C() {
     // debug code that survived because mFlags is atomic?
-    static_cast<void>(mFlags.isOn(Flag::_8));
+    static_cast<void>(mFlags.getDirect());
 
     auto lock = sead::makeScopedLock(mCS);
 
     if (mMotionFlags.isOn(MotionFlag::_1)) {
         mMotionFlags.reset(MotionFlag::_1);
         mMotionFlags.set(MotionFlag::_2);
-    } else if (mFlags.isOn(Flag::_8)) {
+    } else if (isFlag8Set()) {
         setMotionFlag(MotionFlag::_2);
     }
 }
 
+bool RigidBody::x_6() {
+    // debug code that survived because mFlags is atomic?
+    static_cast<void>(mFlags.getDirect());
+
+    auto lock = makeScopedLock(false);
+
+    bool result = true;
+
+    if (isFlag8Set()) {
+        mFlags.reset(Flag::_20);
+
+        if (mMotionFlags.isOn(MotionFlag::_1)) {
+            mMotionFlags.reset(MotionFlag::_1);
+            mMotionFlags.set(MotionFlag::_2);
+        }
+
+        setMotionFlag(MotionFlag::_2);
+        result = false;
+    } else if (mFlags.isOn(Flag::UpdateRequested)) {
+        MemSystem::instance()->getRigidBodyRequestMgr()->pushRigidBody(getLayerType(), this);
+        result = false;
+    }
+
+    if (isSensor()) {
+        auto* accessor = getSensorMotionAccessor();
+        if (accessor && accessor->getLinkedRigidBody() != nullptr) {
+            mFlags.reset(Flag::_20);
+            resetLinkedRigidBody();
+            result = false;
+        }
+    } else if (mMotionAccessor &&
+               getEntityMotionAccessor()->hasFlag(RigidBodyMotionEntity::Flag::_2)) {
+        mFlags.reset(Flag::_20);
+        getEntityMotionAccessor()->deregisterAllAccessors();
+        result = false;
+    }
+
+    mFlags.set(Flag::_20);
+    mFlags.set(Flag::_4);
+    return result;
+}
+
 RigidBodyMotionEntity* RigidBody::getEntityMotionAccessor() const {
     return sead::DynamicCast<RigidBodyMotionEntity>(mMotionAccessor);
 }
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.cpp b/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.cpp
index c88b4795..886da47b 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.cpp
+++ b/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.cpp
@@ -59,7 +59,7 @@ void RigidBodyRequestMgr::init(sead::Heap* heap) {
         _e0.push(&_138[i]);
     }
 
-    _148 = 0;
+    mNumEntitiesInWorld = 0;
 
     mContactPoints =
         RigidContactPointsEx::make(heap, 0x1000, 11, "RigidBodyRequestMgr::Water", 0, 0, 0);
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h b/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h
index f18a6bf0..ca53405b 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h
+++ b/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h
@@ -14,6 +14,8 @@
 #include "KingSystem/Utils/Container/LockFreeQueue.h"
 #include "KingSystem/Utils/Types.h"
 
+class hkpEntity;
+
 namespace ksys::phys {
 
 class MotionAccessor;
@@ -45,6 +47,12 @@ public:
     void init(sead::Heap* heap);
 
     bool pushRigidBody(ContactLayerType type, RigidBody* body);
+    // 0x0000007100fa6d48
+    void addEntityToWorld(ContactLayerType type, hkpEntity* entity);
+    // 0x0000007100fa6dac
+    void removeEntityToWorld(ContactLayerType type, hkpEntity* entity);
+    // 0x0000007100fa6ebc
+    void removeRigidBody(ContactLayerType type, RigidBody* body);
 
     bool registerMotionAccessor(MotionAccessor* accessor);
     bool deregisterMotionAccessor(MotionAccessor* accessor);
@@ -103,7 +111,7 @@ private:
     sead::Buffer<Unk6> _120;
     sead::Atomic<u32> _130;
     sead::Buffer<Unk4> _138;
-    u32 _148{};
+    u32 mNumEntitiesInWorld{};
     RigidContactPointsEx* mContactPoints{};
     sead::SafeArray<sead::CriticalSection, NumRigidBodyBuffers> mCriticalSections;
     sead::CriticalSection mCS;
diff --git a/src/KingSystem/Physics/System/physMemSystem.h b/src/KingSystem/Physics/System/physMemSystem.h
index fecebe56..218f915e 100644
--- a/src/KingSystem/Physics/System/physMemSystem.h
+++ b/src/KingSystem/Physics/System/physMemSystem.h
@@ -52,6 +52,9 @@ public:
     void registerContactPointLayerPair(RigidContactPointsEx* points, ContactLayer layer1,
                                        ContactLayer layer2, bool enabled);
 
+    // 0x0000007101216a20
+    void x_1(RigidBody* body);
+
     void removeSystemGroupHandler(SystemGroupHandler* handler);
 
     void lockWorld(ContactLayerType type, void* a = nullptr, int b = 0, bool c = false);