ksys/phys: Add more RigidBody functions

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

2 files changed, 184 insertions, 30 deletions

diff --git a/src/KingSystem/Physics/RigidBody/physRigidBody.cpp b/src/KingSystem/Physics/RigidBody/physRigidBody.cpp
index 4aa618ee..bb2b8c8c 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBody.cpp
+++ b/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
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBody.h b/src/KingSystem/Physics/RigidBody/physRigidBody.h
index a308227d..2a0d07a9 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBody.h
+++ b/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);
+    [[nodiscard]] auto makeScopedLock(bool also_lock_world) {
+        return ScopedLock(this, also_lock_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;
+    void setEntityMotionFlag1(bool set);
+    bool isEntityMotionFlag1On() const;
 
-    private:
-        RigidBody* mBody;
-        bool mAlsoLockWorld;
-    };
+    void setEntityMotionFlag100(bool set);
+    bool isEntityMotionFlag100On() const;
 
-    [[nodiscard]] auto makeScopedLock(bool also_lock_world) {
-        return ScopedLock(this, also_lock_world);
-    }
+    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{};