ksys/phys: Finish SensorGroupFilter

data/uking_functions.csv

@@ -84129,17 +84129,17 @@ Address,Quality,Size,Name
 0x0000007100fc7af8,O,000096,_ZThn32_N4ksys4phys17SensorGroupFilterD0Ev
 0x0000007100fc7b58,O,000096,_ZThn40_N4ksys4phys17SensorGroupFilterD0Ev
 0x0000007100fc7bb8,O,000004,_ZN4ksys4phys17SensorGroupFilter7doInit_EPN4sead4HeapE
-0x0000007100fc7bbc,U,000432,phys::SensorGroupFilter::x
-0x0000007100fc7d6c,U,000032,phys::SensorGroupFilter::isCollisionEnabled
-0x0000007100fc7d8c,U,000036,
-0x0000007100fc7db0,U,000176,phys::SensorGroupFilter::isCollisionEnabled2
-0x0000007100fc7e60,U,000176,
-0x0000007100fc7f10,U,000304,phys::SensorGroupFilter::isCollisionEnabled1
-0x0000007100fc8040,U,000028,
-0x0000007100fc805c,U,000208,phys::SensorGroupFilter::isCollisionEnabled3
-0x0000007100fc812c,U,000208,
-0x0000007100fc81fc,U,000188,phys::SensorGroupFilter::isCollisionEnabled4
-0x0000007100fc82b8,U,000188,
+0x0000007100fc7bbc,O,000432,_ZNK4ksys4phys17SensorGroupFilter23testCollisionForSensorsEjj
+0x0000007100fc7d6c,O,000032,_ZNK4ksys4phys17SensorGroupFilter18isCollisionEnabledERK13hkpCollidableS4_
+0x0000007100fc7d8c,O,000036,_ZThn16_NK4ksys4phys17SensorGroupFilter18isCollisionEnabledERK13hkpCollidableS4_
+0x0000007100fc7db0,O,000176,_ZNK4ksys4phys17SensorGroupFilter18isCollisionEnabledERK17hkpCollisionInputRK9hkpCdBodyS7_RK17hkpShapeContainerSA_jj
+0x0000007100fc7e60,O,000176,_ZThn24_NK4ksys4phys17SensorGroupFilter18isCollisionEnabledERK17hkpCollisionInputRK9hkpCdBodyS7_RK17hkpShapeContainerSA_jj
+0x0000007100fc7f10,O,000304,_ZNK4ksys4phys17SensorGroupFilter18isCollisionEnabledERK17hkpCollisionInputRK9hkpCdBodyS7_RK17hkpShapeContainerj
+0x0000007100fc8040,O,000028,_ZThn24_NK4ksys4phys17SensorGroupFilter18isCollisionEnabledERK17hkpCollisionInputRK9hkpCdBodyS7_RK17hkpShapeContainerj
+0x0000007100fc805c,O,000208,_ZNK4ksys4phys17SensorGroupFilter18isCollisionEnabledERK20hkpShapeRayCastInputRK17hkpShapeContainerj
+0x0000007100fc812c,O,000208,_ZThn32_NK4ksys4phys17SensorGroupFilter18isCollisionEnabledERK20hkpShapeRayCastInputRK17hkpShapeContainerj
+0x0000007100fc81fc,O,000188,_ZNK4ksys4phys17SensorGroupFilter18isCollisionEnabledERK20hkpWorldRayCastInputRK13hkpCollidable
+0x0000007100fc82b8,O,000188,_ZThn40_NK4ksys4phys17SensorGroupFilter18isCollisionEnabledERK20hkpWorldRayCastInputRK13hkpCollidable
 0x0000007100fc8374,O,000192,_ZN4ksys4phys17SensorGroupFilter30doInitSystemGroupHandlerLists_EPN4sead4HeapE
 0x0000007100fc8434,O,000016,_ZN4ksys4phys17SensorGroupFilter16getFreeListIndexEPKNS0_18SystemGroupHandlerE
 0x0000007100fc8444,O,000072,_ZN4ksys4phys27sensorCollisionMaskSetLayerENS0_12ContactLayerEj

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;
 }
 

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::isCollisionEnabled(const hkpCollidable& a, const hkpCollidable& b) const {
-    return hkpGroupFilter::isCollisionEnabled(a, b);
+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 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) {

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;

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;
 };