ksys/phys: Add RagdollControllerMgr

Also contains readability/accuracy fixes to LockFreeQueue.

data/uking_functions.csv

@@ -95329,14 +95329,14 @@ Address,Quality,Size,Name
 0x00000071012ac688,O,000084,_ZN4ksys4phys24RagdollControllerKeyList20RagdollControllerKeyD2Ev
 0x00000071012ac6dc,O,000004,_ZN4ksys4phys24RagdollControllerKeyList20RagdollControllerKeyD0Ev
 0x00000071012ac6e0,O,000372,_ZN4ksys4phys24RagdollControllerKeyList33checkDerivedRuntimeTypeInfoStaticEPKN4sead15RuntimeTypeInfo9InterfaceE
-0x00000071012ac854,U,000040,
-0x00000071012ac87c,U,000068,
-0x00000071012ac8c0,U,000076,
-0x00000071012ac90c,U,000188,
-0x00000071012ac9c8,U,000148,
-0x00000071012aca5c,U,000128,
-0x00000071012acadc,U,000348,
-0x00000071012acc38,U,000004,nullsub_4772
+0x00000071012ac854,O,000040,_ZN4ksys4phys20RagdollControllerMgrC1Ev
+0x00000071012ac87c,O,000068,_ZN4ksys4phys20RagdollControllerMgrD1Ev
+0x00000071012ac8c0,O,000076,_ZN4ksys4phys20RagdollControllerMgrD0Ev
+0x00000071012ac90c,O,000188,_ZN4ksys4phys20RagdollControllerMgr4initEPN4sead4HeapE
+0x00000071012ac9c8,O,000148,_ZN4ksys4phys20RagdollControllerMgr13addControllerEPNS0_17RagdollControllerE
+0x00000071012aca5c,O,000128,_ZN4ksys4phys20RagdollControllerMgr16removeControllerEPNS0_17RagdollControllerE
+0x00000071012acadc,M,000348,_ZN4ksys4phys20RagdollControllerMgr4calcEv
+0x00000071012acc38,O,000004,_ZN4ksys4phys20RagdollControllerMgr5calc1Ev
 0x00000071012acc3c,O,000048,_ZN4ksys4phys15RagdollResourceC1Ev
 0x00000071012acc6c,O,000004,_ZN4ksys4phys15RagdollResourceD1Ev
 0x00000071012acc70,O,000036,_ZN4ksys4phys15RagdollResourceD0Ev

src/KingSystem/Physics/CMakeLists.txt

@@ -11,6 +11,10 @@ target_sources(uking PRIVATE
   Ragdoll/physRagdollConfig.h
   Ragdoll/physRagdollControllerKeyList.h
   Ragdoll/physRagdollControllerKeyList.cpp
+  Ragdoll/physRagdollController.cpp
+  Ragdoll/physRagdollController.h
+  Ragdoll/physRagdollControllerMgr.cpp
+  Ragdoll/physRagdollControllerMgr.h
   Ragdoll/physRagdollParam.cpp
   Ragdoll/physRagdollParam.h
   Ragdoll/physRagdollResource.cpp

src/KingSystem/Physics/Ragdoll/physRagdollController.cpp

@@ -0,0 +1 @@
+#include "KingSystem/Physics/Ragdoll/physRagdollController.h"

src/KingSystem/Physics/Ragdoll/physRagdollController.h

@@ -0,0 +1,11 @@
+#pragma once
+
+namespace ksys::phys {
+
+// TODO
+class RagdollController {
+public:
+    void update();
+};
+
+}  // namespace ksys::phys

src/KingSystem/Physics/Ragdoll/physRagdollControllerMgr.cpp

@@ -0,0 +1,49 @@
+#include "KingSystem/Physics/Ragdoll/physRagdollControllerMgr.h"
+#include <prim/seadScopedLock.h>
+#include "KingSystem/Physics/Ragdoll/physRagdollController.h"
+#include "KingSystem/Physics/System/physSystem.h"
+
+namespace ksys::phys {
+
+RagdollControllerMgr::RagdollControllerMgr() = default;
+
+RagdollControllerMgr::~RagdollControllerMgr() {
+    mControllers.freeBuffer();
+}
+
+void RagdollControllerMgr::init(sead::Heap* heap) {
+    mControllers.alloc(0x400, heap);
+}
+
+bool RagdollControllerMgr::addController(RagdollController* controller) {
+    return mControllers.push(controller);
+}
+
+void RagdollControllerMgr::removeController(RagdollController* controller) {
+    auto lock = sead::makeScopedLock(mCS);
+    mControllers.erase(controller);
+}
+
+void RagdollControllerMgr::calc() {
+    ScopedWorldLock world_lock{ContactLayerType::Entity};
+    auto lock = sead::makeScopedLock(mCS);
+
+    RagdollController* first_processed = nullptr;
+
+    while (auto* ctrl = mControllers.pop()) {
+        mControllers.push(ctrl);
+
+        // If we have processed the entire buffer and wrapped around, stop.
+        if (first_processed == ctrl)
+            break;
+
+        ctrl->update();
+
+        if (first_processed == nullptr)
+            first_processed = ctrl;
+    }
+}
+
+void RagdollControllerMgr::calc1() {}
+
+}  // namespace ksys::phys

src/KingSystem/Physics/Ragdoll/physRagdollControllerMgr.h

@@ -0,0 +1,32 @@
+#pragma once
+
+#include <thread/seadCriticalSection.h>
+#include "KingSystem/Utils/Container/LockFreeQueue.h"
+
+namespace sead {
+class Heap;
+}
+
+namespace ksys::phys {
+
+class RagdollController;
+
+class RagdollControllerMgr {
+public:
+    RagdollControllerMgr();
+    virtual ~RagdollControllerMgr();
+
+    void init(sead::Heap* heap);
+
+    bool addController(RagdollController* controller);
+    void removeController(RagdollController* controller);
+
+    void calc();
+    void calc1();
+
+private:
+    util::LockFreeQueue<RagdollController> mControllers;
+    sead::CriticalSection mCS;
+};
+
+}  // namespace ksys::phys

src/KingSystem/Utils/Container/LockFreeQueue.h

@@ -32,30 +32,25 @@ public:
 
     /// Pop an element from the front of the queue. Non-blocking.
     /// @returns a non-null pointer to an element or nullptr if the queue is empty.
+    // NON_MATCHING: control flow is a total mess
     T* pop() {
-        while (true) {
-            int read_idx = mReadIdx;
-            if (mWriteIdx - read_idx <= 0) {
-                // The queue is empty.
-                return nullptr;
-            }
-
+        while (getSize() > 0) {
             // No need to check if mWriteIdx has changed: the only thing that *could*
             // happen is mWriteIdx being incremented by a producer. In that situation
             // mWriteIdx - read_idx > 0 still holds.
 
-            T* value = get(read_idx);
+            T* value = get(mReadIdx);
             if (!value) {
                 SEAD_ASSERT_MSG(false, "corrupted buffer");
                 return nullptr;
             }
 
             // Advance the read position and return the value if it's not a dummy pointer.
-            mReadIdx = read_idx + 1;
-            get(read_idx) = nullptr;
+            get(mReadIdx++) = nullptr;
             if (uintptr_t(value) != uintptr_t(-1))
                 return value;
         }
+        return nullptr;
     }
 
     /// Push a new element at the back of the queue. Non-blocking: may fail if the queue is full.
@@ -65,8 +60,8 @@ public:
             return false;
 
         while (true) {
-            const int write_idx = mWriteIdx;
-            if (write_idx - mReadIdx >= getCapacity()) {
+            const u32 write_idx = mWriteIdx;
+            if (int(write_idx) - int(mReadIdx) >= getCapacity()) {
                 // The buffer is full.
                 return false;
             }
@@ -85,19 +80,28 @@ public:
         }
     }
 
-private:
-    T*& get(int idx) { return mBuffer(moduloCapacity(idx)); }
-    const T*& get(int idx) const { return mBuffer(moduloCapacity(idx)); }
+    /// Erase an element from the queue.
+    /// @warning Not thread safe!
+    void erase(T* value) {
+        for (u32 i = mReadIdx; i < mWriteIdx; ++i) {
+            if (get(i) == value)
+                get(i) = reinterpret_cast<T*>(uintptr_t(-1));
+        }
+    }
 
-    int moduloCapacity(int idx) const {
+private:
+    T*& get(u32 idx) { return mBuffer(moduloCapacity(idx)); }
+    const T*& get(u32 idx) const { return mBuffer(moduloCapacity(idx)); }
+
+    int moduloCapacity(u32 idx) const {
         // Because the capacity is a power of 2, we can avoid costly divisions
         // and instead rely on x % 2^n being equal to x & (2^n - 1).
         return idx & (getCapacity() - 1);
     }
 
     sead::Buffer<T*> mBuffer;
-    sead::Atomic<int> mWriteIdx;
-    int mReadIdx{};
+    sead::Atomic<u32> mWriteIdx;
+    u32 mReadIdx{};
 };
 
 }  // namespace ksys::util