From 08ccaf9c3ce8c9ac637a5ea6b5e2f27799ec9fe2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?L=C3=A9o=20Lam?= Date: Mon, 19 Dec 2022 12:25:57 +0100 Subject: [PATCH] ksys/phys: Add RagdollController::getTransform and more Credit to @Dragorn421 for helping me figure out the rotation matrix thing in the getTransform function --- data/uking_functions.csv | 8 +- lib/gsys/include/gsys/gsysModel.h | 1 + .../Base/Math/QsTransform/hkQsTransformf.h | 2 + .../Physics/Ragdoll/physRagdollController.cpp | 175 ++++++++++++++++-- .../Physics/Ragdoll/physRagdollController.h | 26 ++- .../Physics/System/physInstanceSet.cpp | 2 +- src/KingSystem/Physics/physConversions.h | 5 + src/KingSystem/Utils/MathUtil.h | 6 + 8 files changed, 198 insertions(+), 27 deletions(-) diff --git a/data/uking_functions.csv b/data/uking_functions.csv index 3a617dea..670c09dc 100644 --- a/data/uking_functions.csv +++ b/data/uking_functions.csv @@ -93613,8 +93613,8 @@ Address,Quality,Size,Name 0x00000071012217a8,O,000056,_ZN4ksys4phys17RagdollController14setContactNoneEv 0x00000071012217e0,O,000032,_ZN4ksys4phys17RagdollController13setContactAllEi 0x0000007101221800,O,000032,_ZN4ksys4phys17RagdollController14setContactNoneEi -0x0000007101221820,U,001188, -0x0000007101221cc4,U,000016, +0x0000007101221820,O,001188,_ZN4ksys4phys17RagdollController16changeWorldStateENS1_10WorldStateE +0x0000007101221cc4,O,000016,_ZNK4ksys4phys17RagdollController13getWorldStateEv 0x0000007101221cd4,O,000012,_ZN4ksys4phys17RagdollController17setExtraRigidBodyEPNS0_9RigidBodyEi 0x0000007101221ce0,O,000068,_ZN4ksys4phys17RagdollController16setGravityFactorEf 0x0000007101221d24,U,000032, @@ -93625,8 +93625,8 @@ Address,Quality,Size,Name 0x0000007101222144,O,000132,_ZNK4ksys4phys17RagdollController22getParentBoneRigidBodyEPKNS0_9RigidBodyE 0x00000071012221c8,O,000132,_ZNK4ksys4phys17RagdollController16getNumChildBonesEPKNS0_9RigidBodyE 0x000000710122224c,O,000156,_ZNK4ksys4phys17RagdollController21getChildBoneRigidBodyEPKNS0_9RigidBodyEi -0x00000071012222e8,U,001268, -0x00000071012227dc,U,001844, +0x00000071012222e8,O,001268,_ZNK4ksys4phys17RagdollController12getTransformEi +0x00000071012227dc,O,001844,_ZNK4ksys4phys17RagdollController27getTransformWithCustomYAxisEiRKN4sead7Vector3IfEE 0x0000007101222f10,O,000340,_ZNK4ksys4phys17RagdollController24getConstraintIndexByNameERKN4sead14SafeStringBaseIcEE 0x0000007101223064,O,000012,_ZNK4ksys4phys17RagdollController17getNumConstraintsEv 0x0000007101223070,O,000044,_ZN4ksys4phys17RagdollController16enableConstraintEib diff --git a/lib/gsys/include/gsys/gsysModel.h b/lib/gsys/include/gsys/gsysModel.h index 7cc7769b..a307888c 100644 --- a/lib/gsys/include/gsys/gsysModel.h +++ b/lib/gsys/include/gsys/gsysModel.h @@ -39,6 +39,7 @@ public: sead::PtrArray& getUnits() { return mUnitAccess; } const sead::PtrArray& getUnits() const { return mUnitAccess; } + const sead::Matrix34f& getMatrix() const { return mMatrix; } const sead::Vector3f& getScale() const { return _88; } void setBoneLocalMatrix(const BoneAccessKey& key, const sead::Matrix34f& matrix, diff --git a/lib/hkStubs/Havok/Common/Base/Math/QsTransform/hkQsTransformf.h b/lib/hkStubs/Havok/Common/Base/Math/QsTransform/hkQsTransformf.h index d9025bbb..b2bc9965 100644 --- a/lib/hkStubs/Havok/Common/Base/Math/QsTransform/hkQsTransformf.h +++ b/lib/hkStubs/Havok/Common/Base/Math/QsTransform/hkQsTransformf.h @@ -56,6 +56,8 @@ public: /// this *= b HK_FORCE_INLINE void setMulEq(const hkQsTransformf& b); + bool isOk(hkFloat32 epsilon = hkFloat32(1e-3f)) const; + void fastRenormalize(); hkVector4f m_translation; diff --git a/src/KingSystem/Physics/Ragdoll/physRagdollController.cpp b/src/KingSystem/Physics/Ragdoll/physRagdollController.cpp index 51b59047..93611e42 100644 --- a/src/KingSystem/Physics/Ragdoll/physRagdollController.cpp +++ b/src/KingSystem/Physics/Ragdoll/physRagdollController.cpp @@ -9,7 +9,10 @@ #include #include #include +#include +#include #include +#include #include #include "KingSystem/Physics/Ragdoll/physRagdollControllerMgr.h" #include "KingSystem/Physics/Ragdoll/physRagdollRigidBody.h" @@ -22,6 +25,7 @@ #include "KingSystem/Physics/physConversions.h" #include "KingSystem/Utils/Debug.h" #include "KingSystem/Utils/IteratorUtil.h" +#include "KingSystem/Utils/MathUtil.h" #include "KingSystem/Utils/SafeDelete.h" namespace ksys::phys { @@ -87,17 +91,7 @@ bool RagdollController::doInit(const RagdollParam* param, sead::DirectResource* } mBoneRigidBodies.allocBufferAssert(num_bones, heap); - - { - static constexpr size_t Alignment = 0x20; - _b0 = sead::Mathu::roundUpPow2(sizeof(hkQsTransformf) * num_bones, Alignment); - auto* buffer = new (heap, Alignment) u8[_b0]; - if (num_bones > 0 && buffer != nullptr) { - _a0 = num_bones; - _a8 = buffer; - } - } - + allocateBoneTransforms(num_bones, heap); mBoneVectors.allocBufferAssert(num_bones, heap); mBoneStuff.allocBufferAssert(num_bones, heap); @@ -158,6 +152,14 @@ bool RagdollController::doInit(const RagdollParam* param, sead::DirectResource* return true; } +void RagdollController::allocateBoneTransforms(int num_bones, sead::Heap* heap) { + static constexpr size_t Alignment = 0x20; + mBoneTransformsByteSize = + sead::Mathu::roundUpPow2(sizeof(hkQsTransformf) * num_bones, Alignment); + auto* buffer = new (heap, Alignment) u8[mBoneTransformsByteSize]; + mBoneTransforms.setBuffer(num_bones, new (buffer) hkQsTransformf[num_bones]); +} + void RagdollController::registerSelf() { const bool registered = System::instance()->getRagdollControllerMgr()->addController(this); mFlags.change(Flag::IsRegistered, registered); @@ -208,9 +210,10 @@ void RagdollController::finalize() { mBoneStuff.freeBuffer(); mBoneStuff2.freeBuffer(); - if (_b0) { - delete[] _a8; - _b0 = 0; + if (mBoneTransformsByteSize != 0) { + std::destroy_n(mBoneTransforms.getBufferPtr(), mBoneTransforms.size()); + delete[] reinterpret_cast(mBoneTransforms.getBufferPtr()); + mBoneTransformsByteSize = 0; } } @@ -414,6 +417,64 @@ void RagdollController::setContactNone(int bone_index) { mBoneRigidBodies[bone_index]->setContactNone(false); } +void RagdollController::changeWorldState(RagdollController::WorldState state) { + if (getWorldState() == state) + return; + + const int num_rigid_bodies = mRagdollInstance->m_rigidBodies.size(); + + ScopedPhysicsLock lock{this}; + + if (state == WorldState::AddedToWorld) { + if (mModel) { + if (auto* accessor = getModelBoneAccessor()) + accessor->copyModelPoseToHavok(ModelBoneAccessor::EnableScale::No); + + setTransform(mModel->getMatrix()); + } + + for (int i = 0, n = num_rigid_bodies; i < n; ++i) { + mBoneRigidBodies[i]->addToWorld(); + + mBoneRigidBodies[i]->setLinearVelocity(sead::Vector3f::zero); + mBoneRigidBodies[i]->setAngularVelocity(sead::Vector3f::zero); + + if (mKeyframedBones.isOnBit(i)) { + mBoneRigidBodies[i]->changeMotionType(MotionType::Keyframed); + mBoneRigidBodies[i]->setContactLayer(ContactLayer::EntityNoHit); + } else { + mBoneRigidBodies[i]->changeMotionType(MotionType::Dynamic); + mBoneRigidBodies[i]->setContactLayer(mContactLayer); + } + } + + for (auto& x : mBoneStuff) + x = 1; + + mFlags.reset(Flag::_8); + _e8 = _e9; + + for (auto* body : mBoneRigidBodies) + body->setContactNone(true); + + mFlags.set(Flag::_20); + mFlags.set(Flag::AddedToWorld); + mFlags.reset(Flag::_40); + mFlags.reset(Flag::_2); + mFlags.reset(Flag::_4); + } else { + for (int i = 0, n = num_rigid_bodies; i < n; ++i) + mBoneRigidBodies[i]->removeFromWorld(); + + mFlags.reset(Flag::AddedToWorld); + mFlags.reset(Flag::_40); + } +} + +RagdollController::WorldState RagdollController::getWorldState() const { + return mFlags.isOn(Flag::AddedToWorld) ? WorldState::AddedToWorld : WorldState::NotAddedToWorld; +} + void RagdollController::setExtraRigidBody(RigidBody* body, int bone_index) { mExtraRigidBody = body; mBoneIndexForExtraRigidBody = bone_index; @@ -468,6 +529,88 @@ RagdollRigidBody* RagdollController::getChildBoneRigidBody(const RigidBody* body return sead::DynamicCast(body)->getChildBodies_()[index]; } +sead::Matrix34f RagdollController::getTransform(int bone_index) const { + if (mFlags.isOff(Flag::_40)) { + if (util::isMatrixInvalid(mModel->getMatrix())) + return sead::Matrix34f::ident; + + return mModel->getMatrix(); + } + + const sead::Matrix34f parent_transform = mBoneRigidBodies[bone_index]->getTransform(); + hkQsTransformf hk_parent_transform; + toHkQsTransform(&hk_parent_transform, parent_transform, sead::Vector3f::ones); + + hkQsTransformf result; + result.setMulMulInverse(hk_parent_transform, mBoneTransforms[bone_index]); + + const bool parent_ok = hk_parent_transform.isOk(); + SEAD_ASSERT(parent_ok); + const bool bone_ok = mBoneTransforms[bone_index].isOk(); + SEAD_ASSERT(bone_ok); + const bool result_ok = result.isOk(); + SEAD_ASSERT(result_ok); + + sead::Matrix34f out; + toMtx34(&out, result); + return out; +} + +sead::Matrix34f RagdollController::getTransformWithCustomYAxis(int bone_index, + const sead::Vector3f& y_axis) const { + if (mFlags.isOff(Flag::_40)) { + if (util::isMatrixInvalid(mModel->getMatrix())) + return sead::Matrix34f::ident; + + return mModel->getMatrix(); + } + + sead::Matrix34f transform = mBoneRigidBodies[bone_index]->getTransform(); + + { + sead::Vector3f v1 = (transform(1, 1) > 0.0f) ? y_axis : -y_axis; + v1.normalize(); + + sead::Vector3f c2 = util::getCol(transform, 2); + c2.normalize(); + + sead::Vector3f v0; + // Compute the cross product of v1 and c2. + // If v1 and c2 are collinear, then their cross product will be the zero vector; + // compute the cross product of v1 and the first column instead in that case. + // (That product should never be zero because the first column and u2 are not collinear.) + const auto dot = sead::Mathf::abs(v1.dot(c2)); + // XXX: this should probably check if the dot product is approximately equal to 1.0 instead + v0.setCross(v1, dot >= 1.0f ? util::getCol(transform, 0) : c2); + v0.normalize(); + + sead::Vector3f v2; + v2.setCross(v0, v1); + v2.normalize(); + + transform.setBase(0, v0); + transform.setBase(1, v1); + transform.setBase(2, v2); + } + + hkQsTransformf hk_parent_transform; + toHkQsTransform(&hk_parent_transform, transform, sead::Vector3f::ones); + + hkQsTransformf result; + result.setMulMulInverse(hk_parent_transform, mBoneTransforms[bone_index]); + + const bool parent_ok = hk_parent_transform.isOk(); + SEAD_ASSERT(parent_ok); + const bool bone_ok = mBoneTransforms[bone_index].isOk(); + SEAD_ASSERT(bone_ok); + const bool result_ok = result.isOk(); + SEAD_ASSERT(result_ok); + + sead::Matrix34f out; + toMtx34(&out, result); + return out; +} + int RagdollController::getConstraintIndexByName(const sead::SafeString& name) const { for (int i = 0, n = getNumConstraints(); i < n; ++i) { if (name == mRagdollInstance->m_constraints[i]->getName()) @@ -492,7 +635,7 @@ void RagdollController::setContactLayer(ContactLayer layer) { if (mContactLayer.value() == layer) return; - if (mFlags.isOn(Flag::_10)) { + if (mFlags.isOn(Flag::AddedToWorld)) { for (int bone = 0, num_bones = mBoneRigidBodies.size(); bone < num_bones; ++bone) { if (mKeyframedBones.isOffBit(bone)) mBoneRigidBodies[bone]->setContactLayer(layer); @@ -528,7 +671,7 @@ void RagdollController::setMaximumUnk1(u8 value) { } void RagdollController::stopForcingKeyframing() { - if (mFlags.isOn(Flag::_10)) { + if (mFlags.isOn(Flag::AddedToWorld)) { for (int i = 0, n = mBoneRigidBodies.size(); i < n; ++i) { setKeyframed(i, false, {}); } diff --git a/src/KingSystem/Physics/Ragdoll/physRagdollController.h b/src/KingSystem/Physics/Ragdoll/physRagdollController.h index c7524abd..7528e59e 100644 --- a/src/KingSystem/Physics/Ragdoll/physRagdollController.h +++ b/src/KingSystem/Physics/Ragdoll/physRagdollController.h @@ -90,7 +90,14 @@ public: void setContactNone(); void setContactAll(int bone_index); void setContactNone(int bone_index); - u32 sub_7101221CC4(); + + enum class WorldState { + AddedToWorld = 0, + NotAddedToWorld = 2, + }; + void changeWorldState(WorldState state); + WorldState getWorldState() const; + void setExtraRigidBody(RigidBody* body, int bone_index); void setGravityFactor(float factor); @@ -103,6 +110,9 @@ public: int getNumChildBones(const RigidBody* body) const; RagdollRigidBody* getChildBoneRigidBody(const RigidBody* body, int index) const; + sead::Matrix34f getTransform(int bone_index) const; + sead::Matrix34f getTransformWithCustomYAxis(int bone_index, const sead::Vector3f& y_axis) const; + int getConstraintIndexByName(const sead::SafeString& name) const; int getNumConstraints() const; void enableConstraint(int index, bool enable); @@ -133,7 +143,12 @@ private: }; enum class Flag { - _10 = 0x10, + _2 = 0x2, + _4 = 0x4, + _8 = 0x8, + AddedToWorld = 0x10, + _20 = 0x20, + _40 = 0x40, _80 = 0x80, /// Whether this controller has been registered with the RagdollControllerMgr. IsRegistered = 0x100, @@ -149,6 +164,7 @@ private: bool doInit(const RagdollParam* param, sead::DirectResource* res, gsys::Model* model, sead::Heap* heap); + void allocateBoneTransforms(int num_bones, sead::Heap* heap); void finalize(); void removeConstraints(); void setTransform(const hkQsTransformf& transform); @@ -179,10 +195,8 @@ private: sead::Buffer mBoneStuff2; float _98 = 0.1; float mGravityFactorOverride = 1.0; - u32 _a0 = 0; - // TODO: type - u8* _a8 = nullptr; - u32 _b0 = 0; + sead::Buffer mBoneTransforms; + u32 mBoneTransformsByteSize = 0; const RagdollParam* mRagdollParam = nullptr; sead::TypedBitFlag mFlags; sead::BitFlag32 mDisabledConstraints; diff --git a/src/KingSystem/Physics/System/physInstanceSet.cpp b/src/KingSystem/Physics/System/physInstanceSet.cpp index 872cd713..6e5269c8 100644 --- a/src/KingSystem/Physics/System/physInstanceSet.cpp +++ b/src/KingSystem/Physics/System/physInstanceSet.cpp @@ -192,7 +192,7 @@ void InstanceSet::sub_7100FBD284(const sead::Matrix34f& mtx) { if (mRagdollController == nullptr) return; - if (mRagdollController->sub_7101221CC4() == 0) + if (mRagdollController->getWorldState() == RagdollController::WorldState::AddedToWorld) sub_7100FBC890(mtx, false, false); } diff --git a/src/KingSystem/Physics/physConversions.h b/src/KingSystem/Physics/physConversions.h index 2ee002cc..e58bf8dd 100644 --- a/src/KingSystem/Physics/physConversions.h +++ b/src/KingSystem/Physics/physConversions.h @@ -56,6 +56,11 @@ inline void toHkQuat(hkQuaternionf* out, const sead::Quatf& quat) { return {quat.x, quat.y, quat.z, quat.w}; } +inline void toMtx34(sead::Matrix34f* out, const hkQsTransformf& transform) { + out->makeSQT(toVec3(transform.getScale()), toQuat(transform.getRotation()), + toVec3(transform.getTranslation())); +} + inline void toMtx34(sead::Matrix34f* out, const hkTransformf& transform) { const hkRotationf& rotate = transform.getRotation(); const hkVector4f& translate = transform.getTranslation(); diff --git a/src/KingSystem/Utils/MathUtil.h b/src/KingSystem/Utils/MathUtil.h index 6ba5d833..9d4b0ed3 100644 --- a/src/KingSystem/Utils/MathUtil.h +++ b/src/KingSystem/Utils/MathUtil.h @@ -56,4 +56,10 @@ inline bool isMatrixInvalid(const sead::Matrix34f& matrix) { return false; } +inline sead::Vector3f getCol(const sead::Matrix34f& mtx, int col) { + sead::Vector3f result; + mtx.getBase(result, col); + return result; +} + } // namespace ksys::util