ksys/phys: Implement the easier parts of EntityGroupFilter

src/KingSystem/Physics/RigidBody/Shape/physCapsuleShape.cpp

@@ -18,7 +18,7 @@ CapsuleBody* CapsuleShape::init(sead::Heap* heap) {
         body->unk.shape_type = 1 << 23;
     }
     body->unk._10 = nullptr;
-    hk_shape->m_type = body->unk.shape_type;
+    hk_shape->setUserData(body->unk.shape_type);
     return body;
 }
 
@@ -38,7 +38,7 @@ CapsuleBody* CapsuleBody::clone(sead::Heap* heap) {
     body->unk.shape_type = unk.shape_type;
     body->unk._10 = nullptr;
     if (body->shape != nullptr)
-        body->shape->m_type = unk.shape_type;
+        body->shape->setUserData(unk.shape_type);
     return body;
 }
 

src/KingSystem/Physics/RigidBody/physRigidBodyParam.cpp

@@ -133,7 +133,7 @@ void RigidBodyParam::Info::postRead_() {
     }
 
     if (*use_ground_hit_type_mask && !ground_hit_type_mask->isEmpty()) {
-        ground_hit_type_mask_val = 0;
+        ground_hit_mask = 0;
 
         sead::FixedSafeString<64> type_str;
         auto it = ground_hit_type_mask->tokenBegin(",");
@@ -141,10 +141,10 @@ void RigidBodyParam::Info::postRead_() {
         while (end != it) {
             it.getAndForward(&type_str);
             if (!type_str.isEmpty())
-                ground_hit_type_mask_val = orGroundHitTypeMask(ground_hit_type_mask_val, type_str);
+                ground_hit_mask = orEntityGroundHitMask(ground_hit_mask, type_str);
         }
     } else {
-        ground_hit_type_mask_val = 0;
+        ground_hit_mask = 0;
     }
 }
 

src/KingSystem/Physics/RigidBody/physRigidBodyParam.h

@@ -137,7 +137,7 @@ struct RigidBodyParam : agl::utl::ParameterList {
         agl::utl::Parameter<int> shape_num;
         NavMeshType navmesh_val = NavMeshType::NOT_USE;
         NavMeshSubMaterial navmesh_sub_material_val = NavMeshSubMaterial::None;
-        u32 ground_hit_type_mask_val = 0;
+        u32 ground_hit_mask = 0;
 
     protected:
         void postRead_() override;

src/KingSystem/Physics/System/physEntityGroupFilter.cpp

@@ -1,5 +1,13 @@
 #include "KingSystem/Physics/System/physEntityGroupFilter.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/Shape/Compound/Collection/hkpShapeCollection.h>
+#include <Havok/Physics2012/Collide/Shape/Compound/Tree/hkpBvTreeShape.h>
+#include <Havok/Physics2012/Collide/Shape/hkpShapeContainer.h>
+#include <Havok/Physics2012/Dynamics/World/hkpWorldObject.h>
 #include <heap/seadHeap.h>
+#include "KingSystem/Utils/BitField.h"
 #include "KingSystem/Utils/HeapUtil.h"
 
 namespace ksys::phys {
@@ -7,6 +15,42 @@ namespace ksys::phys {
 constexpr int NumEntityHandlersInList0 = 0x10;
 constexpr int NumEntityHandlers = 0x400;
 
+namespace {
+
+/// Internal representation of collision masks for entities.
+/// This is exposed as a u32 externally.
+union EntityCollisionFilterInfo {
+    union Data {
+        ContactLayer getLayer() const { return int(layer); }
+        GroundHit getGroundHit() const { return int(ground_hit); }
+
+        util::BitField<0, 5, u32> layer;
+        util::BitField<24, 1, u32> unk24;
+        util::BitField<25, 1, u32> unk25;
+        util::BitField<26, 4, u32> ground_hit;
+        util::BitField<30, 1, u32> unk30;
+    };
+
+    union GroundHitMask {
+        ContactLayer getLayer() const { return int(layer); }
+
+        util::BitField<0, 8, 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_) {}
+
+    u32 raw;
+    Data data;
+    GroundHitMask ground_hit;
+    util::BitField<31, 1, u32> is_ground_hit_mask;
+};
+static_assert(sizeof(EntityCollisionFilterInfo) == sizeof(u32));
+
+}  // namespace
+
 EntityGroupFilter* EntityGroupFilter::make(ContactLayer::ValueType first,
                                            ContactLayer::ValueType last, sead::Heap* heap) {
     auto* filter = util::alloc<EntityGroupFilter>(heap, first, last);
@@ -24,6 +68,110 @@ void EntityGroupFilter::doInit_(sead::Heap* heap) {
     mMasks.fill(0xffffffff);
 }
 
+hkBool EntityGroupFilter::isCollisionEnabledPhantom(u32 infoPhantom, u32 infoB) const {
+    if (mInhibitCollisions)
+        return false;
+
+    // TODO: figure out what kind of mask infoPhantom is. Receiver/sensor mask?
+    // RigidBodyParam::getParams and ContactInfoTable seem to manipulate similar looking masks.
+    const EntityCollisionFilterInfo info{infoB};
+    if (info.is_ground_hit_mask)
+        return infoPhantom & (1 << info.ground_hit.getLayer());
+    return (infoPhantom & (1 << info.data.layer)) & 0x1ffff;
+}
+
+hkBool EntityGroupFilter::isCollisionEnabled(const hkpCollidable& a, const hkpCollidable& b) const {
+    if (a.getType() == hkpWorldObject::BROAD_PHASE_PHANTOM &&
+        b.getType() == hkpWorldObject::BROAD_PHASE_PHANTOM) {
+        return false;
+    }
+
+    if (a.getType() == hkpWorldObject::BROAD_PHASE_PHANTOM) {
+        if (a.getShape() != nullptr)
+            return isCollisionEnabled(a.getCollisionFilterInfo(), b.getCollisionFilterInfo());
+
+        return isCollisionEnabledPhantom(a.getCollisionFilterInfo(), b.getCollisionFilterInfo());
+    }
+
+    if (b.getType() == hkpWorldObject::BROAD_PHASE_PHANTOM) {
+        if (b.getShape() != nullptr)
+            return isCollisionEnabled(a.getCollisionFilterInfo(), b.getCollisionFilterInfo());
+
+        return isCollisionEnabledPhantom(b.getCollisionFilterInfo(), a.getCollisionFilterInfo());
+    }
+
+    return isCollisionEnabled(a.getCollisionFilterInfo(), b.getCollisionFilterInfo());
+}
+
+hkBool EntityGroupFilter::isCollisionEnabled(const hkpCollisionInput& input,
+                                             const hkpCdBody& collectionBodyA,
+                                             const hkpCdBody& collectionBodyB,
+                                             const hkpShapeContainer& containerShapeA,
+                                             const hkpShapeContainer& containerShapeB,
+                                             hkpShapeKey keyA, hkpShapeKey keyB) const {
+    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 isCollisionEnabled(infoA, infoB);
+}
+
+hkBool EntityGroupFilter::isCollisionEnabled(const hkpCollisionInput& input, const hkpCdBody& a,
+                                             const hkpCdBody& b,
+                                             const hkpShapeContainer& bContainer,
+                                             hkpShapeKey bKey) const {
+    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 isCollisionEnabled(infoA, infoB);
+}
+
 void EntityGroupFilter::doInitSystemGroupHandlerLists_(sead::Heap* heap) {
     for (auto& list : mFreeLists)
         list.initOffset(SystemGroupHandler::getFreeListNodeOffset());
@@ -40,13 +188,50 @@ int EntityGroupFilter::getFreeListIndex(const SystemGroupHandler* handler) {
     return handler->getIndex() < NumEntityHandlersInList0;
 }
 
-u32 orGroundHitTypeMask(u32 mask, GroundHit type) {
-    mask |= (0x100 << type) & 0xffff00;
-    return mask;
+u32 orEntityGroundHitMask(u32 mask, GroundHit type) {
+    EntityCollisionFilterInfo info{mask};
+    info.ground_hit.ground_hit_types |= 1 << type;
+    return info.raw;
 }
 
-u32 orGroundHitTypeMask(u32 mask, const sead::SafeString& type) {
-    return orGroundHitTypeMask(mask, groundHitFromText(type));
+u32 orEntityGroundHitMask(u32 mask, const sead::SafeString& type) {
+    return orEntityGroundHitMask(mask, groundHitFromText(type));
+}
+
+template <bool WithUnk>
+static EntityCollisionFilterInfo makeEntityGroundHitMaskImpl(ContactLayer layer, u32 mask) {
+    const EntityCollisionFilterInfo current{mask};
+    EntityCollisionFilterInfo info{};
+    info.ground_hit.layer.SetUnsafe(layer);
+    info.ground_hit.ground_hit_types = current.ground_hit.ground_hit_types;
+    info.is_ground_hit_mask = true;
+    if constexpr (WithUnk)
+        info.ground_hit.unk = current.ground_hit.unk & 1;
+    return info;
+}
+
+u32 makeEntityGroundHitMask(ContactLayer layer, u32 mask) {
+    return makeEntityGroundHitMaskImpl<false>(layer, mask).raw;
+}
+
+u32 makeEntityCollisionMask(ContactLayer layer, u32 mask) {
+    EntityCollisionFilterInfo current{mask};
+    if (current.is_ground_hit_mask) {
+        return makeEntityGroundHitMaskImpl<true>(layer, mask).raw;
+    } else {
+        current.data.layer.SetUnsafe(layer);
+        return current.raw;
+    }
+}
+
+u32 setEntityCollisionMaskGroundHit(GroundHit ground_hit, u32 mask) {
+    EntityCollisionFilterInfo current{mask};
+    if (current.is_ground_hit_mask) {
+        // This shouldn't happen: this function is not supposed to be called on ground hit masks.
+    } else {
+        current.data.ground_hit.SetUnsafe(ground_hit);
+    }
+    return current.raw;
 }
 
 }  // namespace ksys::phys

src/KingSystem/Physics/System/physEntityGroupFilter.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <container/seadSafeArray.h>
+#include <prim/seadBitUtil.h>
 #include "KingSystem/Physics/System/physDefines.h"
 #include "KingSystem/Physics/System/physGroupFilter.h"
 
@@ -51,6 +52,9 @@ public:
     void m10() override {}
 
 private:
+    hkBool isCollisionEnabled(u32 infoA, u32 infoB) const;
+    hkBool isCollisionEnabledPhantom(u32 infoPhantom, u32 infoB) const;
+
     void doInitSystemGroupHandlerLists_(sead::Heap* heap) override;
     int getFreeListIndex(const SystemGroupHandler* handler) override;
     void doInit_(sead::Heap* heap) override;
@@ -60,7 +64,18 @@ private:
     sead::SafeArray<u32, ContactLayer::size()> mMasks;
 };
 
-u32 orGroundHitTypeMask(u32 mask, GroundHit type);
-u32 orGroundHitTypeMask(u32 mask, const sead::SafeString& type);
+u32 orEntityGroundHitMask(u32 mask, GroundHit type);
+u32 orEntityGroundHitMask(u32 mask, const sead::SafeString& type);
+
+/// Returns a new collision mask in ground hit mask mode.
+/// @param mask A collision mask that has been built using orEntityGroundHitMask.
+u32 makeEntityGroundHitMask(ContactLayer layer, u32 mask);
+
+/// Returns a new collision mask with the specified layer.
+/// Preserves ground hit mask mode.
+u32 makeEntityCollisionMask(ContactLayer layer, u32 mask);
+
+/// Updates the collision mask with the specified ground hit type (*not* mask).
+u32 setEntityCollisionMaskGroundHit(GroundHit ground_hit, u32 mask);
 
 }  // namespace ksys::phys

src/KingSystem/Utils/BitField.h

@@ -0,0 +1,195 @@
+// Copyright 2014 Tony Wasserka
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//     * Redistributions of source code must retain the above copyright
+//       notice, this list of conditions and the following disclaimer.
+//     * Redistributions in binary form must reproduce the above copyright
+//       notice, this list of conditions and the following disclaimer in the
+//       documentation and/or other materials provided with the distribution.
+//     * Neither the name of the owner nor the names of its contributors may
+//       be used to endorse or promote products derived from this software
+//       without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#pragma once
+
+#include <cstddef>
+#include <limits>
+#include <type_traits>
+
+namespace ksys::util {
+
+/*
+ * Abstract bitfield class
+ *
+ * Allows endianness-independent access to individual bitfields within some raw
+ * integer value. The assembly generated by this class is identical to the
+ * usage of raw bitfields, so it's a perfectly fine replacement.
+ *
+ * For BitField<X,Y,Z>, X is the distance of the bitfield to the LSB of the
+ * raw value, Y is the length in bits of the bitfield. Z is an integer type
+ * which determines the sign of the bitfield. Z must have the same size as the
+ * raw integer.
+ *
+ *
+ * General usage:
+ *
+ * Create a new union with the raw integer value as a member.
+ * Then for each bitfield you want to expose, add a BitField member
+ * in the union. The template parameters are the bit offset and the number
+ * of desired bits.
+ *
+ * Changes in the bitfield members will then get reflected in the raw integer
+ * value and vice-versa.
+ *
+ *
+ * Sample usage:
+ *
+ * union SomeRegister
+ * {
+ *     u32 hex;
+ *
+ *     BitField<0,7,u32> first_seven_bits;     // unsigned
+ *     BitField<7,8,u32> next_eight_bits;      // unsigned
+ *     BitField<3,15,s32> some_signed_fields;  // signed
+ * };
+ *
+ * This is equivalent to the little-endian specific code:
+ *
+ * union SomeRegister
+ * {
+ *     u32 hex;
+ *
+ *     struct
+ *     {
+ *         u32 first_seven_bits : 7;
+ *         u32 next_eight_bits : 8;
+ *     };
+ *     struct
+ *     {
+ *         u32 : 3; // padding
+ *         s32 some_signed_fields : 15;
+ *     };
+ * };
+ *
+ *
+ * Caveats:
+ *
+ * 1)
+ * BitField provides automatic casting from and to the storage type where
+ * appropriate. However, when using non-typesafe functions like printf, an
+ * explicit cast must be performed on the BitField object to make sure it gets
+ * passed correctly, e.g.:
+ * printf("Value: %d", (s32)some_register.some_signed_fields);
+ *
+ * 2)
+ * Not really a caveat, but potentially irritating: This class is used in some
+ * packed structures that do not guarantee proper alignment. Therefore we have
+ * to use #pragma pack here not to pack the members of the class, but instead
+ * to break GCC's assumption that the members of the class are aligned on
+ * sizeof(StorageType).
+ */
+#pragma pack(1)
+template <std::size_t position, std::size_t bits, typename T,
+          // StorageType is T for non-enum types and the underlying type of T if
+          // T is an enumeration. Note that T is wrapped within an enable_if in the
+          // former case to workaround compile errors which arise when using
+          // std::underlying_type<T>::type directly.
+          typename StorageType = typename std::conditional_t<
+              std::is_enum<T>::value, std::underlying_type<T>, std::enable_if<true, T>>::type>
+struct BitField {
+    // Force default constructor to be created
+    // so that we can use this within unions
+    constexpr BitField() = default;
+
+    // This constructor might be considered ambiguous:
+    // Would it initialize the storage or just the bitfield?
+    // Hence, delete it. Use the assignment operator to set bitfield values!
+    BitField(T val) = delete;
+
+    inline constexpr void Set(T val) {
+        storage =
+            (storage & ~GetMask()) | ((static_cast<StorageType>(val) << position) & GetMask());
+    }
+
+    /// @warning This does *not* check whether the value fits within the mask,
+    /// so this might overwrite unrelated fields! Using Set() is preferred.
+    inline constexpr void SetUnsafe(T val) {
+        storage = (storage & ~GetMask()) | (static_cast<StorageType>(val) << position);
+    }
+
+    inline constexpr BitField& operator=(const BitField& other) {
+        Set(other.Value());
+        return *this;
+    }
+
+    inline constexpr BitField& operator=(T val) {
+        Set(val);
+        return *this;
+    }
+
+#define BITFIELD_DEFINE_OP_(OP, OP_EQUAL)                                                          \
+    inline constexpr BitField& operator OP_EQUAL(T val) {                                          \
+        *this = Value() OP val;                                                                    \
+        return *this;                                                                              \
+    }
+
+    BITFIELD_DEFINE_OP_(|, |=)
+    BITFIELD_DEFINE_OP_(^, ^=)
+    BITFIELD_DEFINE_OP_(&, &=)
+    BITFIELD_DEFINE_OP_(+, +=)
+    BITFIELD_DEFINE_OP_(-, -=)
+    BITFIELD_DEFINE_OP_(*, *=)
+    BITFIELD_DEFINE_OP_(/, /=)
+#undef BITFIELD_DEFINE_OP_
+
+    constexpr T Value() const {
+        if constexpr (IsSigned()) {
+            const size_t shift_amount = 8 * sizeof(StorageType) - bits;
+            return static_cast<T>((storage << (shift_amount - position)) >> shift_amount);
+        } else {
+            return static_cast<T>((storage & GetMask()) >> position);
+        }
+    }
+
+    constexpr operator T() const { return Value(); }  // NOLINT(google-explicit-constructor)
+    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;
+    }
+
+    StorageType storage;
+
+    static_assert(bits + position <= 8 * sizeof(StorageType), "Bitfield out of range");
+    static_assert(sizeof(T) <= sizeof(StorageType), "T must fit in StorageType");
+
+    // And, you know, just in case people specify something stupid like bits=position=0x80000000
+    static_assert(position < 8 * sizeof(StorageType), "Invalid position");
+    static_assert(bits <= 8 * sizeof(T), "Invalid number of bits");
+    static_assert(bits > 0, "Invalid number of bits");
+};
+#pragma pack()
+
+}  // namespace ksys::util