ksys/phys: Add more RigidBody functions

6 files changed, 230 insertions, 8 deletions

diff --git a/src/KingSystem/Physics/RigidBody/physMotionAccessor.h b/src/KingSystem/Physics/RigidBody/physMotionAccessor.h
index d5b82caa..51f7d4d0 100644
--- a/src/KingSystem/Physics/RigidBody/physMotionAccessor.h
+++ b/src/KingSystem/Physics/RigidBody/physMotionAccessor.h
@@ -61,6 +61,8 @@ public:
     hkpRigidBody* getHkBody() const { return mBody->getHkBody(); }
     u32 get10() const { return _10; }
     u32 get14() const { return _14; }
+    void increment10() { ++_10; }
+    void increment14() { ++_14; }
 
 protected:
     RigidBody* mBody = nullptr;
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBody.cpp b/src/KingSystem/Physics/RigidBody/physRigidBody.cpp
index cf97d063..3db6c834 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBody.cpp
+++ b/src/KingSystem/Physics/RigidBody/physRigidBody.cpp
@@ -35,6 +35,14 @@ static bool isVectorInvalid(const sead::Vector3f& vec) {
     return false;
 }
 
+static bool isMatrixInvalid(const sead::Matrix34f& matrix) {
+    for (float x : matrix.a) {
+        if (std::isnan(x))
+            return true;
+    }
+    return false;
+}
+
 RigidBody::RigidBody(Type type, ContactLayerType layer_type, hkpRigidBody* hk_body,
                      const sead::SafeString& name, sead::Heap* heap, bool a7)
     : mCS(heap), mHkBody(hk_body), mRigidBodyAccessor(hk_body), mType(type) {
@@ -884,6 +892,153 @@ sead::Matrix34f RigidBody::getTransform() const {
     return transform;
 }
 
+void RigidBody::setTransform(const sead::Matrix34f& mtx, bool propagate_to_linked_motions) {
+    if (isMatrixInvalid(mtx)) {
+        onInvalidParameter();
+        return;
+    }
+
+    mMotionAccessor->setTransform(mtx, propagate_to_linked_motions);
+}
+
+bool RigidBody::isTransformDirty() const {
+    return mMotionFlags.isOn(MotionFlag::DirtyTransform);
+}
+
+void RigidBody::updateShape() {
+    if (isFlag8Set()) {
+        setMotionFlag(MotionFlag::DirtyShape);
+        return;
+    }
+
+    auto* shape = getNewShape();
+    if (shape) {
+        mHkBody->setShape(shape);
+        if (isEntity() && mMotionAccessor)
+            mMotionAccessor->increment14();
+    } else {
+        mHkBody->updateShape();
+        if (isEntity() && mMotionAccessor)
+            mMotionAccessor->increment10();
+    }
+
+    if (mUserTag)
+        mUserTag->onBodyShapeChanged(this);
+}
+
+void RigidBody::updateShapeIfNeeded(float x) {
+    if (!hasFlag(Flag::_10))
+        return;
+
+    if (x <= 0.0)
+        x = 1.0;
+
+    if (sead::Mathf::equalsEpsilon(_b0, x))
+        return;
+
+    _b0 = m12(x, _b0);
+    updateShape();
+}
+
+void RigidBody::changeMotionType(MotionType motion_type) {
+    if (getMotionType() == motion_type)
+        return;
+
+    if (isFlag8Set()) {
+        switch (motion_type) {
+        case MotionType::Dynamic:
+            if (isEntity()) {
+                setMotionFlag(MotionFlag::Dynamic);
+                mMotionFlags.reset(MotionFlag::Fixed);
+                mMotionFlags.reset(MotionFlag::Keyframed);
+            }
+            break;
+        case MotionType::Fixed:
+            setMotionFlag(MotionFlag::Fixed);
+            mMotionFlags.reset(MotionFlag::Dynamic);
+            mMotionFlags.reset(MotionFlag::Keyframed);
+            break;
+        case MotionType::Keyframed:
+            setMotionFlag(MotionFlag::Keyframed);
+            mMotionFlags.reset(MotionFlag::Dynamic);
+            mMotionFlags.reset(MotionFlag::Fixed);
+            break;
+        case MotionType::Unknown:
+        case MotionType::Invalid:
+            break;
+        }
+        return;
+    }
+
+    switch (motion_type) {
+    case MotionType::Dynamic:
+        if (!isEntity())
+            return;
+        mMotionFlags.set(MotionFlag::Dynamic);
+        break;
+    case MotionType::Fixed:
+        mMotionFlags.set(MotionFlag::Fixed);
+        break;
+    case MotionType::Keyframed:
+        mMotionFlags.set(MotionFlag::Keyframed);
+        break;
+    case MotionType::Unknown:
+    case MotionType::Invalid:
+        break;
+    }
+
+    doChangeMotionType(motion_type, getMotionType());
+    mMotionFlags.set(MotionFlag::DirtyMass);
+    mMotionFlags.set(MotionFlag::DirtyInertiaLocal);
+    mMotionFlags.set(MotionFlag::DirtyMaxVelOrTimeFactor);
+    mMotionFlags.set(MotionFlag::DirtyDampingOrGravityFactor);
+    mMotionFlags.set(MotionFlag::DirtyCenterOfMassLocal);
+    x_40();
+}
+
+void RigidBody::updateMotionTypeRelatedFlags() {
+    if (hasFlag(Flag::_20000000) || hasFlag(Flag::_80000000) || hasFlag(Flag::_40000000))
+        return;
+
+    switch (getMotionType()) {
+    case MotionType::Dynamic:
+        mFlags.set(Flag::_80000000);
+        mFlags.reset(Flag::_20000000);
+        mFlags.reset(Flag::_40000000);
+        return;
+    case MotionType::Fixed:
+        mFlags.set(Flag::_40000000);
+        mFlags.reset(Flag::_20000000);
+        mFlags.reset(Flag::_80000000);
+        return;
+    case MotionType::Keyframed:
+        mFlags.set(Flag::_20000000);
+        mFlags.reset(Flag::_40000000);
+        mFlags.reset(Flag::_80000000);
+        return;
+    case MotionType::Unknown:
+    case MotionType::Invalid:
+        break;
+    }
+
+    mFlags.reset(Flag::_20000000);
+    mFlags.reset(Flag::_40000000);
+    mFlags.reset(Flag::_80000000);
+}
+
+void RigidBody::triggerScheduledMotionTypeChange() {
+    if (hasFlag(Flag::_20000000)) {
+        changeMotionType(MotionType::Keyframed);
+        mFlags.reset(Flag::_20000000);
+    } else if (hasFlag(Flag::_40000000)) {
+        changeMotionType(MotionType::Fixed);
+        mFlags.reset(Flag::_40000000);
+    } else if (hasFlag(Flag::_80000000)) {
+        changeMotionType(MotionType::Dynamic);
+        mFlags.reset(Flag::_80000000);
+    }
+}
+
 bool RigidBody::setLinearVelocity(const sead::Vector3f& velocity, float epsilon) {
     if (isVectorInvalid(velocity)) {
         onInvalidParameter();
@@ -939,6 +1094,43 @@ void RigidBody::getPointVelocity(sead::Vector3f* velocity, const sead::Vector3f&
     velocity->add(getLinearVelocity());
 }
 
+void RigidBody::computeVelocityForWarping(sead::Vector3f* linear_velocity,
+                                          const sead::Vector3f& target_position,
+                                          bool take_angular_velocity_into_account) {
+    const float factor = getVelocityComputeTimeFactor();
+    const auto hk_target_pos = toHkVec4(target_position);
+    auto hk_current_pos = toHkVec4(getPosition());
+
+    if (take_angular_velocity_into_account) {
+        const auto center = getCenterOfMassInLocal();
+        if (center.x == 0 && center.y == 0 && center.z == 0) {
+            hkVector4f rel_pos;
+            rel_pos.setSub(hk_current_pos, toHkVec4(getCenterOfMassInWorld()));
+
+            hkVector4f correction;
+            correction.setCross(toHkVec4(getAngularVelocity()), rel_pos);
+            correction.mul(1.0f / factor);
+            hk_current_pos.add(correction);
+        }
+    }
+
+    hkVector4f result;
+    result.setSub(hk_target_pos, hk_current_pos);
+    result.mul(factor);
+    storeToVec3(linear_velocity, result);
+}
+
+void RigidBody::computeVelocities(hkVector4f* linear_velocity, hkVector4f* angular_velocity,
+                                  const hkVector4f& position, const hkQuaternionf& rotation) {
+    const float factor = getVelocityComputeTimeFactor();
+    computeVelocities(linear_velocity, angular_velocity, position, rotation, factor);
+}
+
+float RigidBody::getVelocityComputeTimeFactor() const {
+    const float time_factor = getTimeFactor();
+    return time_factor == 0 ? 0 : (1.f / (time_factor * System::instance()->get64()));
+}
+
 void RigidBody::setCenterOfMassInLocal(const sead::Vector3f& center) {
     sead::Vector3f current_center;
     mMotionAccessor->getCenterOfMassInLocal(&current_center);
@@ -1403,7 +1595,7 @@ void RigidBody::clearFlag8000000(bool clear) {
         updateDeactivation();
 }
 
-void* RigidBody::m10() {
+const hkpShape* RigidBody::getNewShape() {
     return nullptr;
 }
 
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBody.h b/src/KingSystem/Physics/RigidBody/physRigidBody.h
index db828d9f..de8101e8 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBody.h
+++ b/src/KingSystem/Physics/RigidBody/physRigidBody.h
@@ -18,6 +18,7 @@ class hkQuaternionf;
 class hkVector4f;
 class hkpCollidable;
 class hkpRigidBody;
+class hkpShape;
 class hkpMaxSizeMotion;
 class hkpMotion;
 
@@ -79,6 +80,10 @@ public:
         _2000000 = 1 << 25,
         _4000000 = 1 << 26,
         _8000000 = 1 << 27,
+        _10000000 = 1 << 28,
+        _20000000 = 1 << 29,
+        _40000000 = 1 << 30,
+        _80000000 = 1 << 31,
     };
 
     enum class MotionFlag {
@@ -96,7 +101,7 @@ public:
         DirtyCenterOfMassLocal = 1 << 11,
         DirtyInertiaLocal = 1 << 12,
         DirtyDampingOrGravityFactor = 1 << 13,
-        _4000 = 1 << 14,
+        DirtyShape = 1 << 14,
         _8000 = 1 << 15,
         _10000 = 1 << 16,
         _20000 = 1 << 17,
@@ -266,8 +271,19 @@ public:
 
     void getTransform(sead::Matrix34f* mtx) const;
     sead::Matrix34f getTransform() const;
-    // 0x0000007100f8fb08
     void setTransform(const sead::Matrix34f& mtx, bool propagate_to_linked_motions);
+    bool isTransformDirty() const;
+
+    void updateShape();
+    void updateShapeIfNeeded(float x);
+
+    void changeMotionType(MotionType motion_type);
+    // 0x0000007100f9045c - calls a bunch of Havok world functions
+    void doChangeMotionType(MotionType x, MotionType y);
+    // 0x0000007100f908c8
+    void x_40();
+    void updateMotionTypeRelatedFlags();
+    void triggerScheduledMotionTypeChange();
 
     bool setLinearVelocity(const sead::Vector3f& velocity, float epsilon = sead::Mathf::epsilon());
     void getLinearVelocity(sead::Vector3f* velocity) const;
@@ -279,9 +295,16 @@ public:
 
     void getPointVelocity(sead::Vector3f* velocity, const sead::Vector3f& point) const;
 
-    // 0x0000007100f92b74
+    /// Compute the linear velocity that would be necessary to instantly warp to the target.
+    void computeVelocityForWarping(sead::Vector3f* linear_velocity,
+                                   const sead::Vector3f& target_position,
+                                   bool take_angular_velocity_into_account);
     void computeVelocities(hkVector4f* linear_velocity, hkVector4f* angular_velocity,
                            const hkVector4f& position, const hkQuaternionf& rotation);
+    // 0x0000007100f91780
+    void computeVelocities(hkVector4f* linear_velocity, hkVector4f* angular_velocity,
+                           const hkVector4f& position, const hkQuaternionf& rotation, float factor);
+    float getVelocityComputeTimeFactor() const;
 
     void setCenterOfMassInLocal(const sead::Vector3f& center);
     void getCenterOfMassInLocal(sead::Vector3f* center) const;
@@ -416,7 +439,7 @@ public:
     bool isEntityMotionFlag200On() const;
 
     virtual void m9() = 0;
-    virtual void* m10();
+    virtual const hkpShape* getNewShape();
     virtual void* m11();
     virtual float m12(float x, float y);
     virtual void resetPosition();
diff --git a/src/KingSystem/Physics/System/physSystem.h b/src/KingSystem/Physics/System/physSystem.h
index 783e7643..efdc0d7a 100644
--- a/src/KingSystem/Physics/System/physSystem.h
+++ b/src/KingSystem/Physics/System/physSystem.h
@@ -30,6 +30,7 @@ class System {
     virtual ~System();
 
 public:
+    float get64() const { return _64; }
     float getTimeFactor() const { return mTimeFactor; }
     GroupFilter* getGroupFilter(ContactLayerType type) const;
     ContactMgr* getContactMgr() const { return mContactMgr; }
@@ -61,7 +62,11 @@ public:
     void unlockWorld(ContactLayerType type, void* a = nullptr, int b = 0, bool c = false);
 
 private:
-    u8 _28[0x74 - 0x28];
+    u8 _28[0x64 - 0x28];
+    float _64 = 1.0 / 30.0;
+    float _68 = 1.0 / 30.0;
+    float _6c = 1.0;
+    float _70 = 1.0 / 30.0;
     float mTimeFactor{};
     u8 _78[0xa8 - 0x78];
     sead::CriticalSection mCS;
diff --git a/src/KingSystem/Physics/System/physUserTag.cpp b/src/KingSystem/Physics/System/physUserTag.cpp
index 8a2fe69d..03f0fcb8 100644
--- a/src/KingSystem/Physics/System/physUserTag.cpp
+++ b/src/KingSystem/Physics/System/physUserTag.cpp
@@ -10,7 +10,7 @@ void UserTag::m3(void* a, void* b, float c) {
     // FIXME
 }
 
-void UserTag::m4() {}
+void UserTag::onBodyShapeChanged(RigidBody* body) {}
 
 void UserTag::m5() {}
 
diff --git a/src/KingSystem/Physics/System/physUserTag.h b/src/KingSystem/Physics/System/physUserTag.h
index 1d2ea480..5942ee45 100644
--- a/src/KingSystem/Physics/System/physUserTag.h
+++ b/src/KingSystem/Physics/System/physUserTag.h
@@ -17,7 +17,7 @@ public:
     virtual void m2(void* a);
     // a and b are probably physics bodies?
     virtual void m3(void* a, void* b, float c);
-    virtual void m4();
+    virtual void onBodyShapeChanged(RigidBody* body);
     virtual void m5();
     virtual const sead::SafeString& getName() const { return sead::SafeString::cEmptyString; }
     virtual void m7(RigidBody* rigid_body, int a);