ksys/phys: Add RayHitCollector

2 files changed, 222 insertions, 1 deletions

diff --git a/src/KingSystem/Physics/System/physRayCast.cpp b/src/KingSystem/Physics/System/physRayCast.cpp
index ea25cea3..a44e1203 100644
--- a/src/KingSystem/Physics/System/physRayCast.cpp
+++ b/src/KingSystem/Physics/System/physRayCast.cpp
@@ -1,8 +1,70 @@
 #include "KingSystem/Physics/System/physRayCast.h"
+#include <Havok/Common/Base/hkBase.h>
+#include <Havok/Physics2012/Collide/Agent/Collidable/hkpCdBody.h>
+#include <Havok/Physics2012/Collide/Agent/Collidable/hkpCollidable.h>
+#include <Havok/Physics2012/Collide/Query/CastUtil/hkpWorldRayCastInput.h>
+#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/Dynamics/World/hkpWorld.h>
+#include <Havok/Physics2012/Internal/Collide/StaticCompound/hkpStaticCompoundShape.h>
+#include "KingSystem/Physics/System/physEntityContactListener.h"
+#include "KingSystem/Physics/System/physGroupFilter.h"
+#include "KingSystem/Physics/System/physSystem.h"
+#include "KingSystem/Physics/physConversions.h"
 #include "KingSystem/Physics/physLayerMaskBuilder.h"
 
 namespace ksys::phys {
 
+class RayHitCollector : public hkpRayHitCollector {
+public:
+    HK_DECLARE_CLASS_ALLOCATOR(RayHitCollector)
+
+    RayHitCollector(hkpWorldRayCastOutput* output, ContactLayerType layer_type,
+                    const sead::PtrArray<SystemGroupHandler>* ignored_groups,
+                    GroundHit ignored_ground_hit)
+        : mOutput(output), mLayerType(layer_type), mIgnoredGroups(ignored_groups),
+          mIgnoredGroundHit(ignored_ground_hit) {}
+
+    ~RayHitCollector() override;
+
+    void addRayHit(const hkpCdBody& cdBody, const hkpShapeRayCastCollectorOutput& hitInfo) override;
+
+protected:
+    bool isIgnoredGroup(const hkpCdBody& cdBody,
+                        const hkpShapeRayCastCollectorOutput& hitInfo) const;
+
+    bool isIgnoredGroundHit(const hkpCdBody& cdBody,
+                            const hkpShapeRayCastCollectorOutput& hitInfo) const;
+
+    hkpWorldRayCastOutput* mOutput;
+    ContactLayerType mLayerType;
+    const sead::PtrArray<SystemGroupHandler>* mIgnoredGroups;
+    GroundHit mIgnoredGroundHit;
+};
+
+class NormalCheckingRayHitCollector : public RayHitCollector {
+public:
+    HK_DECLARE_CLASS_ALLOCATOR(NormalCheckingRayHitCollector)
+
+    NormalCheckingRayHitCollector(hkpWorldRayCastOutput* output, ContactLayerType layer_type,
+                                  const sead::PtrArray<SystemGroupHandler>* ignored_groups,
+                                  GroundHit ignored_ground_hit, const sead::Vector3f* ray_line,
+                                  RayCast::NormalCheckingMode mode)
+        : RayHitCollector(output, layer_type, ignored_groups, ignored_ground_hit),
+          mRayLine(ray_line), mMode(mode) {}
+
+    ~NormalCheckingRayHitCollector() override;
+
+    void addRayHit(const hkpCdBody& cdBody, const hkpShapeRayCastCollectorOutput& hitInfo) override;
+
+    bool checkNormal(const hkpCdBody& cdBody, const hkpShapeRayCastCollectorOutput& hitInfo) const;
+
+private:
+    const sead::Vector3f* mRayLine;
+    RayCast::NormalCheckingMode mMode;
+};
+
 RayCast::RayCast(SystemGroupHandler* group_handler, GroundHit ground_hit)
     : mGroupHandler(group_handler), mGroundHit(ground_hit) {
     reset();
@@ -139,4 +201,157 @@ void RayCast::get34(sead::Vector3f* out) const {
     *out = _34;
 }
 
+RayHitCollector::~RayHitCollector() = default;
+
+bool RayHitCollector::isIgnoredGroup(const hkpCdBody& cdBody,
+                                     const hkpShapeRayCastCollectorOutput& hitInfo) const {
+    if (mIgnoredGroups == nullptr ||
+        cdBody.getRootCollidable()->getShape()->getType() == hkcdShapeType::STATIC_COMPOUND) {
+        return false;
+    }
+
+    auto filter_info = cdBody.getRootCollidable()->getCollisionFilterInfo();
+    auto* group_filter = System::instance()->getGroupFilter(mLayerType);
+    auto group_idx = int(group_filter->getCollisionFilterInfoGroupHandlerIdx(filter_info));
+
+    if (group_idx != 0) {
+        for (int i = 0; i < mIgnoredGroups->size(); ++i) {
+            if ((*mIgnoredGroups)[i]->getIndex() == group_idx)
+                return true;
+        }
+    }
+
+    return false;
+}
+
+bool RayHitCollector::isIgnoredGroundHit(const hkpCdBody& cdBody,
+                                         const hkpShapeRayCastCollectorOutput& hitInfo) const {
+    if (mIgnoredGroundHit == GroundHit::Ignore ||
+        cdBody.getRootCollidable()->getShape()->getType() == hkcdShapeType::STATIC_COMPOUND ||
+        mLayerType != ContactLayerType::Entity) {
+        return false;
+    }
+
+    auto filter_info = cdBody.getRootCollidable()->getCollisionFilterInfo();
+    auto body_ground_hit = EntityCollisionMask(filter_info).getGroundHit();
+    if (int(mIgnoredGroundHit) == body_ground_hit)
+        return true;
+
+    return false;
+}
+
+// NON_MATCHING: group_idx check should be a ccmp
+void RayHitCollector::addRayHit(const hkpCdBody& cdBody,
+                                const hkpShapeRayCastCollectorOutput& hitInfo) {
+    bool closer = hitInfo.m_hitFraction < mOutput->m_hitFraction;
+    if (!closer)
+        return;
+
+    if (isIgnoredGroup(cdBody, hitInfo))
+        return;
+
+    if (isIgnoredGroundHit(cdBody, hitInfo))
+        return;
+
+    if (System::instance()->getEntityContactListenerField91() &&
+        mLayerType == ContactLayerType::Entity &&
+        EntityContactListener::isObjectOrGroundOrNPCOrTree(cdBody)) {
+        // XXX: Similar checks show up in various other collectors. Can this be refactored?
+        return;
+    }
+
+    *static_cast<hkpShapeRayCastCollectorOutput*>(mOutput) = hitInfo;
+    mOutput->m_rootCollidable = cdBody.getRootCollidable();
+    shapeKeysFromCdBody(mOutput->m_shapeKeys, hkpShapeRayCastOutput::MAX_HIERARCHY_DEPTH, cdBody);
+    m_earlyOutHitFraction = hitInfo.m_hitFraction;
+}
+
+NormalCheckingRayHitCollector::~NormalCheckingRayHitCollector() = default;
+
+void NormalCheckingRayHitCollector::addRayHit(const hkpCdBody& cdBody,
+                                              const hkpShapeRayCastCollectorOutput& hitInfo) {
+    bool closer = hitInfo.m_hitFraction < mOutput->m_hitFraction;
+    if (!closer)
+        return;
+
+    if (isIgnoredGroup(cdBody, hitInfo))
+        return;
+
+    if (isIgnoredGroundHit(cdBody, hitInfo))
+        return;
+
+    if (!checkNormal(cdBody, hitInfo))
+        return;
+
+    if (System::instance()->getEntityContactListenerField91() &&
+        mLayerType == ContactLayerType::Entity &&
+        EntityContactListener::isObjectOrGroundOrNPCOrTree(cdBody)) {
+        return;
+    }
+
+    *static_cast<hkpShapeRayCastCollectorOutput*>(mOutput) = hitInfo;
+    mOutput->m_rootCollidable = cdBody.getRootCollidable();
+    shapeKeysFromCdBody(mOutput->m_shapeKeys, hkpShapeRayCastOutput::MAX_HIERARCHY_DEPTH, cdBody);
+    m_earlyOutHitFraction = hitInfo.m_hitFraction;
+}
+
+template <bool Invert>
+static bool checkDot(RayCast::NormalCheckingMode mode, float dot) {
+    if (mode == RayCast::NormalCheckingMode::_0) {
+        if constexpr (Invert)
+            return !(dot < 0);
+        else
+            return (dot < 0);
+    } else {
+        if constexpr (Invert)
+            return !(dot > 0);
+        else
+            return (dot > 0);
+    }
+}
+
+bool NormalCheckingRayHitCollector::checkNormal(
+    const hkpCdBody& cdBody, const hkpShapeRayCastCollectorOutput& hitInfo) const {
+    if (cdBody.getShape()->getType() != hkcdShapeType::TRIANGLE)
+        return checkDot<false>(mMode, mRayLine->dot(toVec3(hitInfo.m_normal)));
+
+    auto* triangle = static_cast<const hkpTriangleShape*>(cdBody.getShape());
+
+    auto vertexA = triangle->getVertex<0>();
+    auto vertexB = triangle->getVertex<1>();
+    auto vertexC = triangle->getVertex<2>();
+
+    hkVector4f BminusA;
+    BminusA.setSub(vertexB, vertexA);
+
+    hkVector4f CminusA;
+    CminusA.setSub(vertexC, vertexA);
+
+    hkVector4f triangle_line;
+    triangle_line.setCross(BminusA, CminusA);
+    triangle_line.normalize<3>();
+
+    const auto body_rotation = cdBody.getRootCollidable()->getTransform().getRotation();
+
+    hkRotationf rotation = body_rotation;
+
+    if (cdBody.getRootCollidable()->getShape()->getType() == hkcdShapeType::STATIC_COMPOUND) {
+        auto* sc =
+            static_cast<const hkpStaticCompoundShape*>(cdBody.getRootCollidable()->getShape());
+
+        int instance_id;
+        hkpShapeKey child_key;
+        sc->decomposeShapeKey(cdBody.getShapeKey(), instance_id, child_key);
+
+        hkRotationf shape_rotation;
+        shape_rotation.set(sc->getInstances()[instance_id].getTransform().getRotation());
+        rotation.mul(shape_rotation);
+    }
+
+    hkVector4f rotated_normal;
+    rotated_normal.setRotatedDir(rotation, hitInfo.m_normal);
+
+    return checkDot<true>(mMode, triangle_line.dot<3>(rotated_normal));
+}
+
 }  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/System/physRayCast.h b/src/KingSystem/Physics/System/physRayCast.h
index d2b915e8..58cd5f5d 100644
--- a/src/KingSystem/Physics/System/physRayCast.h
+++ b/src/KingSystem/Physics/System/physRayCast.h
@@ -19,6 +19,12 @@ class SystemGroupHandler;
 class RayCast {
     SEAD_RTTI_BASE(RayCast)
 public:
+    enum class NormalCheckingMode {
+        _0 = 0,
+        _1 = 1,
+        DoNotCheck = 2,
+    };
+
     // TODO: what kind of callback is this?
     using Callback = sead::IDelegate1<phys::RigidBody*>;
 
@@ -105,7 +111,7 @@ private:
     void* _60;
     sead::SafeArray<sead::BitFlag32, NumContactLayerTypes> mLayerMasks{};
     sead::Atomic<u32> _70;
-    sead::Atomic<u32> _74;
+    NormalCheckingMode mNormalCheckingMode;
     MaterialMask mMaterialMask;
     RigidBody* mRigidBody{};
     sead::Atomic<bool> _98;