ksys/phys: Implement several easy RigidBodyRequestMgr functions

3 files changed, 48 insertions, 11 deletions

diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.cpp b/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.cpp
index 957454a2..063ed25d 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 <Havok/Physics2012/Dynamics/World/hkpWorld.h>
 #include <prim/seadScopedLock.h>
 #include "KingSystem/Physics/RigidBody/physRigidBody.h"
 #include "KingSystem/Physics/System/physSystem.h"
@@ -38,8 +39,8 @@ RigidBodyRequestMgr::~RigidBodyRequestMgr() {
 void RigidBodyRequestMgr::init(sead::Heap* heap) {
     constexpr int Buffer138Size = 0x800;
 
-    mImpulseEntriesPool.allocBufferAssert(0x100, heap);
-    _118 = 0;
+    mImpulseEntriesPool.allocBufferAssert(MaxNumImpulseEntries, heap);
+    mNumActiveImpulseEntries = 0;
 
     _120.allocBufferAssert(0x100, heap);
     _130 = 0;
@@ -103,7 +104,7 @@ void RigidBodyRequestMgr::processImpulseEntries() {
         else
             impulse->body_a->onImpulse(impulse->body_b, impulse->impulse_a);
     }
-    _118 = 0;
+    mNumActiveImpulseEntries = 0;
 }
 
 void RigidBodyRequestMgr::processOobRigidBodyEntries(ContactLayerType layer_type) {
@@ -120,6 +121,42 @@ bool RigidBodyRequestMgr::pushRigidBody(ContactLayerType type, RigidBody* body)
     return mRigidBodies1[int(type)].push(body);
 }
 
+void RigidBodyRequestMgr::addEntityToWorld(ContactLayerType type, hkpEntity* entity) {
+    static_cast<void>(System::instance()->isActorSystemIdle());
+
+    auto* world = System::instance()->getHavokWorld(type);
+    if (world->addEntity(entity))
+        ++mNumEntitiesInWorld;
+}
+
+void RigidBodyRequestMgr::removeEntityFromWorld(ContactLayerType type, hkpEntity* entity) {
+    static_cast<void>(System::instance()->isActorSystemIdle());
+
+    auto* world = System::instance()->getHavokWorld(type);
+    if (world->removeEntity(entity))
+        --mNumEntitiesInWorld;
+}
+
+bool RigidBodyRequestMgr::onMaxPositionExceeded(ContactLayerType layer_type, RigidBody* body) {
+    return mOobRigidBodies[int(layer_type)].push(body);
+}
+
+bool RigidBodyRequestMgr::addImpulse(RigidBody* body_a, RigidBody* body_b, float impulse) {
+    int index = mNumActiveImpulseEntries;
+    ++mNumActiveImpulseEntries;
+    if (mNumActiveImpulseEntries >= MaxNumImpulseEntries)
+        return false;
+
+    auto* entry = mImpulseEntriesPool.get(index);
+    entry->body_a = body_a;
+    entry->body_b = body_b;
+    entry->impulse_a = impulse;
+    if (!mImpulseEntries.push(entry))
+        return false;
+
+    return true;
+}
+
 bool RigidBodyRequestMgr::registerMotionAccessor(MotionAccessor* accessor) {
     auto lock = sead::makeScopedLock(mCS);
     bool ok = !mMotionAccessors.isFull();
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h b/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h
index 56afd4d2..1597b643 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h
+++ b/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h
@@ -51,18 +51,14 @@ public:
     void calc1(ContactLayerType layer_type, bool paused);
 
     bool pushRigidBody(ContactLayerType type, RigidBody* body);
-    // 0x0000007100fa6d48
     void addEntityToWorld(ContactLayerType type, hkpEntity* entity);
-    // 0x0000007100fa6dac
-    void removeEntityToWorld(ContactLayerType type, hkpEntity* entity);
+    void removeEntityFromWorld(ContactLayerType type, hkpEntity* entity);
     // 0x0000007100fa6ebc
     void removeRigidBody(ContactLayerType type, RigidBody* body);
 
-    // 0x0000007100fa728c
-    void onMaxPositionExceeded(ContactLayerType layer_type, RigidBody* body);
+    bool onMaxPositionExceeded(ContactLayerType layer_type, RigidBody* body);
 
-    // 0x0000007100fa7340
-    void addImpulse(RigidBody* body_a, RigidBody* body_b, float impulse);
+    bool addImpulse(RigidBody* body_a, RigidBody* body_b, float impulse);
 
     bool registerMotionAccessor(MotionAccessor* accessor);
     bool deregisterMotionAccessor(MotionAccessor* accessor);
@@ -113,6 +109,7 @@ private:
     void processOobRigidBodyEntries(ContactLayerType layer_type);
 
     static constexpr int NumRigidBodyBuffers = 2;
+    static constexpr int MaxNumImpulseEntries = 0x100;
 
     sead::SafeArray<util::LockFreeQueue<RigidBody>, NumRigidBodyBuffers> mRigidBodies1;
     util::LockFreeQueue<Unk1> _38;
@@ -125,7 +122,7 @@ private:
     util::LockFreeQueue<Unk4> _e0;
     sead::PtrArray<MotionAccessor> mMotionAccessors;
     sead::Buffer<ImpulseEntry> mImpulseEntriesPool;
-    sead::Atomic<u32> _118;
+    sead::Atomic<int> mNumActiveImpulseEntries;
     sead::Buffer<Unk6> _120;
     sead::Atomic<u32> _130;
     sead::Buffer<Unk4> _138;
diff --git a/src/KingSystem/Physics/System/physSystem.h b/src/KingSystem/Physics/System/physSystem.h
index 4e01e60b..9d363650 100644
--- a/src/KingSystem/Physics/System/physSystem.h
+++ b/src/KingSystem/Physics/System/physSystem.h
@@ -112,6 +112,9 @@ public:
     // 0x0000007101216c74
     bool getEntityContactListenerField90() const;
 
+    // 0x0000007101216ca4
+    bool isActorSystemIdle() const;
+
     // 0x0000007101216800
     void setEntityContactListenerField91(bool value);
     // 0x0000007101216814