diff --git a/data/uking_functions.csv b/data/uking_functions.csv index 94eb4309..eaec0cdf 100644 --- a/data/uking_functions.csv +++ b/data/uking_functions.csv @@ -84260,21 +84260,21 @@ Address,Quality,Size,Name 0x0000007100fcc5dc,O,000120,_ZNK4ksys4phys28StaticCompoundRigidBodyGroup26isAnyRigidBodyAddedToWorldEv 0x0000007100fcc654,O,000120,_ZNK4ksys4phys28StaticCompoundRigidBodyGroup31isAnyRigidBodyBeingAddedToWorldEv 0x0000007100fcc6cc,O,000248,_ZN4ksys4phys28StaticCompoundRigidBodyGroup10addToWorldEv -0x0000007100fcc7c4,U,000392,BodyGroup::getMatrix +0x0000007100fcc7c4,O,000392,_ZNK4ksys4phys28StaticCompoundRigidBodyGroup12getTransformEv 0x0000007100fcc94c,O,000212,_ZN4ksys4phys28StaticCompoundRigidBodyGroup15removeFromWorldEv 0x0000007100fcca20,O,000112,_ZN4ksys4phys28StaticCompoundRigidBodyGroup26removeFromWorldImmediatelyEv 0x0000007100fcca90,O,000108,_ZN4ksys4phys28StaticCompoundRigidBodyGroup18setInstanceEnabledENS0_13BodyLayerTypeEib 0x0000007100fccafc,O,000116,_ZN4ksys4phys28StaticCompoundRigidBodyGroup30enableAllInstancesAndShapeKeysEv -0x0000007100fccb70,U,000268,BodyGroup::x_7 -0x0000007100fccc7c,U,000312,BodyGroup::x_8 -0x0000007100fccdb4,U,000944,BodyGroup::x_9 -0x0000007100fcd164,U,000244,BodyGroup::x_10 -0x0000007100fcd258,O,000288,_ZN4ksys4phys28StaticCompoundRigidBodyGroup12modifyMatrixERKN4sead8Matrix34IfEEi -0x0000007100fcd378,U,000028,BodyGroup::x_12 -0x0000007100fcd394,U,000144,BodyGroup::x_16 -0x0000007100fcd424,U,000164,BodyGroup::x_13 -0x0000007100fcd4c8,U,000136,BodyGroup::x_14 -0x0000007100fcd550,U,000124,BodyGroup::x_15 +0x0000007100fccb70,O,000268,_ZN4ksys4phys28StaticCompoundRigidBodyGroup31resetMatricesAndUpdateTransformEv +0x0000007100fccc7c,O,000312,_ZN4ksys4phys28StaticCompoundRigidBodyGroup33restoreMatricesAndUpdateTransformEv +0x0000007100fccdb4,O,000944,_ZN4ksys4phys28StaticCompoundRigidBodyGroup14processUpdatesEv +0x0000007100fcd164,O,000244,_ZN4ksys4phys28StaticCompoundRigidBodyGroup15restoreMatricesEv +0x0000007100fcd258,O,000288,_ZN4ksys4phys28StaticCompoundRigidBodyGroup9setMatrixERKN4sead8Matrix34IfEEi +0x0000007100fcd378,O,000028,_ZNK4ksys4phys28StaticCompoundRigidBodyGroup9getMatrixEi +0x0000007100fcd394,O,000144,_ZNK4ksys4phys28StaticCompoundRigidBodyGroup20getTransformedMatrixERKN4sead8Matrix34IfEE +0x0000007100fcd424,O,000164,_ZNK4ksys4phys28StaticCompoundRigidBodyGroup23getInvTransformedMatrixERKN4sead8Matrix34IfEE +0x0000007100fcd4c8,O,000136,_ZNK4ksys4phys28StaticCompoundRigidBodyGroup17getTransformedPosERKN4sead7Vector3IfEE +0x0000007100fcd550,O,000124,_ZNK4ksys4phys28StaticCompoundRigidBodyGroup13getRotatedDirERKN4sead7Vector3IfEE 0x0000007100fcd5cc,U,000380, 0x0000007100fcd748,U,000036, 0x0000007100fcd76c,U,000052, 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; - sead::TypedBitFlag> mFlags; + float getVelocityMultiplier() const; + + mutable sead::TypedBitFlag> mFlags; sead::Atomic mModifiedMatrices; sead::Buffer mRigidBodiesPerBodyLayerType; sead::Buffer mShapesPerBodyLayerType; // TODO: rename sead::Buffer mMatrices; sead::Buffer 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 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 _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