From 544c33e2eb642dacdc0d2405b7b390a574fb3cc9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?L=C3=A9o=20Lam?= Date: Sat, 15 Jan 2022 21:16:23 +0100 Subject: [PATCH] ksys/phys: Finish RigidBodyMotionProxy --- data/uking_functions.csv | 2 +- .../Physics/RigidBody/physMotionAccessor.h | 2 + .../Physics/RigidBody/physRigidBody.h | 6 ++ .../RigidBody/physRigidBodyMotionProxy.cpp | 91 +++++++++++++++++-- .../RigidBody/physRigidBodyMotionProxy.h | 5 +- 5 files changed, 96 insertions(+), 10 deletions(-) diff --git a/data/uking_functions.csv b/data/uking_functions.csv index 0ae9903b..24781570 100644 --- a/data/uking_functions.csv +++ b/data/uking_functions.csv @@ -83436,7 +83436,7 @@ Address,Quality,Size,Name 0x0000007100fa4f48,O,000252,_ZN4ksys4phys20RigidBodyMotionProxy18setLinkedRigidBodyEPNS0_9RigidBodyE 0x0000007100fa5044,O,000008,_ZNK4ksys4phys20RigidBodyMotionProxy18getLinkedRigidBodyEv 0x0000007100fa504c,O,000012,_ZNK4ksys4phys20RigidBodyMotionProxy14isFlag40000SetEv -0x0000007100fa5058,U,000812,phys::RigidBodyMotionProxy::copyMotionFromLinkedRigidBody +0x0000007100fa5058,O,000812,_ZN4ksys4phys20RigidBodyMotionProxy29copyMotionFromLinkedRigidBodyEv 0x0000007100fa5384,O,000088,_ZN4ksys4phys20RigidBodyMotionProxy11getRotationEP13hkQuaternionf 0x0000007100fa53dc,O,000012,_ZN4ksys4phys20RigidBodyMotionProxy13setTimeFactorEf 0x0000007100fa53e8,O,000008,_ZN4ksys4phys20RigidBodyMotionProxy13getTimeFactorEv diff --git a/src/KingSystem/Physics/RigidBody/physMotionAccessor.h b/src/KingSystem/Physics/RigidBody/physMotionAccessor.h index 070d55b2..d5b82caa 100644 --- a/src/KingSystem/Physics/RigidBody/physMotionAccessor.h +++ b/src/KingSystem/Physics/RigidBody/physMotionAccessor.h @@ -59,6 +59,8 @@ public: RigidBody* getBody() const { return mBody; } hkpRigidBody* getHkBody() const { return mBody->getHkBody(); } + u32 get10() const { return _10; } + u32 get14() const { return _14; } protected: RigidBody* mBody = nullptr; diff --git a/src/KingSystem/Physics/RigidBody/physRigidBody.h b/src/KingSystem/Physics/RigidBody/physRigidBody.h index cfa132e6..faa81308 100644 --- a/src/KingSystem/Physics/RigidBody/physRigidBody.h +++ b/src/KingSystem/Physics/RigidBody/physRigidBody.h @@ -12,6 +12,8 @@ #include "KingSystem/Physics/System/physDefines.h" #include "KingSystem/Utils/Types.h" +class hkQuaternionf; +class hkVector4f; class hkpRigidBody; class hkpMotion; @@ -203,6 +205,10 @@ public: // 0x0000007100f91218 sead::Vector3f getAngularVelocity() const; + // 0x0000007100f92b74 + void computeVelocities(hkVector4f* linear_velocity, hkVector4f* angular_velocity, + const hkVector4f& position, const hkQuaternionf& rotation); + // 0x0000007100f93348 void setMass(float mass); // 0x0000007100f933fc diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodyMotionProxy.cpp b/src/KingSystem/Physics/RigidBody/physRigidBodyMotionProxy.cpp index 6b8d8fd2..c821465c 100644 --- a/src/KingSystem/Physics/RigidBody/physRigidBodyMotionProxy.cpp +++ b/src/KingSystem/Physics/RigidBody/physRigidBodyMotionProxy.cpp @@ -47,7 +47,8 @@ void RigidBodyMotionProxy::setPosition(const sead::Vector3f& position, } void RigidBodyMotionProxy::setTransformFromLinkedBody(const sead::Matrix34f& mtx) { - if (mFlags.isOff(Flag::_80000) && mFlags.isOff(Flag::_100000)) { + if (mFlags.isOff(Flag::HasExtraTranslateForLinkedRigidBody) && + mFlags.isOff(Flag::HasExtraRotateForLinkedRigidBody)) { setTransform(mtx, false); return; } @@ -55,13 +56,13 @@ void RigidBodyMotionProxy::setTransformFromLinkedBody(const sead::Matrix34f& mtx sead::Matrix34f new_mtx = mtx; sead::Vector3f translate; - if (mFlags.isOn(Flag::_80000)) { + if (mFlags.isOn(Flag::HasExtraTranslateForLinkedRigidBody)) { translate.setMul(mtx, mLinkedBodyExtraTranslate); } else { mtx.getTranslation(translate); } - if (mFlags.isOn(Flag::_100000)) { + if (mFlags.isOn(Flag::HasExtraRotateForLinkedRigidBody)) { sead::Quatf quat; mtx.toQuat(quat); quat *= mLinkedBodyExtraRotate; @@ -124,7 +125,8 @@ static sead::Matrix34f makeRT(const hkQuaternionf& r, const hkVector4f& t) { // NON_MATCHING: two fmul instructions reordered in sead::Matrix34f mtx = makeRT(...) void RigidBodyMotionProxy::setTransformFromLinkedBody(const hkVector4f& hk_translate, const hkQuaternionf& hk_rotate) { - if (mFlags.isOff(Flag::_80000) && mFlags.isOff(Flag::_100000)) { + if (mFlags.isOff(Flag::HasExtraTranslateForLinkedRigidBody) && + mFlags.isOff(Flag::HasExtraRotateForLinkedRigidBody)) { setTransform(makeRT(hk_rotate, hk_translate), false); return; } @@ -132,13 +134,13 @@ void RigidBodyMotionProxy::setTransformFromLinkedBody(const hkVector4f& hk_trans sead::Matrix34f mtx = makeRT(hk_rotate, hk_translate); sead::Vector3f translate; - if (mFlags.isOn(Flag::_80000)) { + if (mFlags.isOn(Flag::HasExtraTranslateForLinkedRigidBody)) { translate.setMul(mtx, mLinkedBodyExtraTranslate); } else { storeToVec3(&translate, hk_translate); } - if (mFlags.isOn(Flag::_100000)) { + if (mFlags.isOn(Flag::HasExtraRotateForLinkedRigidBody)) { sead::Quatf quat; toQuat(&quat, hk_rotate); quat *= mLinkedBodyExtraRotate; @@ -320,6 +322,83 @@ bool RigidBodyMotionProxy::isFlag40000Set() const { return mFlags.isOn(Flag::_40000); } +void RigidBodyMotionProxy::copyMotionFromLinkedRigidBody() { + auto lock = mBody->makeScopedLock(mBody->isFlag8Set()); + + auto* accessor = mLinkedRigidBody->getMotionAccessorForProxy(); + auto* linked_hk_body = mLinkedRigidBody->getHkBody(); + auto* this_hk_body = mBody->getHkBody(); + + bool reset_needed = false; + if (mFlags.isOn(Flag::HasLinkedRigidBodyWithoutFlag10)) { + if (_14 != accessor->get14()) { + _14 = accessor->get14(); + this_hk_body->setShape(linked_hk_body->getCollidable()->getShape()); + reset_needed = true; + } + + if (_10 != accessor->get10()) { + _10 = accessor->get10(); + this_hk_body->updateShape(); + reset_needed = true; + } + } + + if (mFlags.isOff(Flag::_40000)) { + hkVector4f position; + if (mFlags.isOn(Flag::HasExtraTranslateForLinkedRigidBody)) { + position.setTransformedPos(linked_hk_body->getTransform(), + toHkVec4(mLinkedBodyExtraTranslate)); + } else { + position = linked_hk_body->getPosition(); + } + + hkQuaternionf rotation; + if (mFlags.isOn(Flag::HasExtraRotateForLinkedRigidBody)) { + rotation.setMul(linked_hk_body->getRotation(), toHkQuat(mLinkedBodyExtraRotate)); + } else { + rotation = linked_hk_body->getRotation(); + } + + if (mLinkedRigidBody->getMotionType() != MotionType::Fixed) { + hkVector4f lin_vel; + hkVector4f ang_vel; + mBody->computeVelocities(&lin_vel, &ang_vel, position, rotation); + + hkVector4f zero; + zero.setZero(); + + hkVector4f vel_threshold; + vel_threshold.setAll(0.01); + constexpr auto mask = hkVector4fComparison::MASK_XYZ; + + const auto set_velocity = [&](const hkVector4f& velocity, auto get, auto set) { + // abs(vel) > 0.01? + hkVector4f abs_vel; + abs_vel.setAbs(velocity); + if (!vel_threshold.greaterEqual(abs_vel).allAreSet()) { + this_hk_body->activate(); + set(velocity); + } else if (!get().equalZero().template allAreSet()) { + this_hk_body->activate(); + set(zero); + } + }; + + set_velocity( + lin_vel, [&] { return this_hk_body->getLinearVelocity(); }, + [&](const auto& vel) { this_hk_body->setLinearVelocity(vel); }); + + set_velocity( + ang_vel, [&] { return this_hk_body->getAngularVelocity(); }, + [&](const auto& vel) { this_hk_body->setAngularVelocity(vel); }); + } + } + + if (reset_needed) + hkpRigidBody::updateBroadphaseAndResetCollisionInformationOfWarpedBody(this_hk_body); +} + void RigidBodyMotionProxy::getRotation(hkQuaternionf* quat) { sead::Quatf rotation; getRotation(&rotation); diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodyMotionProxy.h b/src/KingSystem/Physics/RigidBody/physRigidBodyMotionProxy.h index d7d010c7..4d628203 100644 --- a/src/KingSystem/Physics/RigidBody/physRigidBodyMotionProxy.h +++ b/src/KingSystem/Physics/RigidBody/physRigidBodyMotionProxy.h @@ -12,8 +12,8 @@ class RigidBodyMotionProxy : public MotionAccessor { public: enum class Flag { _40000 = 1 << 18, - _80000 = 1 << 19, - _100000 = 1 << 20, + HasExtraTranslateForLinkedRigidBody = 1 << 19, + HasExtraRotateForLinkedRigidBody = 1 << 20, HasLinkedRigidBodyWithoutFlag10 = 1 << 21, }; @@ -46,7 +46,6 @@ public: void resetLinkedRigidBody(); RigidBody* getLinkedRigidBody() const; bool isFlag40000Set() const; - // 0x0000007100fa5058 - triggers shape, position, velocity updates void copyMotionFromLinkedRigidBody(); ~RigidBodyMotionProxy() override;