ksys/phys: Add RigidBody collision filter info functions

Including a bunch of BitFields additions so we can get the desired
codegen without sacrificing readability or flexibility.
(The alternative would be building masks and masking manually.)

getCollisionFilterInfo was changed to return a u32 instead of
EntityCollisionFilterInfo because a collision filter info mask
can be either an EntityCollisionFilterInfo or a ReceiverMask.

(Also at some point we'll probably want to rename
EntityCollisionFilterInfo because that is also used for sensor
rigid bodies and not just for entities.)

data/uking_functions.csv

@@ -83004,23 +83004,23 @@ Address,Quality,Size,Name
 0x0000007100f8f0c8,O,000176,_ZN4ksys4phys9RigidBody21enableGroundCollisionEb
 0x0000007100f8f178,O,000064,_ZNK4ksys4phys9RigidBody15getContactLayerEv
 0x0000007100f8f1b8,O,000012,_ZNK4ksys4phys9RigidBody22getCollisionFilterInfoEv
-0x0000007100f8f1c4,U,000572,RigidBody::setCollisionFilterMask
+0x0000007100f8f1c4,O,000572,_ZN4ksys4phys9RigidBody22setCollisionFilterInfoEj
 0x0000007100f8f400,O,000172,_ZN4ksys4phys9RigidBody20enableWaterCollisionEb
 0x0000007100f8f4ac,O,000048,_ZNK4ksys4phys9RigidBody23isWaterCollisionEnabledEv
-0x0000007100f8f4dc,U,000064,phys::RigidBody::x_23
+0x0000007100f8f4dc,O,000064,_ZN4ksys4phys9RigidBody23setSensorReceiverLayer2ENS0_12ContactLayerE
-0x0000007100f8f51c,U,000104,phys::RigidBody::x_24
+0x0000007100f8f51c,O,000104,_ZN4ksys4phys9RigidBody25clearSensorReceiverLayer2Ev
-0x0000007100f8f584,U,000144,phys::RigidBody::x_25
+0x0000007100f8f584,O,000144,_ZN4ksys4phys9RigidBody25setContactLayerAndHandlerENS0_12ContactLayerEPNS0_18SystemGroupHandlerE
-0x0000007100f8f614,U,000144,phys::RigidBody::x_26
+0x0000007100f8f614,O,000144,_ZN4ksys4phys9RigidBody15setContactLayerENS0_12ContactLayerE
-0x0000007100f8f6a4,U,000256,RigidBody::setSystemGroupHandlerMaybe
+0x0000007100f8f6a4,O,000256,_ZN4ksys4phys9RigidBody21setSystemGroupHandlerEPNS0_18SystemGroupHandlerE
-0x0000007100f8f7a4,U,000208,phys::RigidBody::x_27
+0x0000007100f8f7a4,O,000208,_ZN4ksys4phys9RigidBody27setContactLayerAndGroundHitENS0_12ContactLayerENS0_9GroundHitE
-0x0000007100f8f874,U,000088,phys::RigidBody::x_28
+0x0000007100f8f874,O,000088,_ZN4ksys4phys9RigidBody16setGroundHitTypeENS0_9GroundHitE
-0x0000007100f8f8cc,U,000216,phys::RigidBody::x_29
+0x0000007100f8f8cc,O,000216,_ZN4ksys4phys9RigidBody37setContactLayerAndGroundHitAndHandlerENS0_12ContactLayerENS0_9GroundHitEPNS0_18SystemGroupHandlerE
-0x0000007100f8f9a4,U,000068,phys::RigidBody::x_30
+0x0000007100f8f9a4,O,000068,_ZN4ksys4phys9RigidBody23setSensorCustomReceiverERKNS0_12ReceiverMaskE
-0x0000007100f8f9e8,U,000092,RigidBody::setSensorCustomReceiverMask
+0x0000007100f8f9e8,O,000092,_ZN4ksys4phys9RigidBody23setSensorCustomReceiverERKNS0_12ReceiverMaskEPKNS0_18SystemGroupHandlerE
-0x0000007100f8fa44,U,000092,RigidBody::setGroundHitMask
+0x0000007100f8fa44,O,000092,_ZN4ksys4phys9RigidBody16setGroundHitMaskENS0_12ContactLayerEj
-0x0000007100f8faa0,U,000056,phys::RigidBody::x_31
+0x0000007100f8faa0,O,000056,_ZN4ksys4phys9RigidBody28addGroundTypeToGroundHitMaskENS0_9GroundHitE
-0x0000007100f8fad8,U,000044,phys::RigidBody::x_32
+0x0000007100f8fad8,O,000044,_ZNK4ksys4phys9RigidBody16getGroundHitTypeEv
-0x0000007100f8fb04,U,000004,phys::RigidBody::x_33_nop
+0x0000007100f8fb04,O,000004,_ZN4ksys4phys9RigidBody8setColorERKN4sead7Color4fEPKvb
 0x0000007100f8fb08,U,000360,phys::RigidBody::x_34_setTransform
 0x0000007100f8fc70,O,000196,_ZN4ksys4phys9RigidBody11setPositionERKN4sead7Vector3IfEEb
 0x0000007100f8fd34,U,000012,phys::RigidBody::x_36
@@ -84139,8 +84139,8 @@ Address,Quality,Size,Name
 0x0000007100fc82b8,U,000188,
 0x0000007100fc8374,U,000192,phys::SensorGroupFilter::m11
 0x0000007100fc8434,U,000016,phys::SensorGroupFilter::m12
-0x0000007100fc8444,U,000072,sensorMaskStuff
+0x0000007100fc8444,O,000072,_ZN4ksys4phys26sensorReceiverMaskSetLayerENS0_12ContactLayerEj
-0x0000007100fc848c,U,000060,sensorMaskStuff_0
+0x0000007100fc848c,O,000060,_ZN4ksys4phys27sensorReceiverMaskSetLayer2EbNS0_12ContactLayerEj
 0x0000007100fc84c8,U,000056,sensorMaskStuff_1
 0x0000007100fc8500,U,000020,sensorMaskStuff_2
 0x0000007100fc8514,U,000020,sensorMaskStuff_3

src/KingSystem/Physics/RigidBody/physRigidBody.cpp

@@ -2,6 +2,8 @@
 #include <Havok/Common/Base/Math/SweptTransform/hkSweptTransformfUtil.h>
 #include <Havok/Common/Base/Types/Geometry/Aabb/hkAabb.h>
 #include <Havok/Physics/Constraint/Data/hkpConstraintData.h>
+#include <Havok/Physics2012/Collide/Shape/Compound/Collection/List/hkpListShape.h>
+#include <Havok/Physics2012/Collide/Shape/Compound/Tree/Mopp/hkpMoppBvTreeShape.h>
 #include <Havok/Physics2012/Dynamics/Collide/hkpResponseModifier.h>
 #include <Havok/Physics2012/Dynamics/Constraint/hkpConstraintInstance.h>
 #include <Havok/Physics2012/Dynamics/Entity/hkpRigidBody.h>
@@ -14,6 +16,9 @@
 #include "KingSystem/Physics/RigidBody/physRigidBodyMotionSensor.h"
 #include "KingSystem/Physics/RigidBody/physRigidBodyParam.h"
 #include "KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h"
+#include "KingSystem/Physics/System/physEntityGroupFilter.h"
+#include "KingSystem/Physics/System/physGroupFilter.h"
+#include "KingSystem/Physics/System/physSensorGroupFilter.h"
 #include "KingSystem/Physics/System/physSystem.h"
 #include "KingSystem/Physics/System/physUserTag.h"
 #include "KingSystem/Physics/physConversions.h"
@@ -587,7 +592,7 @@ void RigidBody::enableGroundCollision(bool enabled) {
     if (int(getContactLayer()) == ContactLayer::EntityRagdoll)
         return;
 
-    const auto current_info = getCollisionFilterInfo();
+    const auto current_info = getEntityCollisionFilterInfo();
     auto info = current_info;
     info.unk5 = false;
     info.no_ground_collision.SetBit(!enabled);
@@ -599,7 +604,7 @@ bool RigidBody::isGroundCollisionEnabled() const {
     if (!isEntity())
         return false;
 
-    const auto info = getCollisionFilterInfo();
+    const auto info = getEntityCollisionFilterInfo();
 
     bool enabled = false;
     enabled |= info.unk5;
@@ -615,7 +620,7 @@ void RigidBody::enableWaterCollision(bool enabled) {
     if (int(getContactLayer()) == ContactLayer::EntityRagdoll)
         return;
 
-    const auto current_info = getCollisionFilterInfo();
+    const auto current_info = getEntityCollisionFilterInfo();
     auto info = current_info;
     info.no_water_collision = !enabled;
     if (current_info != info)
@@ -626,7 +631,7 @@ bool RigidBody::isWaterCollisionEnabled() const {
     if (!isEntity())
         return false;
 
-    const auto info = getCollisionFilterInfo();
+    const auto info = getEntityCollisionFilterInfo();
 
     bool enabled = false;
     // unk30 enables all collisions?
@@ -636,15 +641,194 @@ bool RigidBody::isWaterCollisionEnabled() const {
 }
 
 ContactLayer RigidBody::getContactLayer() const {
-    return getContactLayer(getCollisionFilterInfo());
+    return getContactLayer(getEntityCollisionFilterInfo());
 }
 
 ContactLayer RigidBody::getContactLayer(EntityCollisionFilterInfo info) const {
     return isSensor() ? info.getLayerSensor() : info.getLayer();
 }
 
-EntityCollisionFilterInfo RigidBody::getCollisionFilterInfo() const {
+void RigidBody::setContactLayer(ContactLayer layer) {
-    return EntityCollisionFilterInfo(mHkBody->getCollisionFilterInfo());
+    if (getLayerType() != getContactLayerType(layer))
+        return;
+
+    const auto current_info = getCollisionFilterInfo();
+    auto info = current_info;
+    if (isSensor())
+        info = sensorReceiverMaskSetLayer(layer, info);
+    else
+        info = makeEntityCollisionMask(layer, info);
+
+    if (current_info != info)
+        setCollisionFilterInfo(info);
+}
+
+u32 RigidBody::getCollisionFilterInfo() const {
+    return mHkBody->getCollisionFilterInfo();
+}
+
+static void resetCollisionFilterInfoForListShapes(const hkpShape* shape) {
+    while (true) {
+        switch (shape->getType()) {
+        case hkcdShapeType::LIST: {
+            auto* list = static_cast<const hkpListShape*>(shape);
+            for (auto k = list->getFirstKey(); k != HK_INVALID_SHAPE_KEY; k = list->getNextKey(k)) {
+                // XXX: eww, can we get rid of this const_cast?
+                auto* mut_list = const_cast<hkpListShape*>(list);
+                mut_list->setCollisionFilterInfo(k, 0xffffffff);
+            }
+            return;
+        }
+
+        case hkcdShapeType::MOPP:
+            shape = static_cast<const hkpMoppBvTreeShape*>(shape)->getChild();
+            continue;
+
+        default:
+            return;
+        }
+    }
+}
+
+void RigidBody::setCollisionFilterInfo(u32 info) {
+    const auto current_layer = getContactLayer();
+
+    const auto lock = makeScopedLock(isFlag8Set());
+
+    if (getCollisionFilterInfo() != info) {
+        if (isFlag8Set()) {
+            if (int(current_layer) != getContactLayer(EntityCollisionFilterInfo(info)))
+                System::instance()->registerRigidBodyForContactSystem(this);
+        }
+
+        mHkBody->setCollisionFilterInfo(info);
+        if (auto* shape = mHkBody->getCollidableRw()->getShape())
+            resetCollisionFilterInfoForListShapes(shape);
+
+        if (isFlag8Set())
+            setMotionFlag(MotionFlag::_8000);
+    }
+}
+
+void RigidBody::setSensorReceiverLayer2(ContactLayer layer) {
+    static_cast<void>(isSensor());
+    static_cast<void>(isSensor());
+    const auto info = sensorReceiverMaskSetLayer2(true, layer, getCollisionFilterInfo());
+    setCollisionFilterInfo(info);
+}
+
+void RigidBody::clearSensorReceiverLayer2() {
+    if (!isSensor())
+        return;
+
+    if (getContactLayer() == ContactLayer::SensorCustomReceiver)
+        return;
+
+    // The layer we pass here is actually irrelevant because we're clearing the layer value anyway.
+    const auto info =
+        sensorReceiverMaskSetLayer2(false, ContactLayer::SensorNoHit, getCollisionFilterInfo());
+
+    setCollisionFilterInfo(info);
+}
+
+void RigidBody::setContactLayerAndHandler(ContactLayer layer, SystemGroupHandler* handler) {
+    setContactLayer(layer);
+    setSystemGroupHandler(handler);
+}
+
+void RigidBody::setContactLayerAndGroundHit(ContactLayer layer, GroundHit ground_hit) {
+    setContactLayer(layer);
+    if (isEntity())
+        setGroundHitType(ground_hit);
+}
+
+void RigidBody::setContactLayerAndGroundHitAndHandler(ContactLayer layer, GroundHit ground_hit,
+                                                      SystemGroupHandler* handler) {
+    setContactLayer(layer);
+    if (isEntity())
+        setGroundHitType(ground_hit);
+    setSystemGroupHandler(handler);
+}
+
+void RigidBody::setSystemGroupHandler(SystemGroupHandler* handler) {
+    const auto layer = getContactLayer();
+    const auto ground_hit = getGroundHitType();
+    const auto info = getCollisionFilterInfo();
+
+    if (handler) {
+        if (handler->getLayerType() == getLayerType()) {
+            setCollisionFilterInfo(handler->makeCollisionFilterInfo(info, layer, ground_hit));
+        } else {
+            SEAD_WARN("handler layer type doesn't match rigid body type; ignoring handler");
+        }
+    } else if (isEntity()) {
+        setCollisionFilterInfo(EntityCollisionFilterInfo::make(layer, ground_hit).raw);
+    } else {
+        setCollisionFilterInfo(ReceiverMask::make(layer).raw);
+    }
+}
+
+void RigidBody::setSensorCustomReceiver(const ReceiverMask& mask) {
+    ReceiverMask info = mask;
+
+    if (!isSensor())
+        return;
+
+    info.raw = sensorReceiverMaskSetLayer(ContactLayer::SensorCustomReceiver, info.raw);
+    setCollisionFilterInfo(info.raw);
+}
+
+void RigidBody::setSensorCustomReceiver(const ReceiverMask& mask,
+                                        const SystemGroupHandler* handler) {
+    ReceiverMask info = mask;
+
+    if (!isSensor())
+        return;
+
+    info.raw = sensorReceiverMaskSetLayer(ContactLayer::SensorCustomReceiver, info.raw);
+    if (handler) {
+        info.custom_receiver_data.group_handler_index.SetUnsafe(handler->getIndex());
+    }
+    setCollisionFilterInfo(info.raw);
+}
+
+void RigidBody::setGroundHitMask(ContactLayer layer, u32 mask) {
+    if (getContactLayerType(layer) == ContactLayerType::Entity)
+        setCollisionFilterInfo(makeEntityGroundHitMask(layer, mask));
+}
+
+void RigidBody::addGroundTypeToGroundHitMask(GroundHit ground_hit) {
+    auto info = getEntityCollisionFilterInfo();
+
+    if (!isEntity() || !info.is_ground_hit_mask)
+        return;
+
+    info.ground_hit.addGroundHit(ground_hit);
+    setCollisionFilterInfo(info.raw);
+}
+
+GroundHit RigidBody::getGroundHitType() const {
+    const auto info = getEntityCollisionFilterInfo();
+    if (!isEntity())
+        return {};
+    return info.getGroundHit();
+}
+
+void RigidBody::setGroundHitType(GroundHit ground_hit) {
+    if (!isEntity())
+        return;
+
+    const auto current_info = getCollisionFilterInfo();
+    auto info = current_info;
+    info = setEntityCollisionMaskGroundHit(ground_hit, info);
+
+    if (current_info != info)
+        setCollisionFilterInfo(info);
+}
+
+void RigidBody::setColor(const sead::Color4f& color, const void* a, bool b) {
+    // Stubbed debug function? This would probably have been used to see Area actors
+    // (which are normally invisible).
 }
 
 void RigidBody::setPosition(const sead::Vector3f& position, bool propagate_to_linked_motions) {

src/KingSystem/Physics/RigidBody/physRigidBody.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <container/seadPtrArray.h>
+#include <gfx/seadColor.h>
 #include <heap/seadDisposer.h>
 #include <math/seadBoundBox.h>
 #include <math/seadMathCalcCommon.h>
@@ -27,6 +28,7 @@ struct RigidBodyInstanceParam;
 class RigidBodyMotionEntity;
 class RigidBodyMotionSensor;
 class RigidContactPoints;
+class SystemGroupHandler;
 class UserTag;
 
 class RigidBase {
@@ -193,16 +195,62 @@ public:
     void enableWaterCollision(bool enabled);
     bool isWaterCollisionEnabled() const;
 
+    // region Collision filter info, receiver, group handler
+
     ContactLayer getContactLayer() const;
     ContactLayer getContactLayer(EntityCollisionFilterInfo info) const;
-    EntityCollisionFilterInfo getCollisionFilterInfo() const;
+    /// Set a new contact layer. Its type must match the layer type of this rigid body.
-    // 0x0000007100f8f1c4
+    /// (Otherwise, this function does nothing.)
+    void setContactLayer(ContactLayer layer);
+
+    u32 getCollisionFilterInfo() const;
     void setCollisionFilterInfo(u32 info);
 
-    void sub_7100F8F51C();
+    auto getEntityCollisionFilterInfo() const {
-    void sub_7100F8F8CC(ContactLayer, GroundHit, void*);
+        return EntityCollisionFilterInfo(getCollisionFilterInfo());
-    void sub_7100F8F9E8(ReceiverMask*, void*);
+    }
-    void sub_7100F8FA44(ContactLayer, u32);
+
+    /// Only works for sensor rigid bodies that do not use a custom receiver.
+    // TODO: rename once we figure out what this layer is used for
+    void setSensorReceiverLayer2(ContactLayer layer);
+    /// Only works for sensor rigid bodies that do not use a custom receiver.
+    void clearSensorReceiverLayer2();
+
+    void setContactLayerAndHandler(ContactLayer layer, SystemGroupHandler* handler);
+    void setContactLayerAndGroundHit(ContactLayer layer, GroundHit ground_hit);
+    void setContactLayerAndGroundHitAndHandler(ContactLayer layer, GroundHit ground_hit,
+                                               SystemGroupHandler* handler);
+
+    void setSystemGroupHandler(SystemGroupHandler* handler);
+
+    void setSensorCustomReceiver(const ReceiverMask& mask);
+    void setSensorCustomReceiver(const ReceiverMask& mask, const SystemGroupHandler* handler);
+
+    // endregion
+
+    // region Ground hit
+
+    /// Replace the current collision filter info with a ground hit mask.
+    /// @param layer Contact layer (must be an entity layer; this function does nothing otherwise)
+    /// @param mask  The new ground hit mask
+    void setGroundHitMask(ContactLayer layer, u32 mask);
+
+    /// Add a ground hit type to an existing ground hit mask.
+    /// If this rigid body does not already have a ground hit mask or isn't an entity,
+    /// then this function does nothing.
+    void addGroundTypeToGroundHitMask(GroundHit ground_hit);
+
+    /// Get the ground hit type for this rigid body.
+    /// Only valid for entity rigid bodies that do *not* have a ground hit mask
+    /// but a normal entity mask. Returns 0 if this is a sensor, HitAll if in ground hit mask mode.
+    GroundHit getGroundHitType() const;
+
+    /// Set a ground hit type. This can only be done for entity rigid bodies.
+    void setGroundHitType(GroundHit ground_hit);
+
+    // endregion
+
+    void setColor(const sead::Color4f& color, const void* a, bool b);
 
     void setPosition(const sead::Vector3f& position, bool propagate_to_linked_motions);
     void getPosition(sead::Vector3f* position) const;

src/KingSystem/Physics/System/physDefines.h

@@ -181,18 +181,44 @@ enum class MotionType {
 };
 
 union ReceiverMask {
-    constexpr ReceiverMask() : raw(0) { unk_flag = true; }
+    union Data {
+        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;
+        util::BitField<6, 5, u32> layer2;
+    };
+
+    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 explicit ReceiverMask(u32 raw_) : raw(raw_) {}
+    constexpr ReceiverMask(const ReceiverMask&) = default;
     constexpr ReceiverMask& operator=(const ReceiverMask& m) {
         raw = m.raw;
         return *this;
     }
 
+    static ReceiverMask make(ContactLayer layer) {
+        ReceiverMask mask;
+        if (layer == ContactLayer::SensorCustomReceiver) {
+            mask.is_custom_receiver = true;
+            mask.custom_receiver_data.layer.Init(ContactLayer::SensorCustomReceiver - FirstSensor);
+        } else {
+            mask.is_custom_receiver = false;
+            mask.data.layer.Init(layer - FirstSensor);
+        }
+        return mask;
+    }
+
     u32 raw;
+    Data data;
+    CustomReceiverData custom_receiver_data;
     // FIXME: is this a sensor layer mask?
     util::BitField<0, 21, u32> layer_mask;
-    // FIXME: is sensor layer? is layer mask?
+    util::BitField<31, 1, bool, u32> is_custom_receiver;
-    util::BitField<31, 1, u32> unk_flag;
 };
 
 union EntityCollisionFilterInfo {
@@ -201,6 +227,7 @@ union EntityCollisionFilterInfo {
         ContactLayer getLayerSensor() const { return int(layer + FirstSensor); }
         GroundHit getGroundHit() const { return int(ground_hit); }
 
+        u32 raw;
         util::BitField<0, 5, u32> layer;
         util::BitField<24, 1, u32> unk24;
         util::BitField<25, 1, u32> unk25;
@@ -210,17 +237,34 @@ union EntityCollisionFilterInfo {
     union GroundHitMask {
         ContactLayer getLayer() const { return int(layer); }
 
+        void addGroundHit(GroundHit hit) {
+            raw |= (1 << hit) << decltype(ground_hit_types)::StartBit();
+        }
+
+        u32 raw;
         util::BitField<0, 1, u32> unk;
         util::BitField<8, 16, u32> ground_hit_types;
         util::BitField<24, 1, u32> unk24;
         util::BitField<25, 5, u32> layer;
     };
 
-    explicit EntityCollisionFilterInfo(u32 raw_ = 0) : raw(raw_) {}
+    constexpr explicit EntityCollisionFilterInfo(u32 raw_ = 0) : raw(raw_) {}
+    constexpr EntityCollisionFilterInfo(const EntityCollisionFilterInfo&) = default;
+    constexpr EntityCollisionFilterInfo& operator=(const EntityCollisionFilterInfo& m) {
+        raw = m.raw;
+        return *this;
+    }
 
     bool operator==(EntityCollisionFilterInfo rhs) const { return raw == rhs.raw; }
     bool operator!=(EntityCollisionFilterInfo rhs) const { return raw != rhs.raw; }
 
+    static EntityCollisionFilterInfo make(ContactLayer layer, GroundHit ground_hit) {
+        EntityCollisionFilterInfo mask;
+        mask.data.layer.Init(layer);
+        mask.data.ground_hit.Init(ground_hit);
+        return mask;
+    }
+
     ContactLayer getLayer() const {
         return is_ground_hit_mask ? ground_hit.getLayer() : data.getLayer();
     }
@@ -230,6 +274,10 @@ union EntityCollisionFilterInfo {
                                     data.getLayerSensor();
     }
 
+    GroundHit getGroundHit() const {
+        return is_ground_hit_mask ? GroundHit::HitAll : data.getGroundHit();
+    }
+
     u32 raw;
     Data data;
     GroundHitMask ground_hit;

src/KingSystem/Physics/System/physEntityGroupFilter.cpp

@@ -29,7 +29,7 @@ bool receiverMaskGetSensorLayerMaskForType(ReceiverMask* mask,
 void receiverMaskSetSensorLayerMask(ReceiverMask* mask, u32 layer_mask) {
     *mask = {};
     mask->layer_mask = layer_mask;
-    mask->unk_flag = true;
+    mask->is_custom_receiver = true;
 }
 
 EntityGroupFilter* EntityGroupFilter::make(ContactLayer::ValueType first,

src/KingSystem/Physics/System/physEntityGroupFilter.h

@@ -12,7 +12,7 @@ class EntitySystemGroupHandler : public SystemGroupHandler {
 public:
     explicit EntitySystemGroupHandler(int i) : SystemGroupHandler(i, ContactLayerType::Entity) {}
 
-    u32 m5() override;
+    u32 makeCollisionFilterInfo(u32 info, ContactLayer layer, GroundHit ground_hit) override;
     u32 m6() override;
     bool m8() override;
 };

src/KingSystem/Physics/System/physGroupFilter.h

@@ -17,7 +17,7 @@ public:
         : mIndex(index), mLayerType(layer_type) {}
 
     virtual ~SystemGroupHandler() = default;
-    virtual u32 m5() = 0;
+    virtual u32 makeCollisionFilterInfo(u32 info, ContactLayer layer, GroundHit ground_hit) = 0;
     virtual u32 m6() = 0;
     virtual u32 m7();
     virtual bool m8() = 0;

src/KingSystem/Physics/System/physInstanceSet.cpp

@@ -129,16 +129,16 @@ void InstanceSet::sub_7100FBB00C(phys::RigidBody* body, phys::RigidBodyParam* pa
     phys::RigidBodyInstanceParam instance_params;
     param->makeInstanceParam(&instance_params);
     if (instance_params.contact_layer == phys::ContactLayer::SensorCustomReceiver) {
-        body->sub_7100F8F9E8(&instance_params.receiver_mask, _188[body->isSensor()]);
+        body->setSensorCustomReceiver(instance_params.receiver_mask, _188[body->isSensor()]);
     } else if (instance_params.groundhit_mask) {
-        body->sub_7100F8FA44(instance_params.contact_layer, instance_params.groundhit_mask);
+        body->setGroundHitMask(instance_params.contact_layer, instance_params.groundhit_mask);
     } else {
-        body->sub_7100F8F8CC(instance_params.contact_layer, instance_params.groundhit,
+        body->setContactLayerAndGroundHitAndHandler(
-                             _188[body->isSensor()]);
+            instance_params.contact_layer, instance_params.groundhit, _188[body->isSensor()]);
     }
     body->enableGroundCollision(instance_params.no_hit_ground == 0);
     body->enableWaterCollision(instance_params.no_hit_water == 0);
-    body->sub_7100F8F51C();
+    body->clearSensorReceiverLayer2();
 }
 
 void* InstanceSet::sub_7100FBBC28(const sead::SafeString& name) const {

src/KingSystem/Physics/System/physInstanceSet.h

@@ -18,6 +18,8 @@ public:
 
 namespace ksys::phys {
 
+class SystemGroupHandler;
+
 class Ragdoll {};
 
 class RagdollController {
@@ -117,7 +119,7 @@ private:
     u8 _e0[0x148 - 0xe0];
     sead::TList<RigidBody*> mList;
     u8 _160[0x188 - 0x160];
-    void* _188[2];
+    SystemGroupHandler* _188[2];
 };
 KSYS_CHECK_SIZE_NX150(InstanceSet, 0x198);
 

src/KingSystem/Physics/System/physSensorGroupFilter.cpp

@@ -1 +1,50 @@
 #include "KingSystem/Physics/System/physSensorGroupFilter.h"
+#include <basis/seadRawPrint.h>
+
+namespace ksys::phys {
+
+u32 sensorReceiverMaskSetLayer(ContactLayer layer, u32 mask) {
+    SEAD_ASSERT(getContactLayerType(layer) == ContactLayerType::Sensor);
+
+    ReceiverMask info{mask};
+
+    if (layer == ContactLayer::SensorCustomReceiver) {
+        info.is_custom_receiver = true;
+    } else {
+        if (info.is_custom_receiver) {
+            clearBitFields(&info.raw, info.is_custom_receiver, info.custom_receiver_data.layer);
+        } else {
+            clearBitFields(&info.raw, info.is_custom_receiver, info.data.layer,
+                           info.data.has_layer2, info.data.layer2);
+        }
+
+        if (info.is_custom_receiver) {
+            info.custom_receiver_data.layer.Init(layer - FirstSensor);
+        } else {
+            info.data.layer.Init(layer - FirstSensor);
+        }
+    }
+
+    return info.raw;
+}
+
+u32 sensorReceiverMaskSetLayer2(bool set, ContactLayer layer, u32 mask) {
+    SEAD_ASSERT(getContactLayerType(layer) == ContactLayerType::Sensor);
+
+    ReceiverMask info{mask};
+    if (info.is_custom_receiver)
+        return info.raw;
+
+    info.is_custom_receiver = false;
+    info.data.has_layer2 = false;
+    info.data.layer2 = {};
+
+    if (set) {
+        info.data.has_layer2 = true;
+        info.data.layer2.SetUnsafe(layer - FirstSensor);
+    }
+
+    return info.raw;
+}
+
+}  // namespace ksys::phys

src/KingSystem/Physics/System/physSensorGroupFilter.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include "KingSystem/Physics/System/physDefines.h"
 #include "KingSystem/Physics/System/physGroupFilter.h"
 
 namespace ksys::phys {
@@ -11,4 +12,17 @@ public:
     static SensorGroupFilter* make(ContactLayer::ValueType last, sead::Heap* heap);
 };
 
+/// Set the contact layer in a sensor receiver mask.
+/// @param layer A sensor contact layer
+/// @param mask An existing receiver mask
+u32 sensorReceiverMaskSetLayer(ContactLayer layer, u32 mask);
+
+/// Set or clear a second contact layer in a sensor receiver mask.
+/// This function does nothing when using a custom receiver.
+/// @param set If true, set the specified layer. Clear it otherwise
+/// @param layer A sensor contact layer
+/// @param mask An existing receiver mask
+// TODO: rename once we figure out what this layer is used for
+u32 sensorReceiverMaskSetLayer2(bool set, ContactLayer layer, u32 mask);
+
 }  // namespace ksys::phys

src/KingSystem/Physics/System/physSystem.h

@@ -53,7 +53,7 @@ public:
                                        ContactLayer layer2, bool enabled);
 
     // 0x0000007101216a20
-    void x_1(RigidBody* body);
+    void registerRigidBodyForContactSystem(RigidBody* body);
 
     void removeSystemGroupHandler(SystemGroupHandler* handler);
 

src/KingSystem/Utils/BitField.h

@@ -185,16 +185,15 @@ struct BitField {
     static constexpr bool IsSigned() { return std::is_signed<T>(); }
     static constexpr std::size_t StartBit() { return position; }
     static constexpr std::size_t NumBits() { return bits; }
-
-private:
-    // Unsigned version of StorageType
-    using StorageTypeU = std::make_unsigned_t<StorageType>;
-
     static constexpr StorageType GetMask() {
         return (std::numeric_limits<StorageTypeU>::max() >> (8 * sizeof(StorageType) - bits))
                << position;
     }
 
+private:
+    // Unsigned version of StorageType
+    using StorageTypeU = std::make_unsigned_t<StorageType>;
+
     StorageType storage;
 
     static_assert(bits + position <= 8 * sizeof(StorageType), "Bitfield out of range");
@@ -207,4 +206,22 @@ private:
 };
 #pragma pack()
 
+/// Return the combined mask for all specified BitFields.
+template <typename Storage, typename... BitFields>
+constexpr Storage getMaskForBitFields() {
+    Storage mask{};
+    ((mask |= BitFields::GetMask()), ...);
+    return mask;
+}
+
+/// Clear several BitFields at once.
+///
+/// This can sometimes produce better codegen compared to setting each BitField to zero.
+/// (This function builds a mask for all the BitFields and clears those bits in one pass.)
+template <typename Storage, typename... BitFields>
+constexpr void clearBitFields(Storage* storage, const BitFields&... fields) {
+    constexpr Storage mask = getMaskForBitFields<Storage, BitFields...>();
+    *storage &= ~mask;
+}
+
 }  // namespace ksys::util