ksys/phys: Add more RigidBody functions (motion, collision mask)

src/KingSystem/Physics/RigidBody/physRigidBody.cpp

@@ -1,4 +1,5 @@
 #include "KingSystem/Physics/RigidBody/physRigidBody.h"
+#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/Dynamics/Collide/hkpResponseModifier.h>
@@ -176,6 +177,10 @@ sead::SafeString RigidBody::getHkBodyName() const {
     return name;
 }
 
+hkpCollidable* RigidBody::getCollidable() const {
+    return getHkBody()->getCollidableRw();
+}
+
 // NON_MATCHING: ldr w8, [x20, #0x68] should be ldr w8, [x22] (equivalent)
 void RigidBody::x_0() {
     // debug code that survived because mFlags is atomic
@@ -350,6 +355,111 @@ MotionType RigidBody::getMotionType() const {
     return mRigidBodyAccessor.getMotionType();
 }
 
+void RigidBody::replaceMotionObject() {
+    auto* motion = getMotion();
+    const hkMotionState motion_state = *motion->getMotionState();
+    const auto linvel = mHkBody->getLinearVelocity();
+    const auto angvel = mHkBody->getAngularVelocity();
+    const auto counter = motion->m_deactivationIntegrateCounter;
+    const auto frame0 = motion->m_deactivationNumInactiveFrames[0];
+    const auto frame1 = motion->m_deactivationNumInactiveFrames[1];
+
+    if (mMotionFlags.isOn(MotionFlag::Fixed)) {
+        const auto position = motion->getPosition();
+        const auto rotation = motion->getRotation();
+        new (motion) hkpFixedRigidMotion(position, rotation);
+
+        // Restore relevant motion state.
+        *motion->getMotionState() = motion_state;
+        mHkBody->m_solverData = 0;
+        mHkBody->setQualityType(HK_COLLIDABLE_QUALITY_FIXED);
+
+        mMotionFlags.reset(MotionFlag::Fixed);
+
+        motion->m_deactivationIntegrateCounter = counter;
+        motion->m_deactivationNumInactiveFrames[0] = frame0;
+        motion->m_deactivationNumInactiveFrames[1] = frame1;
+
+        // Freeze the motion state.
+        const auto inv_delta = motion_state.getSweptTransform().getInvDeltaTimeSr();
+        if (!inv_delta.isEqualZero()) {
+            hkSimdReal time;
+            if (auto* world = mHkBody->getWorld()) {
+                time = world->getCurrentTime();
+            } else {
+                time = inv_delta.reciprocal() + motion_state.getSweptTransform().getBaseTimeSr();
+            }
+            hkSweptTransformUtil::freezeMotionState(time, *motion->getMotionState());
+        }
+
+    } else if (mMotionFlags.isOn(MotionFlag::Keyframed)) {
+        const auto position = motion->getPosition();
+        const auto rotation = motion->getRotation();
+        new (getMotion()) hkpKeyframedRigidMotion(position, rotation);
+
+        // Restore relevant motion state.
+        *motion->getMotionState() = motion_state;
+        motion->m_linearVelocity = linvel;
+        motion->m_angularVelocity = angvel;
+        mHkBody->m_solverData = 0;
+        motion->m_deactivationIntegrateCounter = counter;
+        motion->m_deactivationNumInactiveFrames[0] = frame0;
+        motion->m_deactivationNumInactiveFrames[1] = frame1;
+        const bool is_entity = isEntity();
+        mHkBody->setQualityType(is_entity && hasFlag(Flag::HighQualityCollidable) ?
+                                    HK_COLLIDABLE_QUALITY_MOVING :
+                                    HK_COLLIDABLE_QUALITY_KEYFRAMED_REPORTING);
+        mMotionFlags.reset(MotionFlag::Keyframed);
+
+    } else if (mMotionFlags.isOn(MotionFlag::Dynamic)) {
+        getEntityMotionAccessor()->updateRigidBodyMotionExceptStateAndVel();
+        mHkBody->setQualityType(hasFlag(RigidBody::Flag::HighQualityCollidable) ?
+                                    HK_COLLIDABLE_QUALITY_BULLET :
+                                    HK_COLLIDABLE_QUALITY_DEBRIS_SIMPLE_TOI);
+        mMotionFlags.reset(MotionFlag::Dynamic);
+    }
+
+    mHkBody->getCollidableRw()->setMotionState(getMotion()->getMotionState());
+    // XXX: what the heck?
+    mHkBody->getCollidableRw()->setMotionState(getMotion()->getMotionState());
+
+    if (auto* shape = mHkBody->getCollidable()->getShape()) {
+        hkVector4 extent_out;
+        mHkBody->updateCachedShapeInfo(shape, extent_out);
+    }
+
+    if (auto* world = mHkBody->getWorld()) {
+        hkpSolverInfo* solver_info = world->getSolverInfo();
+        getMotion()->setWorldSelectFlagsNeg(
+            solver_info->m_deactivationNumInactiveFramesSelectFlag[0],
+            solver_info->m_deactivationNumInactiveFramesSelectFlag[1],
+            solver_info->m_deactivationIntegrateCounter);
+    }
+}
+
+void RigidBody::x_10() {
+    auto lock = makeScopedLock(isFlag8Set());
+
+    if (isEntity()) {
+        if (mMotionAccessor &&
+            getEntityMotionAccessor()->hasFlag(RigidBodyMotionEntity::Flag::_2)) {
+            mFlags.reset(Flag::_20);
+            getEntityMotionAccessor()->deregisterAllAccessors();
+        }
+    } else {  // isSensor()
+        auto* accessor = getSensorMotionAccessor();
+        if (accessor && accessor->getLinkedRigidBody() != nullptr) {
+            mFlags.reset(Flag::_20);
+            resetLinkedRigidBody();
+        }
+    }
+
+    mFlags.set(Flag::_20);
+    mFlags.set(Flag::_4);
+
+    x_8(nullptr);
+}
+
 void RigidBody::setContactPoints(RigidContactPoints* points) {
     mContactPoints = points;
     if (isFlag8Set() && mContactPoints && !mContactPoints->isLinked())
@@ -442,6 +552,22 @@ void RigidBody::setCollidableQualityType(hkpCollidableQualityType quality) {
     getHkBody()->getCollidableRw()->setQualityType(quality);
 }
 
+static int getLayerBit(int layer, ContactLayerType type) {
+    // This is layer for Entity layers and layer - 0x20 for Sensor layers.
+    // XXX: this should be using makeContactLayerMask.
+    return layer - ContactLayer::SensorObject * int(type);
+}
+
+void RigidBody::addContactLayer(ContactLayer layer) {
+    assertLayerType(layer);
+    mContactMask.setBit(getLayerBit(layer, getLayerType()));
+}
+
+void RigidBody::removeContactLayer(ContactLayer layer) {
+    assertLayerType(layer);
+    mContactMask.resetBit(getLayerBit(layer, getLayerType()));
+}
+
 void RigidBody::setContactMask(u32 value) {
     mContactMask.setDirect(value);
 }
@@ -454,6 +580,73 @@ void RigidBody::setContactNone() {
     mContactMask.makeAllZero();
 }
 
+void RigidBody::enableGroundCollision(bool enabled) {
+    if (!isEntity() || isGroundCollisionEnabled() == enabled)
+        return;
+
+    if (int(getContactLayer()) == ContactLayer::EntityRagdoll)
+        return;
+
+    const auto current_info = getCollisionFilterInfo();
+    auto info = current_info;
+    info.unk5 = false;
+    info.no_ground_collision.SetBit(!enabled);
+    if (current_info != info)
+        setCollisionFilterInfo(info.raw);
+}
+
+bool RigidBody::isGroundCollisionEnabled() const {
+    if (!isEntity())
+        return false;
+
+    const auto info = getCollisionFilterInfo();
+
+    bool enabled = false;
+    enabled |= info.unk5;
+    enabled |= info.unk30;
+    enabled |= !info.no_ground_collision;
+    return enabled;
+}
+
+void RigidBody::enableWaterCollision(bool enabled) {
+    if (!isEntity() || isWaterCollisionEnabled() == enabled)
+        return;
+
+    if (int(getContactLayer()) == ContactLayer::EntityRagdoll)
+        return;
+
+    const auto current_info = getCollisionFilterInfo();
+    auto info = current_info;
+    info.no_water_collision = !enabled;
+    if (current_info != info)
+        setCollisionFilterInfo(info.raw);
+}
+
+bool RigidBody::isWaterCollisionEnabled() const {
+    if (!isEntity())
+        return false;
+
+    const auto info = getCollisionFilterInfo();
+
+    bool enabled = false;
+    // unk30 enables all collisions?
+    enabled |= info.unk30;
+    enabled |= !info.no_water_collision;
+    return enabled;
+}
+
+ContactLayer RigidBody::getContactLayer() const {
+    return getContactLayer(getCollisionFilterInfo());
+}
+
+ContactLayer RigidBody::getContactLayer(EntityCollisionFilterInfo info) const {
+    return isSensor() ? info.getLayerSensor() : info.getLayer();
+}
+
+EntityCollisionFilterInfo RigidBody::getCollisionFilterInfo() const {
+    return EntityCollisionFilterInfo(mHkBody->getCollisionFilterInfo());
+}
+
 void RigidBody::setPosition(const sead::Vector3f& position, bool propagate_to_linked_motions) {
     if (isVectorInvalid(position)) {
         onInvalidParameter();
@@ -1137,6 +1330,12 @@ bool RigidBody::isEntityMotionFlag200On() const {
     return getEntityMotionAccessor()->hasFlag(RigidBodyMotionEntity::Flag::_200);
 }
 
+void RigidBody::assertLayerType(ContactLayer layer) const {
+    const auto type = getContactLayerType(layer);
+    const auto expected_type = getLayerType();
+    SEAD_ASSERT(type == expected_type);
+}
+
 void RigidBody::onInvalidParameter(int code) {
     sead::Vector3f pos, lin_vel, ang_vel;
     mRigidBodyAccessor.getPosition(&pos);

src/KingSystem/Physics/RigidBody/physRigidBody.h

@@ -15,6 +15,7 @@
 enum hkpCollidableQualityType : int;
 class hkQuaternionf;
 class hkVector4f;
+class hkpCollidable;
 class hkpRigidBody;
 class hkpMaxSizeMotion;
 class hkpMotion;
@@ -130,6 +131,7 @@ public:
                       const RigidBodyInstanceParam& param);
 
     sead::SafeString getHkBodyName() const;
+    hkpCollidable* getCollidable() const;
 
     void x_0();
 
@@ -139,7 +141,6 @@ public:
     bool isMotionFlag1Set() const;
     bool isMotionFlag2Set() const;
     void sub_7100F8D21C();
-    // 0x0000007100f8d308
     bool x_6();
 
     /// Get the motion accessor if it is a RigidBodyMotionEntity. Returns nullptr otherwise.
@@ -159,13 +160,11 @@ public:
     bool isSensorMotionFlag40000Set() const;
 
     // 0x0000007100f8d840
-    void x_8();
+    void x_8(void* arg);
 
     MotionType getMotionType() const;
 
-    // Motion functions
-    // 0x0000007100f8dcfc
-    void x_9();
+    void replaceMotionObject();
     // 0x0000007100f8e110
     void x_10();
     // 0x0000007100f8e3fc
@@ -173,7 +172,6 @@ public:
 
     // 0x0000007100f8e72c
     void x_12();
-    // 0x0000007100f8e7b4
     void setContactPoints(RigidContactPoints* points);
 
     void freeze(bool should_freeze, bool preserve_velocities, bool preserve_max_impulse);
@@ -183,13 +181,24 @@ public:
 
     void updateCollidableQualityType(bool high_quality);
 
-    u32 addContactLayer(ContactLayer);
-    u32 removeContactLayer(ContactLayer);
+    void addContactLayer(ContactLayer layer);
+    void removeContactLayer(ContactLayer layer);
     void setContactMask(u32);
     void setContactAll();
     void setContactNone();
-    void setCollideGround(bool);
-    void setCollideWater(bool);
+
+    void enableGroundCollision(bool enabled);
+    bool isGroundCollisionEnabled() const;
+
+    void enableWaterCollision(bool enabled);
+    bool isWaterCollisionEnabled() const;
+
+    ContactLayer getContactLayer() const;
+    ContactLayer getContactLayer(EntityCollisionFilterInfo info) const;
+    EntityCollisionFilterInfo getCollisionFilterInfo() const;
+    // 0x0000007100f8f1c4
+    void setCollisionFilterInfo(u32 info);
+
     void sub_7100F8F51C();
     void sub_7100F8F8CC(ContactLayer, GroundHit, void*);
     void sub_7100F8F9E8(ReceiverMask*, void*);
@@ -379,6 +388,7 @@ public:
 
 private:
     void createMotionAccessor(sead::Heap* heap);
+    void assertLayerType(ContactLayer layer) const;
     void onInvalidParameter(int code = 0);
     void notifyUserTag(int code);
     void updateDeactivation();

src/KingSystem/Physics/System/physDefines.h

@@ -189,6 +189,54 @@ union ReceiverMask {
     util::BitField<31, 1, u32> unk_flag;
 };
 
+union EntityCollisionFilterInfo {
+    union Data {
+        ContactLayer getLayer() const { return int(layer); }
+        ContactLayer getLayerSensor() const { return int(layer + ContactLayer::SensorObject); }
+        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;
+    };
+
+    union GroundHitMask {
+        ContactLayer getLayer() const { return int(layer); }
+
+        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_) {}
+
+    bool operator==(EntityCollisionFilterInfo rhs) const { return raw == rhs.raw; }
+    bool operator!=(EntityCollisionFilterInfo rhs) const { return raw != rhs.raw; }
+
+    ContactLayer getLayer() const {
+        return is_ground_hit_mask ? ground_hit.getLayer() : data.getLayer();
+    }
+
+    ContactLayer getLayerSensor() const {
+        return is_ground_hit_mask ? ContactLayer(ContactLayer::SensorCustomReceiver) :
+                                    data.getLayerSensor();
+    }
+
+    u32 raw;
+    Data data;
+    GroundHitMask ground_hit;
+    util::BitField<5, 1, bool, u32> unk5;
+    /// Whether ground collision is disabled.
+    util::BitField<6, 1, bool, u32> no_ground_collision;
+    /// Whether water collision is disabled.
+    util::BitField<7, 1, bool, u32> no_water_collision;
+    util::BitField<30, 1, bool, u32> unk30;
+    util::BitField<31, 1, bool, u32> is_ground_hit_mask;
+};
+static_assert(sizeof(EntityCollisionFilterInfo) == sizeof(u32));
+
 ContactLayerType getContactLayerType(ContactLayer layer);
 u32 makeContactLayerMask(ContactLayer layer);
 u32 getContactLayerBase(ContactLayerType type);

src/KingSystem/Physics/System/physEntityGroupFilter.cpp

@@ -17,42 +17,6 @@ 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
-
 void receiverMaskEnableLayer(ReceiverMask* mask, ContactLayer layer) {
     mask->raw |= 1 << getContactLayerBaseRelativeValue(layer);
 }

src/KingSystem/Physics/System/physInstanceSet.cpp

@@ -136,8 +136,8 @@ void InstanceSet::sub_7100FBB00C(phys::RigidBody* body, phys::RigidBodyParam* pa
         body->sub_7100F8F8CC(instance_params.contact_layer, instance_params.groundhit,
                              _188[body->isSensor()]);
     }
-    body->setCollideGround(instance_params.no_hit_ground == 0);
-    body->setCollideWater(instance_params.no_hit_water == 0);
+    body->enableGroundCollision(instance_params.no_hit_ground == 0);
+    body->enableWaterCollision(instance_params.no_hit_water == 0);
     body->sub_7100F8F51C();
 }
 

src/KingSystem/Utils/BitField.h

@@ -130,6 +130,12 @@ struct BitField {
             (storage & ~GetMask()) | ((static_cast<StorageType>(val) << position) & GetMask());
     }
 
+    template <auto bits_ = bits, typename = std::enable_if_t<bits_ == 1>>
+    inline constexpr void SetBit(bool set) {
+        const auto mask = set ? ((static_cast<StorageType>(1) << position) & GetMask()) : 0;
+        storage = (storage & ~GetMask()) | mask;
+    }
+
     /// @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) {