ksys/phys: Finish SensorGroupFilter

4 files changed, 171 insertions, 15 deletions

diff --git a/src/KingSystem/Physics/System/physEntityGroupFilter.cpp b/src/KingSystem/Physics/System/physEntityGroupFilter.cpp
index 311f7fb4..207de64b 100644
--- a/src/KingSystem/Physics/System/physEntityGroupFilter.cpp
+++ b/src/KingSystem/Physics/System/physEntityGroupFilter.cpp
@@ -7,9 +7,9 @@
 #include <Havok/Physics2012/Collide/Shape/Compound/Tree/hkpBvTreeShape.h>
 #include <Havok/Physics2012/Collide/Shape/Query/hkpShapeRayCastInput.h>
 #include <Havok/Physics2012/Collide/Shape/hkpShapeContainer.h>
+#include <Havok/Physics2012/Dynamics/Entity/hkpEntity.h>
 #include <Havok/Physics2012/Dynamics/World/hkpWorldObject.h>
 #include <heap/seadHeap.h>
-#include "Havok/Physics2012/Dynamics/Entity/hkpEntity.h"
 #include "KingSystem/Physics/RigidBody/physRigidBody.h"
 #include "KingSystem/Physics/System/physContactMgr.h"
 #include "KingSystem/Physics/System/physSystem.h"
@@ -32,7 +32,7 @@ bool receiverMaskGetSensorLayerMaskForType(SensorCollisionMask* mask,
 
 void receiverMaskSetSensorLayerMask(SensorCollisionMask* mask, u32 layer_mask) {
     *mask = {};
-    mask->layer_mask = layer_mask;
+    mask->custom_receiver_data.layer_mask = layer_mask;
     mask->is_custom_receiver = true;
 }
 
diff --git a/src/KingSystem/Physics/System/physSensorGroupFilter.cpp b/src/KingSystem/Physics/System/physSensorGroupFilter.cpp
index acf8c58e..5d339276 100644
--- a/src/KingSystem/Physics/System/physSensorGroupFilter.cpp
+++ b/src/KingSystem/Physics/System/physSensorGroupFilter.cpp
@@ -1,5 +1,16 @@
 #include "KingSystem/Physics/System/physSensorGroupFilter.h"
+#include <Havok/Physics2012/Collide/Agent/Collidable/hkpCollidable.h>
+#include <Havok/Physics2012/Collide/Agent/hkpCollisionInput.h>
+#include <Havok/Physics2012/Collide/Dispatch/hkpCollisionDispatcher.h>
+#include <Havok/Physics2012/Collide/Query/CastUtil/hkpWorldRayCastInput.h>
+#include <Havok/Physics2012/Collide/Shape/Compound/Collection/hkpShapeCollection.h>
+#include <Havok/Physics2012/Collide/Shape/Compound/Tree/hkpBvTreeShape.h>
+#include <Havok/Physics2012/Collide/Shape/Query/hkpShapeRayCastInput.h>
+#include <Havok/Physics2012/Collide/Shape/hkpShapeContainer.h>
+#include <Havok/Physics2012/Dynamics/Entity/hkpEntity.h>
+#include <Havok/Physics2012/Dynamics/World/hkpWorldObject.h>
 #include <basis/seadRawPrint.h>
+#include "KingSystem/Physics/RigidBody/physRigidBody.h"
 #include "KingSystem/Utils/HeapUtil.h"
 
 namespace ksys::phys {
@@ -7,6 +18,11 @@ namespace ksys::phys {
 constexpr int NumSensorHandlersInList0 = 0x10;
 constexpr int NumSensorHandlers = 0x400;
 
+// XXX: find a better name
+static bool testHandler(u32 idx) {
+    return idx != 0 && idx <= 15;
+}
+
 SensorGroupFilter* SensorGroupFilter::make(ContactLayer::ValueType last, sead::Heap* heap) {
     auto* filter = util::alloc<SensorGroupFilter>(heap, FirstSensor, last);
     filter->initFilter(heap);
@@ -20,41 +36,181 @@ SensorGroupFilter::~SensorGroupFilter() = default;
 
 void SensorGroupFilter::doInit_(sead::Heap* heap) {}
 
-// TODO
+hkBool SensorGroupFilter::testCollisionForSensors(u32 infoA, u32 infoB) const {
+    if (mInhibitCollisions)
+        return false;
+
+    const SensorCollisionMask a{infoA};
+    const SensorCollisionMask b{infoB};
+    const u32 a_xor_b = infoA ^ infoB;
+
+    constexpr auto GroupHandlerIdxMask = decltype(a.group_handler_index)::GetMask();
+    constexpr auto GroupHandlerIdxShift = decltype(a.group_handler_index)::StartBit();
+
+    if (SensorCollisionMask(a_xor_b).is_custom_receiver) {
+        if (a.is_custom_receiver) {
+            if (!(a.custom_receiver_data.layer_mask & (1 << b.data.layer)))
+                return false;
+
+            // TODO: this block of code shows up several times in this function
+            //       and in both EntityGroupFilter and SensorGroupFilter. Can this be refactored?
+            if ((a_xor_b & GroupHandlerIdxMask) == 0) {
+                if (((infoA & GroupHandlerIdxMask) >> GroupHandlerIdxShift) > 15)
+                    return false;
+            } else if (testHandler(a.group_handler_index) || testHandler(b.group_handler_index)) {
+                return false;
+            }
+            return true;
+        } else {
+            if (!(b.custom_receiver_data.layer_mask & (1 << a.data.layer)))
+                return false;
+
+            if ((a_xor_b & GroupHandlerIdxMask) == 0) {
+                if (((infoA & GroupHandlerIdxMask) >> GroupHandlerIdxShift) > 15)
+                    return false;
+            } else if (testHandler(a.group_handler_index) || testHandler(b.group_handler_index)) {
+                return false;
+            }
+            return true;
+        }
+
+    } else {
+        if ((a_xor_b & GroupHandlerIdxMask) == 0) {
+            if (((infoA & GroupHandlerIdxMask) >> GroupHandlerIdxShift) > 15)
+                return false;
+        } else if (testHandler(a.group_handler_index) || testHandler(b.group_handler_index)) {
+            return false;
+        }
+
+        if (a.data.has_layer2 && b.data.layer == a.data.layer2)
+            return false;
+        if (b.data.has_layer2 && a.data.layer == b.data.layer2)
+            return false;
+
+        return m_collisionLookupTable[a.data.layer] & (1 << b.data.layer);
+    }
+
+    return true;
+}
+
 hkBool SensorGroupFilter::isCollisionEnabled(const hkpCollidable& a, const hkpCollidable& b) const {
-    return hkpGroupFilter::isCollisionEnabled(a, b);
+    return testCollisionForSensors(a.getCollisionFilterInfo(), b.getCollisionFilterInfo());
 }
 
-// TODO
 hkBool SensorGroupFilter::isCollisionEnabled(const hkpCollisionInput& input,
                                              const hkpCdBody& collectionBodyA,
                                              const hkpCdBody& collectionBodyB,
                                              const hkpShapeContainer& containerShapeA,
                                              const hkpShapeContainer& containerShapeB,
                                              hkpShapeKey keyA, hkpShapeKey keyB) const {
-    return hkpGroupFilter::isCollisionEnabled(input, collectionBodyA, collectionBodyB,
-                                              containerShapeA, containerShapeB, keyA, keyB);
+    auto infoA = containerShapeA.getCollisionFilterInfo(keyA);
+    if (infoA == 0xffffffff)
+        infoA = collectionBodyA.getRootCollidable()->getCollisionFilterInfo();
+
+    auto infoB = containerShapeB.getCollisionFilterInfo(keyB);
+    if (infoB == 0xffffffff)
+        infoB = collectionBodyB.getRootCollidable()->getCollisionFilterInfo();
+
+    return testCollisionForSensors(infoA, infoB);
 }
 
-// TODO
 hkBool SensorGroupFilter::isCollisionEnabled(const hkpCollisionInput& input, const hkpCdBody& a,
                                              const hkpCdBody& b,
                                              const hkpShapeContainer& bContainer,
                                              hkpShapeKey bKey) const {
-    return hkpGroupFilter::isCollisionEnabled(input, a, b, bContainer, bKey);
+    u32 infoB = bContainer.getCollisionFilterInfo(bKey);
+    if (infoB == 0xffffffff)
+        infoB = b.getRootCollidable()->getCollisionFilterInfo();
+
+    u32 infoA = static_cast<const hkpCollidable&>(a).getCollisionFilterInfo();
+
+    if (a.getParent() != nullptr) {
+        if (mInhibitCollisions)
+            return false;
+
+        hkpCollisionDispatcher* dispatcher = input.m_dispatcher;
+
+        auto* collidable = &a;
+        auto* parent = collidable->getParent();
+        while (parent) {
+            auto* shape = parent->m_shape;
+
+            if (dispatcher->hasAlternateType(shape->m_type, hkcdShapeType::COLLECTION)) {
+                auto* collection = static_cast<const hkpShapeCollection*>(shape);
+                infoA = collection->getCollisionFilterInfo(collidable->getShapeKey());
+                goto end;
+            }
+
+            if (dispatcher->hasAlternateType(shape->m_type, hkcdShapeType::BV_TREE)) {
+                auto* container = shape->getContainer();
+                infoA = container->getCollisionFilterInfo(collidable->getShapeKey());
+                goto end;
+            }
+
+            if (dispatcher->hasAlternateType(shape->m_type, hkcdShapeType::MULTI_SPHERE)) {
+                infoA = a.getRootCollidable()->getCollisionFilterInfo();
+                goto end;
+            }
+
+            if (dispatcher->hasAlternateType(shape->m_type, hkcdShapeType::CONVEX_LIST)) {
+                return true;
+            }
+
+            collidable = parent;
+            parent = collidable->getParent();
+            infoA = static_cast<const hkpCollidable*>(collidable)->getCollisionFilterInfo();
+        }
+    }
+
+end:
+    return testCollisionForSensors(infoA, infoB);
 }
 
-// TODO
 hkBool SensorGroupFilter::isCollisionEnabled(const hkpShapeRayCastInput& aInput,
                                              const hkpShapeContainer& bContainer,
                                              hkpShapeKey bKey) const {
-    return hkpGroupFilter::isCollisionEnabled(aInput, bContainer, bKey);
+    u32 bInfo = bContainer.getCollisionFilterInfo(bKey);
+    if (bInfo == 0)
+        return true;
+
+    if (bInfo == 0xffffffff)
+        bInfo = aInput.m_collidable->getCollisionFilterInfo();
+
+    return testCollisionForRayCasting(aInput.m_filterInfo, bInfo);
 }
 
-// TODO
 hkBool SensorGroupFilter::isCollisionEnabled(const hkpWorldRayCastInput& inputA,
                                              const hkpCollidable& collidableB) const {
-    return hkpGroupFilter::isCollisionEnabled(inputA, collidableB);
+    if (collidableB.getType() == hkpWorldObject::BROAD_PHASE_ENTITY) {
+        auto* entity = static_cast<const hkpEntity*>(collidableB.getOwner());
+        auto* body = entity ? reinterpret_cast<RigidBody*>(entity->getUserData()) : nullptr;
+        if (body && body->hasFlag(RigidBody::Flag::_200))
+            return false;
+    }
+
+    return testCollisionForRayCasting(inputA.m_filterInfo, collidableB.getCollisionFilterInfo());
+}
+
+hkBool SensorGroupFilter::testCollisionForRayCasting(u32 infoRayCast, u32 info) const {
+    if (mInhibitCollisions)
+        return false;
+
+    const SensorCollisionMask mask{info};
+    const SensorQueryCollisionMask query{infoRayCast};
+
+    const u32 mask_handler = mask.group_handler_index;
+    const u32 query_handler = query.group_handler_index;
+
+    if (mask.is_custom_receiver)
+        return false;
+
+    if (query_handler == mask_handler) {
+        if (mask_handler > 15)
+            return false;
+    } else if ((query_handler != 0 && query_handler >> 4 == 0) || testHandler(mask_handler)) {
+        return false;
+    }
+    return query.layer_mask & (1 << mask.data.layer);
 }
 
 void SensorGroupFilter::doInitSystemGroupHandlerLists_(sead::Heap* heap) {
diff --git a/src/KingSystem/Physics/System/physSensorGroupFilter.h b/src/KingSystem/Physics/System/physSensorGroupFilter.h
index c7a486dd..4f85702d 100644
--- a/src/KingSystem/Physics/System/physSensorGroupFilter.h
+++ b/src/KingSystem/Physics/System/physSensorGroupFilter.h
@@ -52,6 +52,7 @@ public:
 protected:
     /// Checks whether two sensors are colliding.
     hkBool testCollisionForSensors(u32 infoA, u32 infoB) const;
+    hkBool testCollisionForRayCasting(u32 infoRayCast, u32 info) const;
 
     void doInitSystemGroupHandlerLists_(sead::Heap* heap) override;
     int getFreeListIndex(const SystemGroupHandler* handler) override;
diff --git a/src/KingSystem/Physics/physDefines.h b/src/KingSystem/Physics/physDefines.h
index 18c8865b..55718aaa 100644
--- a/src/KingSystem/Physics/physDefines.h
+++ b/src/KingSystem/Physics/physDefines.h
@@ -317,6 +317,7 @@ union SensorCollisionMask {
 
     union CustomReceiverData {
         util::BitField<0, NumRegularSensorLayers, u32> layer;
+        util::BitField<0, NumRegularSensorLayers, u32> layer_mask;
     };
 
     constexpr SensorCollisionMask() : raw(0) {}
@@ -349,8 +350,6 @@ union SensorCollisionMask {
     u32 raw;
     Data data;
     CustomReceiverData custom_receiver_data;
-    /// Sensor layer mask.
-    util::BitField<0, NumRegularSensorLayers, u32> layer_mask;
     util::BitField<21, 10, u32> group_handler_index;
     util::BitField<31, 1, bool, u32> is_custom_receiver;
 };