diff --git a/data/uking_functions.csv b/data/uking_functions.csv index 6f0069a7..d17ee8f2 100644 --- a/data/uking_functions.csv +++ b/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 diff --git a/src/KingSystem/Physics/CMakeLists.txt b/src/KingSystem/Physics/CMakeLists.txt index 228071cc..aca9a911 100644 --- a/src/KingSystem/Physics/CMakeLists.txt +++ b/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 diff --git a/src/KingSystem/Physics/Ragdoll/physRagdollController.cpp b/src/KingSystem/Physics/Ragdoll/physRagdollController.cpp new file mode 100644 index 00000000..fee0b35f --- /dev/null +++ b/src/KingSystem/Physics/Ragdoll/physRagdollController.cpp @@ -0,0 +1 @@ +#include "KingSystem/Physics/Ragdoll/physRagdollController.h" diff --git a/src/KingSystem/Physics/Ragdoll/physRagdollController.h b/src/KingSystem/Physics/Ragdoll/physRagdollController.h new file mode 100644 index 00000000..c248e046 --- /dev/null +++ b/src/KingSystem/Physics/Ragdoll/physRagdollController.h @@ -0,0 +1,11 @@ +#pragma once + +namespace ksys::phys { + +// TODO +class RagdollController { +public: + void update(); +}; + +} // namespace ksys::phys diff --git a/src/KingSystem/Physics/Ragdoll/physRagdollControllerMgr.cpp b/src/KingSystem/Physics/Ragdoll/physRagdollControllerMgr.cpp new file mode 100644 index 00000000..c74ad207 --- /dev/null +++ b/src/KingSystem/Physics/Ragdoll/physRagdollControllerMgr.cpp @@ -0,0 +1,49 @@ +#include "KingSystem/Physics/Ragdoll/physRagdollControllerMgr.h" +#include +#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 diff --git a/src/KingSystem/Physics/Ragdoll/physRagdollControllerMgr.h b/src/KingSystem/Physics/Ragdoll/physRagdollControllerMgr.h new file mode 100644 index 00000000..54e89219 --- /dev/null +++ b/src/KingSystem/Physics/Ragdoll/physRagdollControllerMgr.h @@ -0,0 +1,32 @@ +#pragma once + +#include +#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 mControllers; + sead::CriticalSection mCS; +}; + +} // namespace ksys::phys diff --git a/src/KingSystem/Utils/Container/LockFreeQueue.h b/src/KingSystem/Utils/Container/LockFreeQueue.h index 28de8593..f072d69d 100644 --- a/src/KingSystem/Utils/Container/LockFreeQueue.h +++ b/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(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 mBuffer; - sead::Atomic mWriteIdx; - int mReadIdx{}; + sead::Atomic mWriteIdx; + u32 mReadIdx{}; }; } // namespace ksys::util