ksys/phys: Add RayCastRequestMgr

1 files changed, 127 insertions, 0 deletions

diff --git a/src/KingSystem/Physics/System/physRayCastRequestMgr.cpp b/src/KingSystem/Physics/System/physRayCastRequestMgr.cpp
new file mode 100644
index 00000000..cbae2dac
--- /dev/null
+++ b/src/KingSystem/Physics/System/physRayCastRequestMgr.cpp
@@ -0,0 +1,127 @@
+#include "KingSystem/Physics/System/physRayCastRequestMgr.h"
+#include <cmath>
+#include <prim/seadScopedLock.h>
+#include "KingSystem/Framework/frmWorkerSupportThreadMgr.h"
+#include "KingSystem/Physics/System/physRayCastForRequest.h"
+#include "KingSystem/ksys.h"
+
+namespace ksys::phys {
+
+RayCastRequestMgr::RayCastRequestMgr() = default;
+
+RayCastRequestMgr::~RayCastRequestMgr() = default;
+
+void RayCastRequestMgr::init(sead::Heap* heap, int pool_size) {
+    mLargestRecordedQueueSize = 0;
+    for (auto it = mQueuedList.robustBegin(); it != mQueuedList.robustEnd(); it++)
+        mQueuedList.erase(&*it);
+
+    for (int i = 0; i < pool_size; ++i) {
+        auto* ray_cast = new (heap, 0x10) RayCastForRequest(this);
+        mFreeList.pushBack(&ray_cast->mListNode);
+    }
+
+    mWorkerFunction.bind(this, &RayCastRequestMgr::processRequests);
+}
+
+bool RayCastRequestMgr::processRequests(void*) {
+    for (int i = 0; i < mBatchSize; ++i) {
+        mCS.lock();
+        auto* request = mQueuedList.popFront();
+        if (!request) {
+            mCS.unlock();
+            return true;
+        }
+        request->mList = nullptr;
+        mCS.unlock();
+
+        request->mData->worldRayCast(request->mData->mRequestContactLayerType);
+    }
+
+    return true;
+}
+
+void RayCastRequestMgr::clearRequests() {
+    {
+        auto lock = sead::makeScopedLock(mCS);
+        while (!mQueuedList.isEmpty()) {
+            auto* request = mQueuedList.popFront();
+            if (!request)
+                continue;
+
+            auto* ray_cast = request->mData;
+            request->mList = nullptr;
+            releaseRequest(*ray_cast);
+        }
+    }
+    mLargestRecordedQueueSize = 0;
+}
+
+void RayCastRequestMgr::releaseRequest(RayCastForRequest& ray_cast) {
+    auto lock = sead::makeScopedLock(mCS);
+    ray_cast.mRigidBodyHitCallback = nullptr;
+    mFreeList.pushBack(&ray_cast.mListNode);
+}
+
+RayCastForRequest* RayCastRequestMgr::allocRequest(SystemGroupHandler* group_handler,
+                                                   GroundHit ground_hit) {
+    auto lock = sead::makeScopedLock(mCS);
+
+    auto* request = mFreeList.popFront();
+    if (!request)
+        return nullptr;
+
+    request->mList = nullptr;
+
+    if (request->mData->_98) {
+        mFreeList.pushBack(request);
+        return nullptr;
+    }
+
+    request->mData->reset();
+    request->mData->mGroupHandler = group_handler;
+    request->mData->setGroundHit(ground_hit);
+    request->mData->mNormalCheckingMode = RayCast::NormalCheckingMode::_0;
+    return request->mData;
+}
+
+static bool isVectorInvalid(const sead::Vector3f& vec) {
+    for (int i = 0; i < 3; ++i) {
+        if (std::isnan(vec.e[i]))
+            return true;
+    }
+    return false;
+}
+
+bool RayCastRequestMgr::submitRequest(RayCastForRequest& ray_cast, ContactLayerType layer_type) {
+    if (isVectorInvalid(ray_cast.mFrom) || isVectorInvalid(ray_cast.mTo))
+        return false;
+
+    if (ray_cast.mListNode.isLinked())
+        return false;
+
+    auto lock = sead::makeScopedLock(mCS);
+
+    ray_cast.mRequestContactLayerType = layer_type;
+    mQueuedList.pushBack(&ray_cast.mListNode);
+
+    if (mQueuedList.size() > mLargestRecordedQueueSize)
+        mLargestRecordedQueueSize = mQueuedList.size();
+
+    return true;
+}
+
+bool RayCastRequestMgr::isRequestQueued(const RayCastForRequest& ray_cast) const {
+    return ray_cast.mListNode.mList == &mQueuedList;
+}
+
+bool RayCastRequestMgr::isRequestFinished(const RayCastForRequest& ray_cast) const {
+    return ray_cast.mListNode.mList == &mFreeList;
+}
+
+void RayCastRequestMgr::scheduleRequestProcessing() {
+    if (!isGameOver())
+        frm::WorkerSupportThreadMgr::instance()->submitRequest(5, &mWorkerFunction);
+}
+
+}  // namespace ksys::phys