From f852073fd4d751ea77e33c642448ba5c4eb30abf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?L=C3=A9o=20Lam?= Date: Tue, 1 Mar 2022 00:46:22 +0100 Subject: [PATCH] ksys/phys: Finish SensorGroupFilter --- data/uking_functions.csv | 22 +-- .../Physics/System/physEntityGroupFilter.cpp | 4 +- .../Physics/System/physSensorGroupFilter.cpp | 180 ++++++++++++++++-- .../Physics/System/physSensorGroupFilter.h | 1 + src/KingSystem/Physics/physDefines.h | 3 +- 5 files changed, 183 insertions(+), 27 deletions(-) diff --git a/data/uking_functions.csv b/data/uking_functions.csv index 63d0a0dc..830151ea 100644 --- a/data/uking_functions.csv +++ b/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 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 #include #include +#include #include #include -#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 +#include +#include +#include +#include +#include +#include +#include +#include +#include #include +#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(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(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(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(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(collidableB.getOwner()); + auto* body = entity ? reinterpret_cast(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; };