ksys/phys: Finish RigidBodyMotion

data/uking_functions.csv

@@ -83385,7 +83385,7 @@ Address,Quality,Size,Name
 0x0000007100fa2a7c,O,000168,_ZN4ksys4phys15RigidBodyMotion7setMassEf
 0x0000007100fa2b24,O,000028,_ZNK4ksys4phys15RigidBodyMotion7getMassEv
 0x0000007100fa2b40,O,000040,_ZNK4ksys4phys15RigidBodyMotion10getMassInvEv
-0x0000007100fa2b68,U,000776,phys::RigidBodyMotion::setInertiaLocal
+0x0000007100fa2b68,O,000776,_ZN4ksys4phys15RigidBodyMotion15setInertiaLocalERKN4sead7Vector3IfEE
 0x0000007100fa2e70,O,000040,_ZNK4ksys4phys15RigidBodyMotion16getGravityFactorEv
 0x0000007100fa2e98,O,000420,_ZN4ksys4phys15RigidBodyMotion32updateRigidBodyMotionExceptStateEv
 0x0000007100fa303c,O,000112,_ZNK4ksys4phys15RigidBodyMotion15getInertiaLocalEPN4sead7Vector3IfEE

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

@@ -29,6 +29,7 @@ public:
 
     HK_FORCE_INLINE void set(hkFloat32 x, hkFloat32 y, hkFloat32 z, hkFloat32 w = 0);
     HK_FORCE_INLINE void setAll(hkFloat32 x);
+    HK_FORCE_INLINE void setZero();
 
     // ========== Vector operations
 

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

@@ -32,6 +32,10 @@ inline void hkVector4f::setAll(hkReal x) {
     v = {x, x, x, x};
 }
 
+inline void hkVector4f::setZero() {
+    setAll(0);
+}
+
 inline void hkVector4f::add(hkVector4fParameter a) {
     setAdd(*this, a);
 }

src/KingSystem/Physics/RigidBody/physRigidBodyMotion.cpp

@@ -1,11 +1,13 @@
 #include "KingSystem/Physics/RigidBody/physRigidBodyMotion.h"
 #include <Havok/Physics2012/Dynamics/Entity/hkpRigidBody.h>
+#include <Havok/Physics2012/Dynamics/Motion/Rigid/hkpBoxMotion.h>
 #include <Havok/Physics2012/Dynamics/Motion/Rigid/hkpKeyframedRigidMotion.h>
 #include <Havok/Physics2012/Dynamics/Motion/hkpMotion.h>
 #include <basis/seadTypes.h>
 #include <cstring>
 #include <prim/seadSafeString.h>
 #include <prim/seadScopedLock.h>
+#include "Havok/Physics2012/Dynamics/Motion/Rigid/hkpSphereMotion.h"
 #include "KingSystem/Physics/RigidBody/physRigidBodyMotionProxy.h"
 #include "KingSystem/Physics/physConversions.h"
 #include "KingSystem/Utils/Debug.h"
@@ -82,14 +84,21 @@ void RigidBodyMotion::setPosition(const sead::Vector3f& position,
 }
 
 void RigidBodyMotion::getPosition(sead::Vector3f* position) {
+    storeToVec3(position, getPosition());
+}
+
+hkVector4f RigidBodyMotion::getPosition() const {
     auto* motion = getHkBodyMotionOrLocalMotionIf(RigidBody::MotionFlag::DirtyTransform);
-    const auto hk_position = motion->getPosition();
-    storeToVec3(position, hk_position);
+    return motion->getPosition();
 }
 
 void RigidBodyMotion::getRotation(sead::Quatf* rotation) {
+    toQuat(rotation, getRotation());
+}
+
+hkQuaternionf RigidBodyMotion::getRotation() const {
     auto* motion = getHkBodyMotionOrLocalMotionIf(RigidBody::MotionFlag::DirtyTransform);
-    toQuat(rotation, motion->getRotation());
+    return motion->getRotation();
 }
 
 void RigidBodyMotion::getTransform(sead::Matrix34f* mtx) {
@@ -292,6 +301,87 @@ float RigidBodyMotion::getMassInv() const {
     return mMotion->getMassInv();
 }
 
+static inline float max3(float a, float b, float c) {
+    return sead::Mathf::max(c, a > b ? a : b);
+}
+
+static inline float min3(float a, float b, float c) {
+    return sead::Mathf::min(a < b ? a : b, c);
+}
+
+void RigidBodyMotion::setInertiaLocal(const sead::Vector3f& inertia) {
+    if (mBody->isCharacterControllerType())
+        return;
+
+    if (arePropertyChangesBlocked()) {
+        mInertiaLocal = inertia;
+        return;
+    }
+
+    const float max = max3(inertia.x, inertia.y, inertia.z);
+    const float min = min3(inertia.x, inertia.y, inertia.z);
+    const float threshold = max * 0.8f;
+
+    bool need_to_recreate_motion = false;
+    switch (mMotion->getType()) {
+    case hkpMotion::MOTION_BOX_INERTIA:
+        need_to_recreate_motion = min > threshold;
+        break;
+    case hkpMotion::MOTION_SPHERE_INERTIA:
+        need_to_recreate_motion = min > threshold;
+        // The condition is inverted for spheres.
+        need_to_recreate_motion ^= true;
+        break;
+    default:
+        break;
+    }
+
+    if (need_to_recreate_motion) {
+        const float mass = getMass();
+        const auto position = getPosition();
+        const auto rotation = getRotation();
+        const auto gravity_factor = getGravityFactor();
+
+        // Recreate the Havok motion.
+        if (min > threshold) {
+            hkpSphereMotion tmp_motion(position, rotation);
+            mMotion->getMotionStateAndVelocitiesAndDeactivationType(&tmp_motion);
+            new (mMotion) hkpSphereMotion(position, rotation);
+            tmp_motion.getMotionStateAndVelocitiesAndDeactivationType(mMotion);
+        } else {
+            // This little trick lets us copy the motion state and various other state
+            // out of the existing Havok motion so we can recreate it safely.
+            hkpBoxMotion tmp_motion(position, rotation);
+            mMotion->getMotionStateAndVelocitiesAndDeactivationType(&tmp_motion);
+            new (mMotion) hkpBoxMotion(position, rotation);
+            tmp_motion.getMotionStateAndVelocitiesAndDeactivationType(mMotion);
+        }
+
+        // Some properties are not automatically transferred over. Copy them manually.
+        mMotion->setGravityFactor(gravity_factor);
+        mMotion->setMass(mass);
+        if (mBody->isFlag8Set())
+            setMotionFlag(RigidBody::MotionFlag::DirtyMiscState);
+        else if (mBody->getMotionType() == MotionType::Dynamic)
+            updateRigidBodyMotionExceptState();
+    }
+
+    hkMatrix3f hk_inertia;
+    hk_inertia.m_col0.set(inertia.x, 0, 0);
+    hk_inertia.m_col1.set(0, inertia.y, 0);
+    hk_inertia.m_col2.set(0, 0, inertia.z);
+    mMotion->setInertiaLocal(hk_inertia);
+
+    if (mBody->isFlag8Set()) {
+        setMotionFlag(RigidBody::MotionFlag::DirtyInertiaLocal);
+    } else if (mBody->getMotionType() == MotionType::Dynamic &&
+               !mBody->isCharacterControllerType()) {
+        hkMatrix3f inertia_inv;
+        mMotion->getInertiaInvLocal(inertia_inv);
+        getHkBody()->getMotion()->setInertiaInvLocal(inertia_inv);
+    }
+}
+
 void RigidBodyMotion::getInertiaLocal(sead::Vector3f* inertia) const {
     if (arePropertyChangesBlocked()) {
         inertia->e = mInertiaLocal.e;

src/KingSystem/Physics/RigidBody/physRigidBodyMotion.h

@@ -27,7 +27,9 @@ public:
     void setTransform(const sead::Matrix34f& mtx, bool propagate_to_linked_motions) override;
     void setPosition(const sead::Vector3f& position, bool propagate_to_linked_motions) override;
     void getPosition(sead::Vector3f* position) override;
+    hkVector4f getPosition() const;
     void getRotation(sead::Quatf* rotation) override;
+    hkQuaternionf getRotation() const;
     void getTransform(sead::Matrix34f* mtx) override;
 
     void setCenterOfMassInLocal(const sead::Vector3f& center) override;
@@ -66,8 +68,7 @@ public:
     float getMass() const;
     float getMassInv() const;
 
-    // 0x0000007100fa2b68
-    void setInertiaLocal(const sead::Vector3f&);
+    void setInertiaLocal(const sead::Vector3f& inertia);
     void getInertiaLocal(sead::Vector3f* inertia) const;
 
     void setLinearDamping(float value);