ksys/phys: Finish StaticCompoundRigidBodyGroup

3 files changed, 228 insertions, 17 deletions

diff --git a/src/KingSystem/Physics/StaticCompound/physStaticCompoundRigidBodyGroup.cpp b/src/KingSystem/Physics/StaticCompound/physStaticCompoundRigidBodyGroup.cpp
index e16101f8..2485dbe5 100644
--- a/src/KingSystem/Physics/StaticCompound/physStaticCompoundRigidBodyGroup.cpp
+++ b/src/KingSystem/Physics/StaticCompound/physStaticCompoundRigidBodyGroup.cpp
@@ -8,12 +8,24 @@
 #include "KingSystem/Physics/StaticCompound/physStaticCompoundInfo.h"
 #include "KingSystem/Physics/StaticCompound/physStaticCompoundMgr.h"
 #include "KingSystem/Physics/System/physSystem.h"
+#include "KingSystem/Utils/MathUtil.h"
 
 namespace ksys::phys {
 
 // TODO: rename
 constexpr int BodyGroupNumMatrices = 8;
 
+static StaticCompoundRigidBodyGroup::Config sScRigidBodyGroupConfig;
+static StaticCompoundRigidBodyGroup::Epsilons sScRigidBodyGroupEpsilons;
+
+StaticCompoundRigidBodyGroup::Config& StaticCompoundRigidBodyGroup::getConfig() {
+    return sScRigidBodyGroupConfig;
+}
+
+StaticCompoundRigidBodyGroup::Epsilons& StaticCompoundRigidBodyGroup::getEpsilons() {
+    return sScRigidBodyGroupEpsilons;
+}
+
 StaticCompoundRigidBodyGroup::StaticCompoundRigidBodyGroup() = default;
 
 StaticCompoundRigidBodyGroup::~StaticCompoundRigidBodyGroup() {
@@ -86,10 +98,7 @@ void StaticCompoundRigidBodyGroup::init(const hkpPhysicsSystem& system, sead::Ma
 
     mMatrices2.allocBufferAssert(BodyGroupNumMatrices, heap);
     mMatrices.allocBufferAssert(BodyGroupNumMatrices, heap);
-    for (int i = 0, n = mMatrices2.size(); i < n; ++i) {
-        mMatrices2[i].makeIdentity();
-        mMatrices[i].makeIdentity();
-    }
+    resetMatrices();
 
     mFlags.set(Flag::Initialised);
 }
@@ -116,7 +125,7 @@ void StaticCompoundRigidBodyGroup::addToWorld() {
 
         auto lock = body->makeScopedLock();
 
-        body->setTransform(getMatrix(), true);
+        body->setTransform(getTransform(), true);
 
         if (body->getMotionFlags().isOn(RigidBody::MotionFlag::BodyRemovalRequested)) {
             body->resetMotionFlagDirect(RigidBody::MotionFlag::BodyRemovalRequested);
@@ -163,13 +172,172 @@ void StaticCompoundRigidBodyGroup::enableAllInstancesAndShapeKeys() {
     }
 }
 
-void StaticCompoundRigidBodyGroup::modifyMatrix(const sead::Matrix34f& matrix, int index) {
+void StaticCompoundRigidBodyGroup::resetMatrices() {
+    for (int i = 0, n = mMatrices2.size(); i < n; ++i) {
+        mMatrices2[i].makeIdentity();
+        mMatrices[i].makeIdentity();
+    }
+}
+
+void StaticCompoundRigidBodyGroup::resetMatricesAndUpdateTransform() {
+    resetMatrices();
+    mModifiedMatrices = 0;
+
+    ScopedWorldLock lock_entity{ContactLayerType::Entity};
+    ScopedWorldLock lock_sensor{ContactLayerType::Sensor};
+    restoreMatricesAndUpdateTransform();
+}
+
+void StaticCompoundRigidBodyGroup::restoreMatricesAndUpdateTransform() {
+    mFlags.set(Flag::_2);
+    mFlags.set(Flag::_4);
+
+    if (mModifiedMatrices != 0) {
+        for (int i = 0, n = mMatrices.size(); i < n; ++i) {
+            if ((1 << i) & mModifiedMatrices) {
+                auto& dest = mMatrices2[i];
+                dest = mMatrices[i];
+            }
+        }
+        mModifiedMatrices = 0;
+    }
+
+    for (int i = 0, n = mRigidBodies.size(); i < n; ++i) {
+        mRigidBodies[i]->setTransform(getTransform(), true);
+    }
+}
+
+void StaticCompoundRigidBodyGroup::restoreMatrices() {
+    if (mModifiedMatrices != 0) {
+        for (int i = 0, n = mMatrices.size(); i < n; ++i) {
+            if ((1 << i) & mModifiedMatrices) {
+                auto& dest = mMatrices2[i];
+                dest = mMatrices[i];
+            }
+        }
+        mModifiedMatrices = 0;
+        mFlags.set(Flag::_2);
+        mFlags.set(Flag::_4);
+    }
+    mTransform = getTransform();
+}
+
+void StaticCompoundRigidBodyGroup::setMatrix(const sead::Matrix34f& matrix, int index) {
     if (mMatrices[index] == matrix)
         return;
 
     mMatrices[index] = matrix;
     mModifiedMatrices |= 1 << index;
-    mFlags.set(Flag::HasModifiedMatrix);
+    mFlags.set(Flag::ShouldMoveBody);
+}
+
+const sead::Matrix34f& StaticCompoundRigidBodyGroup::getMatrix(int index) const {
+    return mMatrices[index];
+}
+
+sead::Matrix34f
+StaticCompoundRigidBodyGroup::getTransformedMatrix(const sead::Matrix34f& mtx) const {
+    return getTransform() * mtx;
+}
+
+sead::Matrix34f
+StaticCompoundRigidBodyGroup::getInvTransformedMatrix(const sead::Matrix34f& mtx) const {
+    sead::Matrix34f inv_transform;
+    inv_transform.setInverse(getTransform());
+    return inv_transform * mtx;
+}
+
+sead::Vector3f StaticCompoundRigidBodyGroup::getTransformedPos(const sead::Vector3f& pos) const {
+    return getTransform() * pos;
+}
+
+sead::Vector3f StaticCompoundRigidBodyGroup::getRotatedDir(const sead::Vector3f& dir) const {
+    sead::Vector3f rotated;
+    rotated.setRotated(getTransform(), dir);
+    return rotated;
+}
+
+void StaticCompoundRigidBodyGroup::processUpdates() {
+    if (mFlags.isOn(Flag::HasEnabledOrDisabledInstance)) {
+        for (int i = 0, n = mRigidBodies.size(); i < n; ++i) {
+            mRigidBodies[i]->updateShape();
+            mFlags.reset(Flag::HasEnabledOrDisabledInstance);
+        }
+    }
+
+    if (mFlags.isOn(Flag::ShouldMoveBody) || mFlags.isOn(Flag::IsMovingBody)) {
+        bool initialised_velocities = false;
+        sead::Vector3f linvel, angvel;
+
+        for (int i = 0, n = mRigidBodies.size(); i < n; ++i) {
+            auto* body = mRigidBodies[i];
+
+            body->changeMotionType(MotionType::Keyframed);
+
+            if (!initialised_velocities) {
+                body->computeVelocities(&linvel, &angvel, mTransform);
+
+                if (mFlags.isOff(Flag::ShouldMoveBody)) {
+                    linvel *= getVelocityMultiplier();
+                    angvel *= getVelocityMultiplier();
+                }
+
+                util::lerp(&linvel, mLinearVelocity, linvel, getConfig().unk2);
+                util::lerp(&angvel, mAngularVelocity, angvel, getConfig().unk3);
+            }
+
+            body->setLinearVelocity(linvel, getEpsilons().linvel);
+            body->setAngularVelocity(angvel, getEpsilons().angvel);
+            initialised_velocities = true;
+        }
+
+        mLinearVelocity = linvel;
+        mAngularVelocity = angvel;
+
+        if (mFlags.isOn(Flag::ShouldMoveBody)) {
+            mUpdateTimer = getConfig().move_duration_ticks;
+            mFlags.set(Flag::IsMovingBody);
+        } else if (mUpdateTimer-- > 0) {
+            mFlags.set(Flag::IsMovingBody);
+        } else {
+            mFlags.reset(Flag::IsMovingBody);
+        }
+
+        mFlags.reset(Flag::ShouldMoveBody);
+
+    } else {
+        for (int i = 0, n = mRigidBodies.size(); i < n; ++i) {
+            auto* body = mRigidBodies[i];
+            body->setLinearVelocity(sead::Vector3f::zero, getEpsilons().linvel);
+            body->setAngularVelocity(sead::Vector3f::zero, getEpsilons().angvel);
+            mLinearVelocity = {0, 0, 0};
+            mAngularVelocity = {0, 0, 0};
+        }
+    }
+}
+
+float StaticCompoundRigidBodyGroup::getVelocityMultiplier() const {
+    return getConfig().unk1 * float(mUpdateTimer) / float(getConfig().move_duration_ticks);
+}
+
+const sead::Matrix34f& StaticCompoundRigidBodyGroup::getTransform() const {
+    if (!mFlags.isOn(Flag::Initialised))
+        return sead::Matrix34f::ident;
+
+    if (mFlags.isOn(Flag::_2)) {
+        mMtx0 = mMatrices2[0];
+        for (int i = 1, n = mMatrices2.size(); i < n; ++i)
+            mMtx0 = mMatrices2[i] * mMtx0;
+
+        mFlags.reset(Flag::_2);
+    }
+
+    if (mFlags.isOn(Flag::_4) && mMtxPtr) {
+        mTransform = *mMtxPtr * mMtx0;
+        mFlags.reset(Flag::_4);
+    }
+
+    return mTransform;
 }
 
 }  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/StaticCompound/physStaticCompoundRigidBodyGroup.h b/src/KingSystem/Physics/StaticCompound/physStaticCompoundRigidBodyGroup.h
index 308c8516..94db5519 100644
--- a/src/KingSystem/Physics/StaticCompound/physStaticCompoundRigidBodyGroup.h
+++ b/src/KingSystem/Physics/StaticCompound/physStaticCompoundRigidBodyGroup.h
@@ -20,6 +20,18 @@ class StaticCompound;
 
 class StaticCompoundRigidBodyGroup {
 public:
+    struct Config {
+        float unk1 = 1;
+        int move_duration_ticks = 30;
+        float unk2 = 1;
+        float unk3 = 1;
+    };
+
+    struct Epsilons {
+        float linvel = 0;
+        float angvel = 0;
+    };
+
     StaticCompoundRigidBodyGroup();
     ~StaticCompoundRigidBodyGroup();
 
@@ -44,15 +56,31 @@ public:
     bool setInstanceEnabled(BodyLayerType body_layer_type, int instance_id, bool enabled);
     void enableAllInstancesAndShapeKeys();
 
-    void modifyMatrix(const sead::Matrix34f& matrix, int index);
+    void resetMatrices();
+    void resetMatricesAndUpdateTransform();
+    void restoreMatricesAndUpdateTransform();
+    void restoreMatrices();
+
+    void setMatrix(const sead::Matrix34f& matrix, int index);
+    const sead::Matrix34f& getMatrix(int index) const;
+
+    sead::Matrix34f getTransformedMatrix(const sead::Matrix34f& mtx) const;
+    sead::Matrix34f getInvTransformedMatrix(const sead::Matrix34f& mtx) const;
+    sead::Vector3f getTransformedPos(const sead::Vector3f& pos) const;
+    sead::Vector3f getRotatedDir(const sead::Vector3f& dir) const;
+
+    void processUpdates();
+
+    static Config& getConfig();
+    static Epsilons& getEpsilons();
 
 private:
     enum class Flag {
         Initialised = 1 << 0,
         _2 = 1 << 1,
         _4 = 1 << 2,
-        HasModifiedMatrix = 1 << 3,
-        _10 = 1 << 4,
+        ShouldMoveBody = 1 << 3,
+        IsMovingBody = 1 << 4,
         HasEnabledOrDisabledInstance = 1 << 5,
     };
 
@@ -62,23 +90,25 @@ private:
         u8 _8[0xc0];
     };
 
-    const sead::Matrix34f& getMatrix();
+    const sead::Matrix34f& getTransform() const;
+
+    float getVelocityMultiplier() const;
 
-    sead::TypedBitFlag<Flag, sead::Atomic<u32>> mFlags;
+    mutable sead::TypedBitFlag<Flag, sead::Atomic<u32>> mFlags;
     sead::Atomic<u32> mModifiedMatrices;
     sead::Buffer<RigidBody*> mRigidBodiesPerBodyLayerType;
     sead::Buffer<hkpStaticCompoundShape*> mShapesPerBodyLayerType;
     // TODO: rename
     sead::Buffer<sead::Matrix34f> mMatrices;
     sead::Buffer<sead::Matrix34f> mMatrices2;
-    sead::Matrix34f mMtx0 = sead::Matrix34f::ident;
-    sead::Matrix34f mMtx1 = sead::Matrix34f::ident;
+    mutable sead::Matrix34f mMtx0 = sead::Matrix34f::ident;
+    mutable sead::Matrix34f mTransform = sead::Matrix34f::ident;
     sead::Matrix34f* mMtxPtr{};
     sead::PtrArray<RigidBody> mRigidBodies;
     StaticCompound* mStaticCompound{};
-    sead::Vector3f _c8 = sead::Vector3f::zero;
-    sead::Vector3f _d4 = sead::Vector3f::zero;
-    u32 _e0{};
+    sead::Vector3f mLinearVelocity = sead::Vector3f::zero;
+    sead::Vector3f mAngularVelocity = sead::Vector3f::zero;
+    int mUpdateTimer{};
     sead::Buffer<Unk1> _e8;
 };
 KSYS_CHECK_SIZE_NX150(StaticCompoundRigidBodyGroup, 0xf8);
diff --git a/src/KingSystem/Utils/MathUtil.h b/src/KingSystem/Utils/MathUtil.h
index 7450440e..e2db881f 100644
--- a/src/KingSystem/Utils/MathUtil.h
+++ b/src/KingSystem/Utils/MathUtil.h
@@ -25,4 +25,17 @@ inline float dot(sead::Vector3f u, const sead::Matrix34f& mtx, int row) {
     return u.x * mtx(row, 0) + u.y * mtx(row, 1) + u.z * mtx(row, 2);
 }
 
+inline void lerp(sead::Vector3f* result, const sead::Vector3f& a, const sead::Vector3f& b,
+                 float t) {
+    result->x = a.x + (b.x - a.x) * t;
+    result->y = a.y + (b.y - a.y) * t;
+    result->z = a.z + (b.z - a.z) * t;
+}
+
+inline sead::Vector3f lerp(const sead::Vector3f& a, const sead::Vector3f& b, float t) {
+    sead::Vector3f result;
+    lerp(&result, a, b, t);
+    return result;
+}
+
 }  // namespace ksys::util