ksys/phys: Start implementing SensorGroupFilter

src/KingSystem/Physics/RigidBody/physRigidBody.cpp

@@ -794,7 +794,7 @@ void RigidBody::setSensorCustomReceiver(const ReceiverMask& mask,
 
     info.raw = sensorReceiverMaskSetLayer(ContactLayer::SensorCustomReceiver, info.raw);
     if (handler) {
-        info.custom_receiver_data.group_handler_index.SetUnsafe(handler->getIndex());
+        info.group_handler_index.SetUnsafe(handler->getIndex());
     }
     setCollisionFilterInfo(info.raw);
 }

src/KingSystem/Physics/RigidBody/physRigidBodyFromShape.cpp

@@ -351,7 +351,7 @@ RigidBodyT* RigidBodyFromShape::make(const Shape& shape, RigidBodyInstanceParam*
         const u32 idx = group_handler ? group_handler->getIndex() : 0;
         collision_filter_info = [&] {
             ReceiverMask info{collision_filter_info};
-            info.custom_receiver_data.group_handler_index.SetUnsafe(idx);
+            info.group_handler_index.SetUnsafe(idx);
             return info.raw;
         }();
     }

src/KingSystem/Physics/RigidBody/physRigidBodyParam.h

@@ -82,7 +82,7 @@ public:
     GroundHit groundhit = GroundHit::HitAll;
     u32 groundhit_mask = 0;
     u32 contact_mask = 0;
-    ReceiverMask receiver_mask;
+    ReceiverMask receiver_mask{ReceiverMask::CustomReceiverTag{}};
     bool ignore_normal_for_impulse = false;
     bool no_hit_ground = false;
     bool no_hit_water = false;

src/KingSystem/Physics/System/physSensorGroupFilter.cpp

@@ -1,8 +1,78 @@
 #include "KingSystem/Physics/System/physSensorGroupFilter.h"
 #include <basis/seadRawPrint.h>
+#include "KingSystem/Utils/HeapUtil.h"
 
 namespace ksys::phys {
 
+constexpr int NumSensorHandlersInList0 = 0x10;
+constexpr int NumSensorHandlers = 0x400;
+
+SensorGroupFilter* SensorGroupFilter::make(ContactLayer::ValueType last, sead::Heap* heap) {
+    auto* filter = util::alloc<SensorGroupFilter>(heap, FirstSensor, last);
+    filter->initFilter(heap);
+    return filter;
+}
+
+SensorGroupFilter::SensorGroupFilter(ContactLayer::ValueType first, ContactLayer::ValueType last)
+    : GroupFilter(ContactLayerType::Sensor, first, last) {}
+
+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);
+}
+
+// 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);
+}
+
+// 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);
+}
+
+// TODO
+hkBool SensorGroupFilter::isCollisionEnabled(const hkpShapeRayCastInput& aInput,
+                                             const hkpShapeContainer& bContainer,
+                                             hkpShapeKey bKey) const {
+    return hkpGroupFilter::isCollisionEnabled(aInput, bContainer, bKey);
+}
+
+// TODO
+hkBool SensorGroupFilter::isCollisionEnabled(const hkpWorldRayCastInput& inputA,
+                                             const hkpCollidable& collidableB) const {
+    return hkpGroupFilter::isCollisionEnabled(inputA, collidableB);
+}
+
+void SensorGroupFilter::doInitSystemGroupHandlerLists_(sead::Heap* heap) {
+    for (auto& list : mFreeLists)
+        list.initOffset(SystemGroupHandler::getFreeListNodeOffset());
+
+    mUsedList.initOffset(SystemGroupHandler::getUsedListNodeOffset());
+
+    for (int i = 1; i < NumSensorHandlers; ++i) {
+        auto& list = mFreeLists[i < NumSensorHandlersInList0];
+        list.pushBack(new (heap) SensorSystemGroupHandler(i));
+    }
+}
+
+int SensorGroupFilter::getFreeListIndex(const SystemGroupHandler* handler) {
+    return handler->getIndex() < NumSensorHandlersInList0;
+}
+
 u32 sensorReceiverMaskSetLayer(ContactLayer layer, u32 mask) {
     SEAD_ASSERT(getContactLayerType(layer) == ContactLayerType::Sensor);
 

src/KingSystem/Physics/System/physSensorGroupFilter.h

@@ -5,11 +5,57 @@
 
 namespace ksys::phys {
 
-// FIXME
+class SensorSystemGroupHandler : public SystemGroupHandler {
+public:
+    explicit SensorSystemGroupHandler(int i) : SystemGroupHandler(i, ContactLayerType::Sensor) {}
+
+    u32 makeCollisionFilterInfo(u32 info, ContactLayer layer, GroundHit ground_hit) override;
+    u32 makeQueryCollisionMask(u32 layer_mask, GroundHit ground_hit, bool unk) override;
+    bool m8() override;
+};
+
 class SensorGroupFilter : public GroupFilter {
     SEAD_RTTI_OVERRIDE(SensorGroupFilter, GroupFilter)
 public:
     static SensorGroupFilter* make(ContactLayer::ValueType last, sead::Heap* heap);
+
+    SensorGroupFilter(ContactLayer::ValueType first, ContactLayer::ValueType last);
+    ~SensorGroupFilter() override;
+
+    hkBool isCollisionEnabled(const hkpCollidable& a, const hkpCollidable& b) const override;
+    hkBool isCollisionEnabled(const hkpCollisionInput& input, const hkpCdBody& a,
+                              const hkpCdBody& b, const hkpShapeContainer& bContainer,
+                              hkpShapeKey bKey) const override;
+    hkBool isCollisionEnabled(const hkpCollisionInput& input, const hkpCdBody& collectionBodyA,
+                              const hkpCdBody& collectionBodyB,
+                              const hkpShapeContainer& containerShapeA,
+                              const hkpShapeContainer& containerShapeB, hkpShapeKey keyA,
+                              hkpShapeKey keyB) const override;
+    hkBool isCollisionEnabled(const hkpShapeRayCastInput& aInput,
+                              const hkpShapeContainer& bContainer, hkpShapeKey bKey) const override;
+    hkBool isCollisionEnabled(const hkpWorldRayCastInput& inputA,
+                              const hkpCollidable& collidableB) const override;
+
+    u32 makeCollisionFilterInfo(ContactLayer layer, GroundHit ground_hit) override;
+    ContactLayer getCollisionFilterInfoLayer(u32 info) override;
+    u32 makeQueryCollisionMask(u32 layer_mask, GroundHit ground_hit, bool unk) override;
+    GroundHit getQueryCollisionMaskGroundHit(u32 info) override;
+    void getCollisionFilterInfoLayerAndGroundHit(u32 info, ContactLayer* layer,
+                                                 GroundHit* ground_hit) override;
+    const char* getCollisionFilterInfoLayerText(u32 info) override;
+    u32 getCollisionFilterInfoGroupHandlerIdx(u32 info) override;
+
+    virtual u32 makeCollisionFilterInfo(ContactLayer layer, GroundHit ground_hit, u32 unk5,
+                                        u32 unk10);
+    virtual void setSensorLayerCollisionEnabledMask(ContactLayer layer, u32 mask);
+
+protected:
+    /// Checks whether two sensors are colliding.
+    hkBool testCollisionForSensors(u32 infoA, u32 infoB) const;
+
+    void doInitSystemGroupHandlerLists_(sead::Heap* heap) override;
+    int getFreeListIndex(const SystemGroupHandler* handler) override;
+    void doInit_(sead::Heap* heap) override;
 };
 
 /// Set the contact layer in a sensor receiver mask.
@@ -25,4 +71,77 @@ u32 sensorReceiverMaskSetLayer(ContactLayer layer, u32 mask);
 // TODO: rename once we figure out what this layer is used for
 u32 sensorReceiverMaskSetLayer2(bool set, ContactLayer layer, u32 mask);
 
+inline u32 SensorSystemGroupHandler::makeCollisionFilterInfo(u32 info, ContactLayer layer,
+                                                             GroundHit ground_hit) {
+    const ReceiverMask current_info{info};
+    ReceiverMask result;
+
+    if (layer == ContactLayer::SensorCustomReceiver) {
+        result.is_custom_receiver = true;
+        result.group_handler_index.Init(getIndex());
+        result.custom_receiver_data.layer.Init(current_info.custom_receiver_data.layer);
+    } else {
+        result.is_custom_receiver = false;
+        result.group_handler_index.Init(getIndex());
+        result.data.layer.Init(layer - FirstSensor);
+    }
+    return result.raw;
+}
+
+inline u32 SensorSystemGroupHandler::makeQueryCollisionMask(u32 layer_mask, GroundHit ground_hit,
+                                                            bool unk) {
+    SensorQueryCollisionMask mask;
+    mask.layer_mask = layer_mask;
+    mask.group_handler_index.Init(getIndex());
+    return mask.raw;
+}
+
+inline bool SensorSystemGroupHandler::m8() {
+    return getIndex() > 0 && getIndex() < 0x400;
+}
+
+inline u32 SensorGroupFilter::makeCollisionFilterInfo(ContactLayer layer, GroundHit ground_hit) {
+    return ReceiverMask::make(layer).raw;
+}
+
+inline ContactLayer SensorGroupFilter::getCollisionFilterInfoLayer(u32 info) {
+    return ReceiverMask(info).getLayer();
+}
+
+inline u32 SensorGroupFilter::makeQueryCollisionMask(u32 layer_mask, GroundHit ground_hit,
+                                                     bool unk) {
+    SensorQueryCollisionMask mask;
+    mask.layer_mask = layer_mask;
+    return mask.raw;
+}
+
+inline GroundHit SensorGroupFilter::getQueryCollisionMaskGroundHit(u32 info) {
+    return GroundHit::HitAll;
+}
+
+inline void SensorGroupFilter::getCollisionFilterInfoLayerAndGroundHit(u32 info,
+                                                                       ContactLayer* layer,
+                                                                       GroundHit* ground_hit) {
+    ReceiverMask mask{info};
+    *layer = mask.getLayer();
+    *ground_hit = {};
+}
+
+inline const char* SensorGroupFilter::getCollisionFilterInfoLayerText(u32 info) {
+    return contactLayerToText(getCollisionFilterInfoLayer(info));
+}
+
+inline u32 SensorGroupFilter::getCollisionFilterInfoGroupHandlerIdx(u32 info) {
+    return ReceiverMask(info).group_handler_index;
+}
+
+inline u32 SensorGroupFilter::makeCollisionFilterInfo(ContactLayer layer, GroundHit ground_hit,
+                                                      u32 unk5, u32 unk10) {
+    return 0;
+}
+
+inline void SensorGroupFilter::setSensorLayerCollisionEnabledMask(ContactLayer layer, u32 mask) {
+    m_collisionLookupTable[layer - mLayerFirst] = mask;
+}
+
 }  // namespace ksys::phys

src/KingSystem/Physics/physDefines.h

@@ -201,8 +201,14 @@ enum class WaterCollisionMode {
     IgnoreWater = 1,
 };
 
+// TODO: rename to SensorCollisionFilterInfo.
 union ReceiverMask {
+    struct CustomReceiverTag {};
+
     union Data {
+        ContactLayer getLayer() const { return int(layer) + FirstSensor; }
+
+        /// ContactLayer minus FirstSensor.
         util::BitField<0, 5, u32> layer;
         // TODO: rename once we figure out what this layer is used for
         util::BitField<5, 1, bool, u32> has_layer2;
@@ -211,10 +217,10 @@ union ReceiverMask {
 
     union CustomReceiverData {
         util::BitField<0, 21, u32> layer;
-        util::BitField<21, 10, u32> group_handler_index;
     };
 
-    constexpr ReceiverMask() : raw(0) { is_custom_receiver = true; }
+    constexpr ReceiverMask() : raw(0) {}
+    constexpr explicit ReceiverMask(CustomReceiverTag) : raw(0) { is_custom_receiver = true; }
     constexpr explicit ReceiverMask(u32 raw_) : raw(raw_) {}
     constexpr ReceiverMask(const ReceiverMask&) = default;
     constexpr ReceiverMask& operator=(const ReceiverMask& m) {
@@ -234,11 +240,17 @@ union ReceiverMask {
         return mask;
     }
 
+    ContactLayer getLayer() const {
+        return is_custom_receiver ? ContactLayer::SensorCustomReceiver : data.getLayer();
+    }
+
     u32 raw;
     Data data;
     CustomReceiverData custom_receiver_data;
-    // FIXME: is this a sensor layer mask?
+    /// Sensor layer mask.
+    // TODO: add a constant for 21.
     util::BitField<0, 21, u32> layer_mask;
+    util::BitField<21, 10, u32> group_handler_index;
     util::BitField<31, 1, bool, u32> is_custom_receiver;
 };
 
@@ -319,7 +331,8 @@ union EntityCollisionFilterInfo {
 };
 static_assert(sizeof(EntityCollisionFilterInfo) == sizeof(u32));
 
-/// Collision mask that is used for raycast-based queries.
+/// Collision mask that is used for raycast-based queries against entities.
+// TODO: rename to make it clear this is used for entities, not sensors
 union RayCastCollisionMask {
     constexpr explicit RayCastCollisionMask(u32 raw_ = 0) : raw(raw_) {}
     constexpr RayCastCollisionMask(const RayCastCollisionMask&) = default;
@@ -337,6 +350,23 @@ union RayCastCollisionMask {
     u32 raw;
 };
 
+/// Collision mask that is used for raycast-based queries against sensors.
+union SensorQueryCollisionMask {
+    constexpr explicit SensorQueryCollisionMask(u32 raw_ = 0) : raw(raw_) {}
+    constexpr SensorQueryCollisionMask(const SensorQueryCollisionMask&) = default;
+    constexpr SensorQueryCollisionMask& operator=(const SensorQueryCollisionMask& m) {
+        raw = m.raw;
+        return *this;
+    }
+    constexpr bool operator==(SensorQueryCollisionMask rhs) const { return raw == rhs.raw; }
+    constexpr bool operator!=(SensorQueryCollisionMask rhs) const { return raw != rhs.raw; }
+
+    // TODO: add a constant for 21.
+    util::BitField<0, 21, u32> layer_mask;
+    util::BitField<22, 10, u32> group_handler_index;
+    u32 raw;
+};
+
 ContactLayerType getContactLayerType(ContactLayer layer);
 u32 makeContactLayerMask(ContactLayer layer);
 u32 getContactLayerBase(ContactLayerType type);