ksys/phys: Implement world/shape casting RayCast functions

3 files changed, 206 insertions, 47 deletions

diff --git a/src/KingSystem/Physics/System/physRayCast.cpp b/src/KingSystem/Physics/System/physRayCast.cpp
index a44e1203..34a03926 100644
--- a/src/KingSystem/Physics/System/physRayCast.cpp
+++ b/src/KingSystem/Physics/System/physRayCast.cpp
@@ -6,8 +6,12 @@
 #include <Havok/Physics2012/Collide/Query/CastUtil/hkpWorldRayCastOutput.h>
 #include <Havok/Physics2012/Collide/Query/hkpRayHitCollector.h>
 #include <Havok/Physics2012/Collide/Shape/Convex/Triangle/hkpTriangleShape.h>
+#include <Havok/Physics2012/Collide/Shape/Query/hkpShapeRayCastInput.h>
+#include <Havok/Physics2012/Dynamics/Entity/hkpRigidBody.h>
 #include <Havok/Physics2012/Dynamics/World/hkpWorld.h>
 #include <Havok/Physics2012/Internal/Collide/StaticCompound/hkpStaticCompoundShape.h>
+#include <prim/seadScopeGuard.h>
+#include "KingSystem/Physics/RigidBody/physRigidBody.h"
 #include "KingSystem/Physics/System/physEntityContactListener.h"
 #include "KingSystem/Physics/System/physGroupFilter.h"
 #include "KingSystem/Physics/System/physSystem.h"
@@ -47,12 +51,12 @@ class NormalCheckingRayHitCollector : public RayHitCollector {
 public:
     HK_DECLARE_CLASS_ALLOCATOR(NormalCheckingRayHitCollector)
 
-    NormalCheckingRayHitCollector(hkpWorldRayCastOutput* output, ContactLayerType layer_type,
+    NormalCheckingRayHitCollector(hkpWorldRayCastOutput* output, const sead::Vector3f& ray_line,
+                                  RayCast::NormalCheckingMode mode, ContactLayerType layer_type,
                                   const sead::PtrArray<SystemGroupHandler>* ignored_groups,
-                                  GroundHit ignored_ground_hit, const sead::Vector3f* ray_line,
-                                  RayCast::NormalCheckingMode mode)
+                                  GroundHit ignored_ground_hit)
         : RayHitCollector(output, layer_type, ignored_groups, ignored_ground_hit),
-          mRayLine(ray_line), mMode(mode) {}
+          mRayLine(&ray_line), mMode(mode) {}
 
     ~NormalCheckingRayHitCollector() override;
 
@@ -77,7 +81,7 @@ void RayCast::reset() {
     mTo = sead::Vector3f::zero;
     mFrom = sead::Vector3f::zero;
     mLayerMasks = {};
-    mExtraGroupHandlers.clear();
+    mIgnoredGroups.clear();
 
     resetCastResult();
 }
@@ -85,12 +89,12 @@ void RayCast::reset() {
 void RayCast::resetCastResult() {
     static_cast<void>(_98.load());
 
-    _30 = {};
-    _34 = sead::Vector3f::zero;
+    mHasHit = false;
+    mHitNormal = sead::Vector3f::zero;
     mHitFraction = -1.0;
-    _48 = {};
-    _50 = {};
-    _54 = {};
+    mHitCollidable = {};
+    mHitShapeKey = {};
+    mHasHitSpecifiedRigidBody = false;
     _58 = {};
     _60 = {};
     _70 = {};
@@ -146,8 +150,8 @@ GroundHit RayCast::getGroundHit() const {
     return mGroundHit;
 }
 
-void RayCast::setD8(int value) {
-    _d8 = value;
+void RayCast::setIgnoredGroundHit(GroundHit ground_hit) {
+    mIgnoredGroundHit = ground_hit;
 }
 
 void RayCast::set9A(bool value) {
@@ -180,11 +184,11 @@ void RayCast::setStartAndDisplacementScaled(const sead::Vector3f& start,
     setEnd(start + displacement * displacement_scale);
 }
 
-bool RayCast::addGroupHandler(SystemGroupHandler* group_handler) {
-    if (mExtraGroupHandlers.size() == mExtraGroupHandlers.capacity() || group_handler == nullptr)
+bool RayCast::addIgnoredGroup(SystemGroupHandler* group_handler) {
+    if (mIgnoredGroups.size() == mIgnoredGroups.capacity() || group_handler == nullptr)
         return false;
 
-    mExtraGroupHandlers.pushBack(group_handler);
+    mIgnoredGroups.pushBack(group_handler);
     return true;
 }
 
@@ -193,12 +197,154 @@ void RayCast::setRigidBody(RigidBody* body) {
         mRigidBody = body;
 }
 
-void RayCast::getHitPosition(sead::Vector3f* out) const {
-    *out = ((1 - mHitFraction) * mFrom) + (mHitFraction * mTo);
+void RayCast::fillCastInput(hkpWorldRayCastInput& input, ContactLayerType layer_type) {
+    const u32 layer_mask = getLayerMask(layer_type);
+    input.m_enableShapeCollectionFilter = true;
+    input.m_filterInfo = getFilterInfo(layer_type, layer_mask);
+    input.m_from = toHkVec4(mFrom);
+    input.m_to = toHkVec4(mTo);
 }
 
-void RayCast::get34(sead::Vector3f* out) const {
-    *out = _34;
+void RayCast::fillCastInput(hkpShapeRayCastInput& input, ContactLayerType layer_type) {
+    input.m_filterInfo = getFilterInfo(layer_type, getLayerMask(layer_type));
+}
+
+u32 RayCast::getFilterInfo(ContactLayerType layer_type, u32 layer_mask) const {
+    if (mGroupHandler != nullptr) {
+        return mGroupHandler->makeQueryCollisionMask(layer_mask, mGroundHit, _9a);
+    } else {
+        auto* filter = System::instance()->getGroupFilter(layer_type);
+        auto info = filter->makeQueryCollisionMask(layer_mask, mGroundHit, _9a);
+#ifdef MATCHING_HACK_NX_CLANG
+        asm("");
+#endif
+        return info;
+    }
+}
+
+void RayCast::preCast() {
+    if (_70 == 1)
+        resetCastResult();
+    _98 = true;
+}
+
+bool RayCast::postCast(const hkpWorldRayCastOutput& output) {
+    mHasHit = output.hasHit();
+    if (mHasHit) {
+        updateHitInformation(output);
+        getActorInfoMaybe(output);
+    }
+    _98 = false;
+    _70 = 1;
+    return mHasHit;
+}
+
+void RayCast::updateHitInformation(const hkpWorldRayCastOutput& output) {
+    storeToVec3(&mHitNormal, output.m_normal);
+    mHitFraction = output.m_hitFraction;
+    mHitCollidable = output.m_rootCollidable;
+    mHitShapeKey = output.m_shapeKeys[0];
+
+    auto* hit_body = getRigidBody(*mHitCollidable);
+    if (hit_body) {
+        if (mRigidBody != nullptr && hit_body == mRigidBody)
+            mHasHitSpecifiedRigidBody = true;
+
+        onRigidBodyHit(hit_body);
+        if (mRigidBodyHitCallback)
+            mRigidBodyHitCallback->invoke(hit_body);
+    }
+}
+
+bool RayCast::worldRayCast(ContactLayerType layer_type) {
+    hkpWorldRayCastOutput output;
+    preCast();
+    worldRayCastImpl(&output, layer_type);
+    return postCast(output);
+}
+
+void RayCast::worldRayCastImpl(hkpWorldRayCastOutput* output, ContactLayerType layer_type) {
+    hkpWorldRayCastInput input;
+    fillCastInput(input, layer_type);
+
+    System::instance()->lockWorld(layer_type, "raycast", 0, OnlyLockIfNeeded::Yes);
+    auto world_guard = sead::makeScopeGuard(
+        [&] { System::instance()->unlockWorld(layer_type, "raycast", 0, OnlyLockIfNeeded::Yes); });
+
+    if (mNormalCheckingMode == NormalCheckingMode::DoNotCheck) {
+        if (mIgnoredGroups.size() > 0 || int(mIgnoredGroundHit) != GroundHit::Ignore) {
+            RayHitCollector collector{output, layer_type,
+                                      mIgnoredGroups.size() > 0 ? &mIgnoredGroups : nullptr,
+                                      mIgnoredGroundHit};
+            System::instance()->getHavokWorld(layer_type)->castRay(input, collector);
+        } else {
+            System::instance()->getHavokWorld(layer_type)->castRay(input, *output);
+        }
+
+    } else {
+        sead::Vector3f ray_line = mTo - mFrom;
+        ray_line.normalize();
+
+        auto* groups = mIgnoredGroups.size() > 0 ? &mIgnoredGroups : nullptr;
+        NormalCheckingRayHitCollector collector(output, ray_line, mNormalCheckingMode, layer_type,
+                                                groups, mIgnoredGroundHit);
+        System::instance()->getHavokWorld(layer_type)->castRay(input, collector);
+    }
+}
+
+bool RayCast::shapeRayCast(RigidBody* rigid_body) {
+    hkpWorldRayCastOutput output;
+    preCast();
+    shapeRayCastImpl(&output, rigid_body);
+    return postCast(output);
+}
+
+void RayCast::shapeRayCastImpl(hkpWorldRayCastOutput* output, RigidBody* body) {
+    hkpShapeRayCastInput input;
+    auto layer_type = body->getLayerType();
+    fillCastInput(input, layer_type);
+
+    sead::Matrix34f transform;
+    body->getTransform(&transform);
+    transform.invert();
+    const auto from = transform * mFrom;
+    const auto to = transform * mTo;
+    input.m_from = toHkVec4(from);
+    input.m_to = toHkVec4(to);
+
+    System::instance()->incrementWorldUnkCounter(ContactLayerType::Entity);
+    auto world_guard = sead::makeScopeGuard(
+        [&] { System::instance()->decrementWorldUnkCounter(ContactLayerType::Entity); });
+
+    auto* collidable = body->getHkBody()->getCollidable();
+
+    if (mNormalCheckingMode == NormalCheckingMode::DoNotCheck) {
+        if (mIgnoredGroups.size() > 0 || int(mIgnoredGroundHit) != GroundHit::Ignore) {
+            RayHitCollector collector{output, layer_type,
+                                      mIgnoredGroups.size() > 0 ? &mIgnoredGroups : nullptr,
+                                      mIgnoredGroundHit};
+            collidable->getShape()->castRayWithCollector(input, *collidable, collector);
+        } else {
+            collidable->getShape()->castRay(input, *output);
+        }
+
+    } else {
+        sead::Vector3f ray_line = mTo - mFrom;
+        ray_line.normalize();
+
+        auto* groups = mIgnoredGroups.size() > 0 ? &mIgnoredGroups : nullptr;
+        NormalCheckingRayHitCollector collector(output, ray_line, mNormalCheckingMode, layer_type,
+                                                groups, mIgnoredGroundHit);
+        collidable->getShape()->castRayWithCollector(input, *collidable, collector);
+    }
+}
+
+void RayCast::getHitPosition(sead::Vector3f* position) const {
+    *position = ((1 - mHitFraction) * mFrom) + (mHitFraction * mTo);
+}
+
+void RayCast::getHitNormal(sead::Vector3f* normal) const {
+    *normal = mHitNormal;
 }
 
 RayHitCollector::~RayHitCollector() = default;
diff --git a/src/KingSystem/Physics/System/physRayCast.h b/src/KingSystem/Physics/System/physRayCast.h
index 58cd5f5d..bd11a64f 100644
--- a/src/KingSystem/Physics/System/physRayCast.h
+++ b/src/KingSystem/Physics/System/physRayCast.h
@@ -9,6 +9,12 @@
 #include "KingSystem/Physics/physDefines.h"
 #include "KingSystem/Physics/physMaterialMask.h"
 
+class hkpCollidable;
+struct hkpShapeRayCastInput;
+struct hkpShapeRayCastOutput;
+struct hkpWorldRayCastInput;
+struct hkpWorldRayCastOutput;
+
 namespace ksys::phys {
 
 struct ActorInfo;
@@ -25,13 +31,12 @@ public:
         DoNotCheck = 2,
     };
 
-    // TODO: what kind of callback is this?
-    using Callback = sead::IDelegate1<phys::RigidBody*>;
+    using RigidBodyHitCallback = sead::IDelegate1<phys::RigidBody*>;
 
     RayCast(SystemGroupHandler* group_handler, GroundHit ground_hit);
     virtual ~RayCast();
 
-    virtual void m4(void*) {}
+    virtual void onRigidBodyHit(RigidBody* body) {}
 
     void reset();
     void resetCastResult();
@@ -44,8 +49,8 @@ public:
     void setGroundHit(GroundHit ground_hit);
     GroundHit getGroundHit() const;
 
+    void setIgnoredGroundHit(GroundHit ground_hit);
     // TODO: rename
-    void setD8(int value);
     void set9A(bool value);
 
     void setStart(const sead::Vector3f& start);
@@ -56,22 +61,20 @@ public:
                                        const sead::Vector3f& displacement,
                                        float displacement_scale);
 
-    bool addGroupHandler(SystemGroupHandler* group_handler);
+    /// @warning Only up to 4 groups can be ignored.
+    bool addIgnoredGroup(SystemGroupHandler* group_handler);
     void setRigidBody(RigidBody* body);
 
-    void setCallback(Callback* callback) { mCallback = callback; }
-    Callback* getCallback() const { return mCallback; }
+    void setCallback(RigidBodyHitCallback* callback) { mRigidBodyHitCallback = callback; }
+    RigidBodyHitCallback* getCallback() const { return mRigidBodyHitCallback; }
 
-    // 0x0000007100fc39b4
     bool worldRayCast(ContactLayerType layer_type);
-    // 0x0000007100fc3da4
     bool shapeRayCast(RigidBody* rigid_body);
     // 0x0000007100fc4248
     bool phantomRayCast(void* unk);
 
-    void getHitPosition(sead::Vector3f* out) const;
-    // TODO: rename
-    void get34(sead::Vector3f* out) const;
+    void getHitPosition(sead::Vector3f* position) const;
+    void getHitNormal(sead::Vector3f* normal) const;
     // TODO: rename
     // 0x0000007100fc4844
     void getUnkVectors(sead::Vector3f* unk1, sead::Vector3f* unk2, void* unk3) const;
@@ -79,34 +82,40 @@ public:
     // 0x0000007100fc4bd4
     bool x_1(sead::Vector3f* out) const;
 
-private:
+protected:
     auto& getLayerMask(ContactLayerType type) { return mLayerMasks[int(type)]; }
     auto& getLayerMask(ContactLayerType type) const { return mLayerMasks[int(type)]; }
 
-    // 0x0000007100fc3b1c
-    void worldRayCastImpl(void* arg, ContactLayerType layer_type);
-    // 0x0000007100fc3f0c
-    void shapeRayCastImpl(void* arg, RigidBody* rigid_body);
+    void worldRayCastImpl(hkpWorldRayCastOutput* output, ContactLayerType layer_type);
+    void shapeRayCastImpl(hkpWorldRayCastOutput* output, RigidBody* body);
     // 0x0000007100fc43b0
-    void phantomRayCastImpl(void* unk);
+    void phantomRayCastImpl(hkpWorldRayCastOutput* output);
 
     // 0x0000007100fc4630
-    const ActorInfo* getActorInfoMaybe(void* unk);
+    const ActorInfo* getActorInfoMaybe(const hkpShapeRayCastOutput& output);
 
     // 0x0000007100fc4764
     bool x_2(sead::Vector3f* out, void* unk1, int unk2) const;
 
+    void fillCastInput(hkpWorldRayCastInput& input, ContactLayerType layer_type);
+    void fillCastInput(hkpShapeRayCastInput& input, ContactLayerType layer_type);
+    u32 getFilterInfo(ContactLayerType layer_type, u32 layer_mask) const;
+
+    void preCast();
+    bool postCast(const hkpWorldRayCastOutput& output);
+    void updateHitInformation(const hkpWorldRayCastOutput& output);
+
     sead::Vector3f mFrom = sead::Vector3f::zero;
     sead::Vector3f mTo = sead::Vector3f::zero;
     SystemGroupHandler* mGroupHandler{};
     GroundHit mGroundHit{};
     u32 _2c;
-    bool _30;
-    sead::Vector3f _34;
+    bool mHasHit;
+    sead::Vector3f mHitNormal;
     float mHitFraction;
-    void* _48;
-    u32 _50;
-    bool _54;
+    const hkpCollidable* mHitCollidable;
+    u32 mHitShapeKey;
+    bool mHasHitSpecifiedRigidBody;
     void* _58{};
     void* _60;
     sead::SafeArray<sead::BitFlag32, NumContactLayerTypes> mLayerMasks{};
@@ -117,10 +126,9 @@ private:
     sead::Atomic<bool> _98;
     bool _99{};
     bool _9a{};
-    sead::FixedPtrArray<SystemGroupHandler, 4> mExtraGroupHandlers;
-    // TODO: rename once we figure out what kind of callback this is
-    Callback* mCallback{};
-    u32 _d8 = 0x10;
+    sead::FixedPtrArray<SystemGroupHandler, 4> mIgnoredGroups;
+    RigidBodyHitCallback* mRigidBodyHitCallback{};
+    GroundHit mIgnoredGroundHit = GroundHit::Ignore;
 };
 
 }  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/System/physSystem.h b/src/KingSystem/Physics/System/physSystem.h
index 8c61d82d..6b130476 100644
--- a/src/KingSystem/Physics/System/physSystem.h
+++ b/src/KingSystem/Physics/System/physSystem.h
@@ -102,6 +102,11 @@ public:
     // 0x0000007101216814
     bool getEntityContactListenerField91() const;
 
+    // 0x000000710121682c
+    void incrementWorldUnkCounter(ContactLayerType layer_type);
+    // 0x000000710121684c
+    void decrementWorldUnkCounter(ContactLayerType layer_type);
+
     // 0x0000007101216cec
     sead::Heap* getPhysicsTempHeap(LowPriority low_priority) const;