ksys/phys: Add more RigidBody functions

And fix a bunch of hkVector4f / hkSimdFloat32 interop matching issues.

data/uking_functions.csv

@@ -83118,29 +83118,29 @@ Address,Quality,Size,Name
 0x0000007100f955c0,U,000768,phys::RigidBody::x_108
 0x0000007100f958c0,U,000416,phys::RigidBody::x_109
 0x0000007100f95a60,U,000376,phys::RigidBody::x_110
-0x0000007100f95bd8,U,000316,phys::RigidBody::x_111
-0x0000007100f95d14,U,000316,phys::RigidBody::x_112
-0x0000007100f95e50,U,000316,phys::RigidBody::x_113
+0x0000007100f95bd8,O,000316,_ZN4ksys4phys9RigidBody16clearFlag2000000Eb
+0x0000007100f95d14,O,000316,_ZN4ksys4phys9RigidBody16clearFlag4000000Eb
+0x0000007100f95e50,O,000316,_ZN4ksys4phys9RigidBody16clearFlag8000000Eb
 0x0000007100f95f8c,U,001048,phys::RigidBody::x_114
-0x0000007100f963a4,U,000156,phys::RigidBody::x_115
-0x0000007100f96440,U,000156,phys::RigidBody::x_116
-0x0000007100f964dc,U,000148,phys::RigidBody::x_117
-0x0000007100f96570,U,000008,phys::RigidBody::m10
-0x0000007100f96578,U,000008,phys::RigidBody::m11
+0x0000007100f963a4,O,000156,_ZNK4ksys4phys9RigidBody22isEntityMotionFlag80OnEv
+0x0000007100f96440,O,000156,_ZN4ksys4phys9RigidBody25setMagneMassScalingFactorEf
+0x0000007100f964dc,O,000148,_ZNK4ksys4phys9RigidBody25getMagneMassScalingFactorEv
+0x0000007100f96570,O,000008,_ZN4ksys4phys9RigidBody3m10Ev
+0x0000007100f96578,O,000008,_ZN4ksys4phys9RigidBody3m11Ev
 0x0000007100f96580,O,000240,_ZN4ksys4phys9RigidBody13resetPositionEv
 0x0000007100f96670,O,000144,_ZN4ksys4phys9RigidBody7getNameEv
-0x0000007100f96700,U,000068,phys::RigidBody::m5
-0x0000007100f96744,U,000176,phys::RigidBody::x_118
-0x0000007100f967f4,U,000428,phys::RigidBody::x_119
+0x0000007100f96700,O,000068,_ZNK4ksys4phys9RigidBody11logPositionEv
+0x0000007100f96744,O,000176,_ZNK4ksys4phys9RigidBody14getAabbInLocalEPN4sead9BoundBox3IfEE
+0x0000007100f967f4,m,000428,_ZNK4ksys4phys9RigidBody14getAabbInWorldEPN4sead9BoundBox3IfEE
 0x0000007100f969a0,O,000072,_ZN4ksys4phys9RigidBody4lockEb
 0x0000007100f969e8,O,000088,_ZN4ksys4phys9RigidBody6unlockEb
 0x0000007100f96a40,O,000012,_ZNK4ksys4phys9RigidBody9getMotionEv
 0x0000007100f96a4c,U,000076,phys::RigidBody::x_123
-0x0000007100f96a98,U,000188,phys::RigidBody::x_124
-0x0000007100f96b54,U,000156,phys::RigidBody::x_125
-0x0000007100f96bf0,U,000204,phys::RigidBody::x_126
-0x0000007100f96cbc,U,000156,phys::RigidBody::x_127
-0x0000007100f96d58,U,000204,phys::RigidBody::x_128
+0x0000007100f96a98,O,000188,_ZN4ksys4phys9RigidBody20setEntityMotionFlag1Eb
+0x0000007100f96b54,O,000156,_ZNK4ksys4phys9RigidBody21isEntityMotionFlag1OnEv
+0x0000007100f96bf0,O,000204,_ZN4ksys4phys9RigidBody22setEntityMotionFlag100Eb
+0x0000007100f96cbc,O,000156,_ZNK4ksys4phys9RigidBody23isEntityMotionFlag100OnEv
+0x0000007100f96d58,O,000204,_ZN4ksys4phys9RigidBody22setEntityMotionFlag200Eb
 0x0000007100f96e24,U,000096,
 0x0000007100f96e84,U,000384,
 0x0000007100f97004,O,000112,_ZNK4ksys4phys9RigidBody27checkDerivedRuntimeTypeInfoEPKN4sead15RuntimeTypeInfo9InterfaceE

lib/hkStubs/Havok/Common/Base/Math/Matrix/hkTransformf.h

@@ -21,6 +21,8 @@ public:
 
     HK_FORCE_INLINE void set(const hkRotationf& r, hkVector4fParameter t);
     HK_FORCE_INLINE void set(hkQuaternionfParameter q, hkVector4fParameter t);
+
+    HK_FORCE_INLINE static const hkTransformf& getIdentity();
     HK_FORCE_INLINE void setIdentity();
 
     hkRotationf m_rotation;
@@ -56,6 +58,10 @@ inline void hkTransformf::set(const hkQuaternionf& q, const hkVector4f& t) {
     m_translation = t;
 }
 
+inline const hkTransformf& hkTransformf::getIdentity() {
+    return reinterpret_cast<const hkTransformf&>(g_vectorfConstants[HK_QUADREAL_1000]);
+}
+
 inline void hkTransformf::setIdentity() {
     m_rotation.setIdentity();
     m_translation.setZero();

lib/hkStubs/Havok/Common/Base/Math/Vector/hkSimdFloat32.h

@@ -60,6 +60,8 @@ public:
 
     void setAbs(hkSimdFloat32Parameter x);
 
+    HK_FORCE_INLINE m128 toQuad() const;
+
     Storage m_real;
 };
 
@@ -150,3 +152,11 @@ inline void hkSimdFloat32::setAbs(hkSimdFloat32Parameter x) {
         m_real[i] = std::abs(x.m_real[i]);
 #endif
 }
+
+inline m128 hkSimdFloat32::toQuad() const {
+#ifdef HK_SIMD_FLOAT32_AARCH64_NEON
+    return vcombine_f32(m_real, m_real);
+#else
+    return m_real;
+#endif
+}

lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.h

@@ -31,6 +31,7 @@ public:
     HK_FORCE_INLINE void setXYZ(hkVector4fParameter xyz);
     HK_FORCE_INLINE void setXYZ_W(hkVector4fParameter xyz, hkSimdFloat32Parameter w);
     HK_FORCE_INLINE void setAll(hkFloat32 x);
+    HK_FORCE_INLINE void setAll(hkSimdFloat32Parameter x);
     HK_FORCE_INLINE void setZero();
 
     // ========== Vector operations

lib/hkStubs/Havok/Common/Base/Math/Vector/hkVector4f.inl

@@ -55,6 +55,10 @@ inline void hkVector4f::setAll(hkReal x) {
     v = {x, x, x, x};
 }
 
+inline void hkVector4f::setAll(hkSimdFloat32Parameter x) {
+    v = x.toQuad();
+}
+
 inline void hkVector4f::setZero() {
     setAll(0);
 }
@@ -96,11 +100,7 @@ inline void hkVector4f::mul(hkSimdFloat32Parameter a) {
 }
 
 inline void hkVector4f::setMul(hkVector4fParameter a, hkSimdFloat32Parameter r) {
-#ifdef HK_VECTOR4F_AARCH64_NEON
-    v = vmulq_n_f32(a.v, r);
-#else
-    v *= r.val();
-#endif
+    v = a.v * r.toQuad();
 }
 
 inline void hkVector4f::setMul(hkSimdFloat32Parameter r, hkVector4fParameter a) {
@@ -108,19 +108,11 @@ inline void hkVector4f::setMul(hkSimdFloat32Parameter r, hkVector4fParameter a)
 }
 
 inline void hkVector4f::setAdd(hkVector4fParameter a, hkSimdFloat32Parameter b) {
-#ifdef HK_VECTOR4F_AARCH64_NEON
-    v = vaddq_f32(a.v, vdupq_n_f32(b));
-#else
-    v += b.val();
-#endif
+    v = a.v + b.toQuad();
 }
 
 inline void hkVector4f::setSub(hkVector4fParameter a, hkSimdFloat32Parameter b) {
-#ifdef HK_VECTOR4F_AARCH64_NEON
-    v = vsubq_f32(a.v, vdupq_n_f32(b));
-#else
-    v -= b.val();
-#endif
+    v = a.v - b.toQuad();
 }
 
 inline void hkVector4f::setReciprocal(hkVector4fParameter a) {

lib/hkStubs/Havok/Common/Base/Types/Geometry/Aabb/hkAabb.h

@@ -11,6 +11,19 @@ public:
     hkAabb() {}
     HK_FORCE_INLINE hkAabb(const hkVector4& min, const hkVector4& max) : m_min(min), m_max(max) {}
 
+    HK_FORCE_INLINE void getExtents(hkVector4& e) const;
+    HK_FORCE_INLINE void getCenter(hkVector4& center) const;
+
     hkVector4 m_min;
     hkVector4 m_max;
 };
+
+inline void hkAabb::getExtents(hkVector4& e) const {
+    e.setSub(m_max, m_min);
+}
+
+inline void hkAabb::getCenter(hkVector4& center) const {
+    hkVector4 s;
+    s.setAdd(m_min, m_max);
+    center.setMul(s, hkSimdReal::getConstant<HK_QUADREAL_INV_2>());
+}

src/KingSystem/Physics/RigidBody/physRigidBody.cpp

@@ -1,4 +1,5 @@
 #include "KingSystem/Physics/RigidBody/physRigidBody.h"
+#include <Havok/Common/Base/Types/Geometry/Aabb/hkAabb.h>
 #include <Havok/Physics/Constraint/Data/hkpConstraintData.h>
 #include <Havok/Physics/Constraint/hkpConstraintInstance.h>
 #include <Havok/Physics2012/Dynamics/Collide/hkpResponseModifier.h>
@@ -653,14 +654,14 @@ float RigidBody::getWaterFlowEffectiveRate() const {
 }
 
 void RigidBody::setMagneMassScalingFactor(float factor) {
-    if (!isEntity())
+    if (!isEntity() || !mMotionAccessor)
         return;
     getEntityMotionAccessor()->setMagneMassScalingFactor(factor);
 }
 
 float RigidBody::getMagneMassScalingFactor() const {
-    if (!isEntity())
-        return 0.0;
+    if (!isEntity() || !mMotionAccessor)
+        return -1.0;
     return getEntityMotionAccessor()->getMagneMassScalingFactor();
 }
 
@@ -758,6 +759,18 @@ bool RigidBody::isEntityMotionFlag20Off() const {
     return !getEntityMotionAccessor()->hasFlag(RigidBodyMotionEntity::Flag::_20);
 }
 
+void RigidBody::setEntityMotionFlag80(bool set) {
+    if (!isEntity() || !mMotionAccessor)
+        return;
+    getEntityMotionAccessor()->changeFlag(RigidBodyMotionEntity::Flag::_80, set);
+}
+
+bool RigidBody::isEntityMotionFlag80On() const {
+    if (!isEntity() || !mMotionAccessor)
+        return false;
+    return getEntityMotionAccessor()->hasFlag(RigidBodyMotionEntity::Flag::_80);
+}
+
 void RigidBody::setColImpulseScale(float scale) {
     if (!isEntity())
         return;
@@ -828,6 +841,50 @@ void RigidBody::computeShapeVolumeMassProperties(float* volume, sead::Vector3f*
     }
 }
 
+void RigidBody::clearFlag2000000(bool clear) {
+    if (mFlags.isOff(Flag::_2000000) == clear)
+        return;
+
+    mFlags.change(Flag::_2000000, !clear);
+
+    if (isFlag8Set())
+        setMotionFlag(MotionFlag::_10000);
+    else
+        updateDeactivation();
+}
+
+void RigidBody::clearFlag4000000(bool clear) {
+    if (mFlags.isOff(Flag::_4000000) == clear)
+        return;
+
+    mFlags.change(Flag::_4000000, !clear);
+
+    if (isFlag8Set())
+        setMotionFlag(MotionFlag::_10000);
+    else
+        updateDeactivation();
+}
+
+void RigidBody::clearFlag8000000(bool clear) {
+    if (mFlags.isOff(Flag::_8000000) == clear)
+        return;
+
+    mFlags.change(Flag::_8000000, !clear);
+
+    if (isFlag8Set())
+        setMotionFlag(MotionFlag::_10000);
+    else
+        updateDeactivation();
+}
+
+void* RigidBody::m10() {
+    return nullptr;
+}
+
+void* RigidBody::m11() {
+    return nullptr;
+}
+
 void RigidBody::resetPosition() {
     // debug logging?
     [[maybe_unused]] sead::Vector3f position = getPosition();
@@ -838,6 +895,39 @@ const char* RigidBody::getName() {
     return mUserTag ? mUserTag->getName().cstr() : getHkBodyName().cstr();
 }
 
+void RigidBody::logPosition() const {
+    sead::Vector3f position;
+    getPosition(&position);
+    // debug logging?
+}
+
+static void convertHkAabb(const hkAabb& hk_aabb, sead::BoundBox3f* aabb) {
+    hkVector4f center;
+    hk_aabb.getCenter(center);
+
+    hkVector4f extents;
+    hk_aabb.getExtents(extents);
+    auto half_extents = 0.5f * toVec3(extents);
+
+    aabb->setMin(toVec3(center) - half_extents);
+    aabb->setMax(toVec3(center) + half_extents);
+}
+
+void RigidBody::getAabbInLocal(sead::BoundBox3f* aabb) const {
+    hkAabb hk_aabb;
+    getHkBody()->getCollidable()->getShape()->getAabb(hkTransformf::getIdentity(), 0.0, hk_aabb);
+    convertHkAabb(hk_aabb, aabb);
+}
+
+// NON_MATCHING: paired stores in convertHkAabb that shouldn't be paired
+void RigidBody::getAabbInWorld(sead::BoundBox3f* aabb) const {
+    hkTransformf hk_transform;
+    toHkTransform(&hk_transform, getTransform());
+    hkAabb hk_aabb;
+    getHkBody()->getCollidable()->getShape()->getAabb(hk_transform, 0.0, hk_aabb);
+    convertHkAabb(hk_aabb, aabb);
+}
+
 void RigidBody::lock() {
     mCS.lock();
 }
@@ -862,6 +952,42 @@ hkpMotion* RigidBody::getMotion() const {
     return getHkBody()->getMotion();
 }
 
+void RigidBody::setEntityMotionFlag1(bool set) {
+    if (!isEntity() || !mMotionAccessor)
+        return;
+    getEntityMotionAccessor()->changeFlag(RigidBodyMotionEntity::Flag::_1, set);
+}
+
+bool RigidBody::isEntityMotionFlag1On() const {
+    if (!isEntity() || !mMotionAccessor)
+        return false;
+    return getEntityMotionAccessor()->hasFlag(RigidBodyMotionEntity::Flag::_1);
+}
+
+void RigidBody::setEntityMotionFlag100(bool set) {
+    if (!isEntity() || !mMotionAccessor)
+        return;
+    getEntityMotionAccessor()->changeFlag(RigidBodyMotionEntity::Flag::_100, set);
+}
+
+bool RigidBody::isEntityMotionFlag100On() const {
+    if (!isEntity() || !mMotionAccessor)
+        return false;
+    return getEntityMotionAccessor()->hasFlag(RigidBodyMotionEntity::Flag::_100);
+}
+
+void RigidBody::setEntityMotionFlag200(bool set) {
+    if (!isEntity() || !mMotionAccessor)
+        return;
+    getEntityMotionAccessor()->changeFlag(RigidBodyMotionEntity::Flag::_200, set);
+}
+
+bool RigidBody::isEntityMotionFlag200On() const {
+    if (!isEntity() || !mMotionAccessor)
+        return false;
+    return getEntityMotionAccessor()->hasFlag(RigidBodyMotionEntity::Flag::_200);
+}
+
 void RigidBody::onInvalidParameter(int code) {
     sead::Vector3f pos, lin_vel, ang_vel;
     mRigidBodyAccessor.getPosition(&pos);
@@ -876,4 +1002,13 @@ void RigidBody::notifyUserTag(int code) {
         mUserTag->m7(this, code);
 }
 
+void RigidBody::updateDeactivation() {
+    if (mFlags.isOn(Flag::_2000000) || mFlags.isOn(Flag::_4000000) || mFlags.isOn(Flag::_8000000)) {
+        if (getHkBody()->isDeactivationEnabled())
+            mHkBody->enableDeactivation(false);
+    } else if (!getHkBody()->isDeactivationEnabled()) {
+        mHkBody->enableDeactivation(true);
+    }
+}
+
 }  // namespace ksys::phys

src/KingSystem/Physics/RigidBody/physRigidBody.h

@@ -2,6 +2,7 @@
 
 #include <container/seadPtrArray.h>
 #include <heap/seadDisposer.h>
+#include <math/seadBoundBox.h>
 #include <math/seadMathCalcCommon.h>
 #include <prim/seadRuntimeTypeInfo.h>
 #include <prim/seadTypedBitFlag.h>
@@ -98,21 +99,27 @@ public:
         _80000 = 1 << 19,
     };
 
+    class ScopedLock {
+    public:
+        explicit ScopedLock(RigidBody* body, bool also_lock_world)
+            : mBody(body), mAlsoLockWorld(also_lock_world) {
+            mBody->lock(also_lock_world);
+        }
+        ~ScopedLock() { mBody->unlock(mAlsoLockWorld); }
+        ScopedLock(const ScopedLock&) = delete;
+        auto operator=(const ScopedLock&) = delete;
+
+    private:
+        RigidBody* mBody;
+        bool mAlsoLockWorld;
+    };
+
     RigidBody(Type type, ContactLayerType layer_type, hkpRigidBody* hk_body,
               const sead::SafeString& name, sead::Heap* heap, bool a7);
     ~RigidBody() override;
 
     // FIXME: types and names
     virtual float m4();
-    virtual void m5();
-
-    /// Recalculate inertia, volume and center of mass based on the shape and mass of the rigid body
-    /// and update this rigid body to match the computed values.
-    virtual void resetInertiaAndCenterOfMass();
-
-    /// All three parameters may be null.
-    virtual void computeShapeVolumeMassProperties(float* volume, sead::Vector3f* center_of_mass,
-                                                  sead::Vector3f* inertia_tensor);
 
     bool initMotionAccessorForDynamicMotion(sead::Heap* heap);
     bool initMotionAccessor(const RigidBodyInstanceParam& param, sead::Heap* heap,
@@ -193,6 +200,9 @@ public:
     void setPosition(const sead::Vector3f& position, bool propagate_to_linked_motions);
     void getPosition(sead::Vector3f* position) const;
     sead::Vector3f getPosition() const;
+    virtual void logPosition() const;
+    void getAabbInLocal(sead::BoundBox3f* aabb) const;
+    void getAabbInWorld(sead::BoundBox3f* aabb) const;
 
     void getRotation(sead::Quatf* rotation) const;
     sead::Quatf getRotation() const;
@@ -243,6 +253,14 @@ public:
     void getInertiaLocal(sead::Vector3f* inertia) const;
     sead::Vector3f getInertiaLocal() const;
 
+    /// Recalculate inertia, volume and center of mass based on the shape and mass of the rigid body
+    /// and update this rigid body to match the computed values.
+    virtual void resetInertiaAndCenterOfMass();
+
+    /// All three parameters may be null.
+    virtual void computeShapeVolumeMassProperties(float* volume, sead::Vector3f* center_of_mass,
+                                                  sead::Vector3f* inertia_tensor);
+
     void setLinearDamping(float value);
     float getLinearDamping() const;
 
@@ -289,6 +307,9 @@ public:
     void clearEntityMotionFlag20(bool clear);
     bool isEntityMotionFlag20Off() const;
 
+    void setEntityMotionFlag80(bool set);
+    bool isEntityMotionFlag80On() const;
+
     bool isSensor() const { return mFlags.isOn(Flag::IsSensor); }
     bool isEntity() const { return !mFlags.isOn(Flag::IsSensor); }
     ContactLayerType getLayerType() const {
@@ -316,36 +337,33 @@ public:
     void setEntityMotionFlag40(bool set);
     bool isEntityMotionFlag40On() const;
 
+    void clearFlag2000000(bool clear);
+    void clearFlag4000000(bool clear);
+    void clearFlag8000000(bool clear);
+
     void lock();
     void lock(bool also_lock_world);
     void unlock();
     void unlock(bool also_unlock_world);
-
-    hkpMotion* getMotion() const;
-
-    class ScopedLock {
-    public:
-        explicit ScopedLock(RigidBody* body, bool also_lock_world)
-            : mBody(body), mAlsoLockWorld(also_lock_world) {
-            mBody->lock(also_lock_world);
-        }
-        ~ScopedLock() { mBody->unlock(mAlsoLockWorld); }
-        ScopedLock(const ScopedLock&) = delete;
-        auto operator=(const ScopedLock&) = delete;
-
-    private:
-        RigidBody* mBody;
-        bool mAlsoLockWorld;
-    };
-
     [[nodiscard]] auto makeScopedLock(bool also_lock_world) {
         return ScopedLock(this, also_lock_world);
     }
 
+    hkpMotion* getMotion() const;
+
+    void setEntityMotionFlag1(bool set);
+    bool isEntityMotionFlag1On() const;
+
+    void setEntityMotionFlag100(bool set);
+    bool isEntityMotionFlag100On() const;
+
+    void setEntityMotionFlag200(bool set);
+    bool isEntityMotionFlag200On() const;
+
     // FIXME: should be pure
     virtual void m9();
-    virtual void m10();
-    virtual void m11();
+    virtual void* m10();
+    virtual void* m11();
     virtual float m12(float x, float y);
     virtual void resetPosition();
     virtual const char* getName();
@@ -354,6 +372,7 @@ private:
     void createMotionAccessor(sead::Heap* heap);
     void onInvalidParameter(int code = 0);
     void notifyUserTag(int code);
+    void updateDeactivation();
 
     sead::CriticalSection mCS;
     sead::TypedBitFlag<Flag, sead::Atomic<u32>> mFlags{};