diff options
| author | Léo Lam <leo@leolam.fr> | 2022-01-17 12:12:51 +0100 |
|---|---|---|
| committer | Léo Lam <leo@leolam.fr> | 2022-01-17 16:58:22 +0100 |
| commit | 392c0973c71fc40ce451b8a191dd50592553e745 (patch) | |
| tree | 0c753fa8c3591d08ce83b5cad11f9897f98e3d6f /src | |
| parent | ab71075dee21ef1fe4103c08b4b12b37f29e39cb (diff) | |
ksys/phys: Add a bunch of easy RigidBody functions
Diffstat (limited to 'src')
| -rw-r--r-- | src/KingSystem/ActorSystem/actPhysicsUserTag.h | 2 | ||||
| -rw-r--r-- | src/KingSystem/Physics/RigidBody/physRigidBody.cpp | 303 | ||||
| -rw-r--r-- | src/KingSystem/Physics/RigidBody/physRigidBody.h | 77 | ||||
| -rw-r--r-- | src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.cpp | 18 | ||||
| -rw-r--r-- | src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h | 18 | ||||
| -rw-r--r-- | src/KingSystem/Physics/System/physMemSystem.h | 5 | ||||
| -rw-r--r-- | src/KingSystem/Physics/System/physUserTag.cpp | 2 | ||||
| -rw-r--r-- | src/KingSystem/Physics/System/physUserTag.h | 4 |
8 files changed, 399 insertions, 30 deletions
diff --git a/src/KingSystem/ActorSystem/actPhysicsUserTag.h b/src/KingSystem/ActorSystem/actPhysicsUserTag.h index 0c47e866..8388b106 100644 --- a/src/KingSystem/ActorSystem/actPhysicsUserTag.h +++ b/src/KingSystem/ActorSystem/actPhysicsUserTag.h @@ -23,7 +23,7 @@ public: void m4() override; void m5() override; const sead::SafeString& getName() const override; - void m7() override; + void m7(phys::RigidBody* rigid_body, int a) override; const sead::SafeString& getName2() const override; private: diff --git a/src/KingSystem/Physics/RigidBody/physRigidBody.cpp b/src/KingSystem/Physics/RigidBody/physRigidBody.cpp index 59350e90..33b6687a 100644 --- a/src/KingSystem/Physics/RigidBody/physRigidBody.cpp +++ b/src/KingSystem/Physics/RigidBody/physRigidBody.cpp @@ -3,18 +3,28 @@ #include <Havok/Physics2012/Dynamics/Entity/hkpRigidBody.h> #include <Havok/Physics2012/Dynamics/Motion/Rigid/hkpFixedRigidMotion.h> #include <Havok/Physics2012/Dynamics/Motion/Rigid/hkpKeyframedRigidMotion.h> +#include <cmath> #include "KingSystem/Physics/RigidBody/physMotionAccessor.h" #include "KingSystem/Physics/RigidBody/physRigidBodyMotion.h" #include "KingSystem/Physics/RigidBody/physRigidBodyMotionProxy.h" #include "KingSystem/Physics/RigidBody/physRigidBodyParam.h" #include "KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h" #include "KingSystem/Physics/System/physMemSystem.h" +#include "KingSystem/Physics/System/physUserTag.h" #include "KingSystem/Physics/physConversions.h" namespace ksys::phys { constexpr float MinInertia = 0.001; +static bool isVectorInvalid(const sead::Vector3f& vec) { + for (int i = 0; i < 3; ++i) { + if (std::isnan(vec.e[i])) + return true; + } + return false; +} + RigidBody::RigidBody(Type type, u32 mass_scaling, hkpRigidBody* hk_body, const sead::SafeString& name, sead::Heap* heap, bool a7) : mCS(heap), mHkBody(hk_body), mRigidBodyAccessor(hk_body), mType(type) { @@ -204,6 +214,36 @@ void RigidBody::sub_7100F8D21C() { } } +RigidBodyMotion* RigidBody::getMotionAccessor() const { + return sead::DynamicCast<RigidBodyMotion>(mMotionAccessor); +} + +RigidBodyMotion* RigidBody::getMotionAccessorForProxy() const { + return getMotionAccessor(); +} + +RigidBodyMotionProxy* RigidBody::getMotionProxy() const { + if (!isMassScaling()) + return nullptr; + if (!mMotionAccessor) + return nullptr; + return sead::DynamicCast<RigidBodyMotionProxy>(mMotionAccessor); +} + +RigidBody* RigidBody::getLinkedRigidBody() const { + auto* proxy = getMotionProxy(); + if (!proxy) + return nullptr; + return proxy->getLinkedRigidBody(); +} + +void RigidBody::resetLinkedRigidBody() const { + auto* proxy = getMotionProxy(); + if (!proxy) + return; + proxy->resetLinkedRigidBody(); +} + MotionType RigidBody::getMotionType() const { if (mMotionFlags.isOn(MotionFlag::Dynamic)) return MotionType::Dynamic; @@ -214,6 +254,17 @@ MotionType RigidBody::getMotionType() const { return mRigidBodyAccessor.getMotionType(); } +void RigidBody::setContactPoints(RigidContactPoints* points) { + mContactPoints = points; + if (isFlag8Set() && mContactPoints && !mContactPoints->isLinked()) + MemSystem::instance()->registerContactPoints(points); +} + +void RigidBody::resetFrozenState() { + if (mMotionAccessor) + mMotionAccessor->resetFrozenState(); +} + void RigidBody::setContactMask(u32 value) { mContactMask.setDirect(value); } @@ -226,8 +277,260 @@ void RigidBody::setContactNone() { mContactMask.makeAllZero(); } +void RigidBody::getPosition(sead::Vector3f* position) const { + if (mMotionAccessor) + mMotionAccessor->getPosition(position); + else + mRigidBodyAccessor.getPosition(position); +} + +sead::Vector3f RigidBody::getPosition() const { + sead::Vector3f position; + getPosition(&position); + return position; +} + +void RigidBody::getRotation(sead::Quatf* rotation) const { + if (mMotionAccessor) + mMotionAccessor->getRotation(rotation); + else + mRigidBodyAccessor.getRotation(rotation); +} + +sead::Quatf RigidBody::getRotation() const { + sead::Quatf rotation; + getRotation(&rotation); + return rotation; +} + +void RigidBody::getPositionAndRotation(sead::Vector3f* position, sead::Quatf* rotation) const { + getPosition(position); + getRotation(rotation); +} + +void RigidBody::getTransform(sead::Matrix34f* mtx) const { + if (mMotionAccessor) + mMotionAccessor->getTransform(mtx); + else + mRigidBodyAccessor.getTransform(mtx); +} + +sead::Matrix34f RigidBody::getTransform() const { + sead::Matrix34f transform; + getTransform(&transform); + return transform; +} + +bool RigidBody::setLinearVelocity(const sead::Vector3f& velocity, float epsilon) { + if (isVectorInvalid(velocity)) { + onInvalidParameter(); + return false; + } + + if (!isMassScaling() && RigidBodyRequestMgr::Config::isLinearVelocityTooHigh(velocity)) { + onInvalidParameter(1); + return false; + } + + return mMotionAccessor->setLinearVelocity(velocity, epsilon); +} + +void RigidBody::getLinearVelocity(sead::Vector3f* velocity) const { + if (mMotionAccessor) + mMotionAccessor->getLinearVelocity(velocity); + else + mRigidBodyAccessor.getLinearVelocity(velocity); +} + +sead::Vector3f RigidBody::getLinearVelocity() const { + sead::Vector3f v; + getLinearVelocity(&v); + return v; +} + +bool RigidBody::setAngularVelocity(const sead::Vector3f& velocity, float epsilon) { + if (isVectorInvalid(velocity)) { + onInvalidParameter(); + return false; + } + + return mMotionAccessor->setAngularVelocity(velocity, epsilon); +} + +void RigidBody::getAngularVelocity(sead::Vector3f* velocity) const { + if (mMotionAccessor) + mMotionAccessor->getAngularVelocity(velocity); + else + mRigidBodyAccessor.getAngularVelocity(velocity); +} + +sead::Vector3f RigidBody::getAngularVelocity() const { + sead::Vector3f v; + getAngularVelocity(&v); + return v; +} + +void RigidBody::getPointVelocity(sead::Vector3f* velocity, const sead::Vector3f& point) const { + const auto rel_pos = point - getCenterOfMassInWorld(); + velocity->setCross(getAngularVelocity(), rel_pos); + velocity->add(getLinearVelocity()); +} + +void RigidBody::setCenterOfMassInLocal(const sead::Vector3f& center) { + sead::Vector3f current_center; + mMotionAccessor->getCenterOfMassInLocal(¤t_center); + if (current_center != center) + mMotionAccessor->setCenterOfMassInLocal(center); +} + +void RigidBody::getCenterOfMassInLocal(sead::Vector3f* center) const { + mMotionAccessor->getCenterOfMassInLocal(center); +} + +sead::Vector3f RigidBody::getCenterOfMassInLocal() const { + sead::Vector3f center; + getCenterOfMassInLocal(¢er); + return center; +} + +void RigidBody::getCenterOfMassInWorld(sead::Vector3f* center) const { + if (mMotionFlags.isAnyOn({MotionFlag::DirtyCenterOfMassLocal, MotionFlag::DirtyTransform})) { + sead::Vector3f local_center; + getCenterOfMassInLocal(&local_center); + + sead::Matrix34f transform; + getTransform(&transform); + + center->setMul(transform, local_center); + } else { + auto hk_center = getMotion()->getCenterOfMassInWorld(); + storeToVec3(center, hk_center); + } +} + +sead::Vector3f RigidBody::getCenterOfMassInWorld() const { + sead::Vector3f center; + getCenterOfMassInWorld(¢er); + return center; +} + +void RigidBody::setMaxLinearVelocity(float max) { + if (!sead::Mathf::equalsEpsilon(max, getMaxLinearVelocity())) + mMotionAccessor->setMaxLinearVelocity(max); +} + +float RigidBody::getMaxLinearVelocity() const { + return mMotionAccessor->getMaxLinearVelocity(); +} + +void RigidBody::setMaxAngularVelocity(float max) { + if (!sead::Mathf::equalsEpsilon(max, getMaxAngularVelocity())) + mMotionAccessor->setMaxAngularVelocity(max); +} + +float RigidBody::getMaxAngularVelocity() const { + return mMotionAccessor->getMaxAngularVelocity(); +} + +void RigidBody::applyLinearImpulse(const sead::Vector3f& impulse) { + if (MemSystem::instance()->isPaused()) + return; + + if (hasFlag(Flag::_400) || hasFlag(Flag::_40)) + return; + + if (isVectorInvalid(impulse)) { + onInvalidParameter(); + return; + } + + if (!isMassScaling()) + getMotionAccessor()->applyLinearImpulse(impulse); +} + +void RigidBody::applyAngularImpulse(const sead::Vector3f& impulse) { + if (MemSystem::instance()->isPaused()) + return; + + if (hasFlag(Flag::_400) || hasFlag(Flag::_40)) + return; + + if (isVectorInvalid(impulse)) { + onInvalidParameter(); + return; + } + + if (!isMassScaling()) + getMotionAccessor()->applyAngularImpulse(impulse); +} + +void RigidBody::applyPointImpulse(const sead::Vector3f& impulse, const sead::Vector3f& point) { + if (MemSystem::instance()->isPaused()) + return; + + if (hasFlag(Flag::_400) || hasFlag(Flag::_40)) + return; + + if (isVectorInvalid(impulse)) { + onInvalidParameter(); + return; + } + + if (isVectorInvalid(point)) { + onInvalidParameter(); + return; + } + + if (!isMassScaling()) + getMotionAccessor()->applyPointImpulse(impulse, point); +} + +void RigidBody::setMass(float mass) { + if (isMassScaling()) + return; + getMotionAccessor()->setMass(mass); +} + +float RigidBody::getMass() const { + if (isMassScaling()) + return 0.0; + return getMotionAccessor()->getMass(); +} + +float RigidBody::getMassInv() const { + if (isMassScaling()) + return 0.0; + return getMotionAccessor()->getMassInv(); +} + +void RigidBody::lock(bool also_lock_world) { + if (also_lock_world) + MemSystem::instance()->lockWorld(getLayerType()); + mCS.lock(); +} + +void RigidBody::unlock(bool also_unlock_world) { + mCS.unlock(); + if (also_unlock_world) + MemSystem::instance()->unlockWorld(getLayerType()); +} + hkpMotion* RigidBody::getMotion() const { return getHkBody()->getMotion(); } +void RigidBody::onInvalidParameter(int code) { + sead::Vector3f pos, lin_vel, ang_vel; + mRigidBodyAccessor.getPosition(&pos); + mRigidBodyAccessor.getLinearVelocity(&lin_vel); + mRigidBodyAccessor.getAngularVelocity(&ang_vel); + // debug prints? + notifyUserTag(code); +} + +void RigidBody::notifyUserTag(int code) { + if (mUserTag) + mUserTag->m7(this, code); +} + } // namespace ksys::phys diff --git a/src/KingSystem/Physics/RigidBody/physRigidBody.h b/src/KingSystem/Physics/RigidBody/physRigidBody.h index 3f76d8fe..3bdde81d 100644 --- a/src/KingSystem/Physics/RigidBody/physRigidBody.h +++ b/src/KingSystem/Physics/RigidBody/physRigidBody.h @@ -22,6 +22,7 @@ namespace ksys::phys { class MotionAccessor; struct RigidBodyInstanceParam; class RigidBodyMotion; +class RigidBodyMotionProxy; class RigidContactPoints; class UserTag; @@ -127,29 +128,28 @@ public: // 0x0000007100f8cfa0 void x_0(); - void setMotionFlag(MotionFlag); - bool isActive() const; - // 0x0000007100f8d1f8 bool isFlag8Set() const; - // 0x0000007100f8d204 bool isMotionFlag1Set() const; - // 0x0000007100f8d210 bool isMotionFlag2Set() const; - // 0x0000007100f8d21c void sub_7100F8D21C(); // 0x0000007100f8d308 bool x_6(); - // 0x0000007100f8d680 + /// Get the motion accessor if it is a RigidBodyMotion. Returns nullptr otherwise. RigidBodyMotion* getMotionAccessor() const; - // 0x0000007100f90f28 - for internal use + /// Get the motion accessor if it is a RigidBodyMotion. Returns nullptr otherwise. + /// For internal use by the physics system. RigidBodyMotion* getMotionAccessorForProxy() const; - // 0x0000007100f8d70c - void* getMotionAccessorType2Stuff(); - // 0x0000007100f8d7a8 - void motionAccessorType2Stuff2(); + + /// Get the motion accessor if it is a RigidBodyMotionProxy. Returns nullptr otherwise. + RigidBodyMotionProxy* getMotionProxy() const; + /// Get the linked rigid body from the motion proxy (or nullptr if there is none). + RigidBody* getLinkedRigidBody() const; + /// Reset the linked rigid body if we have a motion proxy. + void resetLinkedRigidBody() const; + // 0x0000007100f8d840 void x_8(); @@ -174,12 +174,8 @@ public: void x_14(bool a, bool b, bool c); // 0x0000007100f8eabc void x_15(bool a, bool b); - // 0x0000007100f8ec3c - bool setLinearVelocityMaybe(const sead::Vector3f& velocity, float x); - // 0x0000007100f8ed74 - bool setAngularVelocityMaybe(const sead::Vector3f& velocity, float x); // 0x0000007100f8ee38 - void x_16(); + void resetFrozenState(); u32 addContactLayer(ContactLayer); u32 removeContactLayer(ContactLayer); @@ -193,32 +189,52 @@ public: void sub_7100F8F9E8(ReceiverMask*, void*); void sub_7100F8FA44(ContactLayer, u32); - // 0x0000007100f9004c + void getPosition(sead::Vector3f* position) const; + sead::Vector3f getPosition() const; + + void getRotation(sead::Quatf* rotation) const; + sead::Quatf getRotation() const; + + void getPositionAndRotation(sead::Vector3f* position, sead::Quatf* rotation) const; + void getTransform(sead::Matrix34f* mtx) const; + sead::Matrix34f getTransform() const; // 0x0000007100f8fb08 void setTransform(const sead::Matrix34f& mtx, bool propagate_to_linked_motions); - // 0x0000007100f8ec3c bool setLinearVelocity(const sead::Vector3f& velocity, float epsilon = sead::Mathf::epsilon()); - // 0x0000007100f9118c void getLinearVelocity(sead::Vector3f* velocity) const; - // 0x0000007100f911ac sead::Vector3f getLinearVelocity() const; - // 0x0000007100f8ed74 bool setAngularVelocity(const sead::Vector3f& velocity, float epsilon = sead::Mathf::epsilon()); - // 0x0000007100f911f8 void getAngularVelocity(sead::Vector3f* velocity) const; - // 0x0000007100f91218 sead::Vector3f getAngularVelocity() const; + // 0x0000007100f91264 + void getPointVelocity(sead::Vector3f* velocity, const sead::Vector3f& point) const; + // 0x0000007100f92b74 void computeVelocities(hkVector4f* linear_velocity, hkVector4f* angular_velocity, const hkVector4f& position, const hkQuaternionf& rotation); - // 0x0000007100f93348 + void setCenterOfMassInLocal(const sead::Vector3f& center); + void getCenterOfMassInLocal(sead::Vector3f* center) const; + sead::Vector3f getCenterOfMassInLocal() const; + + void getCenterOfMassInWorld(sead::Vector3f* center) const; + sead::Vector3f getCenterOfMassInWorld() const; + + void setMaxLinearVelocity(float max); + float getMaxLinearVelocity() const; + + void setMaxAngularVelocity(float max); + float getMaxAngularVelocity() const; + + void applyLinearImpulse(const sead::Vector3f& impulse); + void applyAngularImpulse(const sead::Vector3f& impulse); + void applyPointImpulse(const sead::Vector3f& impulse, const sead::Vector3f& point); + void setMass(float mass); - // 0x0000007100f933fc float getMass() const; // 0x0000007100f93498 float getMassInv() const; @@ -247,6 +263,7 @@ public: bool hasFlag(Flag flag) const { return mFlags.isOn(flag); } const auto& getMotionFlags() const { return mMotionFlags; } void resetMotionFlagDirect(const MotionFlag flag) { mMotionFlags.reset(flag); } + void setMotionFlag(MotionFlag flag); hkpRigidBody* getHkBody() const { return mHkBody; } @@ -280,7 +297,13 @@ public: } private: + ContactLayerType getLayerType() const { + return !isMassScaling() ? ContactLayerType::Entity : ContactLayerType::Sensor; + } + void createMotionAccessor(sead::Heap* heap); + void onInvalidParameter(int code = 0); + void notifyUserTag(int code); sead::CriticalSection mCS; sead::TypedBitFlag<Flag, sead::Atomic<u32>> mFlags{}; @@ -288,7 +311,7 @@ private: sead::BitFlag32 mContactMask{}; hkpRigidBody* mHkBody; UserTag* mUserTag = nullptr; - void* _88 = nullptr; + RigidContactPoints* mContactPoints = nullptr; void* _90 = nullptr; u16 _98 = 0; RigidBodyAccessor mRigidBodyAccessor; diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.cpp b/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.cpp index 171d72ed..694d6de3 100644 --- a/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.cpp +++ b/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.cpp @@ -4,6 +4,9 @@ namespace ksys::phys { +static RigidBodyRequestMgr::Config sRigidBodyRequestMgrConfig; +static bool sEnableLinearVelocityChecks; + RigidBodyRequestMgr::RigidBodyRequestMgr() = default; RigidBodyRequestMgr::~RigidBodyRequestMgr() { @@ -103,4 +106,19 @@ bool RigidBodyRequestMgr::deregisterMotionAccessor(MotionAccessor* accessor) { return true; } +RigidBodyRequestMgr::Config& RigidBodyRequestMgr::Config::get() { + return sRigidBodyRequestMgrConfig; +} + +bool RigidBodyRequestMgr::Config::isLinearVelocityTooHigh(const sead::Vector3f& velocity) { + if (!sEnableLinearVelocityChecks) + return false; + + return velocity.squaredLength() > get().linear_velocity_threshold_sq; +} + +void RigidBodyRequestMgr::Config::enableLinearVelocityChecks(bool enable) { + sEnableLinearVelocityChecks = enable; +} + } // namespace ksys::phys diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h b/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h index c7ba6f9b..eb90c7ba 100644 --- a/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h +++ b/src/KingSystem/Physics/RigidBody/physRigidBodyRequestMgr.h @@ -20,6 +20,24 @@ class RigidBody; class RigidBodyRequestMgr : public sead::hostio::Node { public: + struct Config { + float _0 = 0.6; + float _4 = 0.7; + float _8 = 1.25; + float _c = 1.0; + float _10 = 0.2; + float _14 = 0.9; + float _18 = 0.5; + float _1c = 1.0; + float _20 = 4.0; + // 5000m/s (squared) + float linear_velocity_threshold_sq = 2.5e7; + + static Config& get(); + static bool isLinearVelocityTooHigh(const sead::Vector3f& velocity); + static void enableLinearVelocityChecks(bool enable); + }; + RigidBodyRequestMgr(); virtual ~RigidBodyRequestMgr(); diff --git a/src/KingSystem/Physics/System/physMemSystem.h b/src/KingSystem/Physics/System/physMemSystem.h index 63652c5a..143c9764 100644 --- a/src/KingSystem/Physics/System/physMemSystem.h +++ b/src/KingSystem/Physics/System/physMemSystem.h @@ -36,6 +36,8 @@ public: SystemData* getSystemData() const { return mSystemData; } MaterialTable* getMaterialTable() const { return mMaterialTable; } + bool isPaused() const; + void initSystemData(sead::Heap* heap); RigidContactPoints* allocContactPoints(sead::Heap* heap, int num, const sead::SafeString& name, @@ -51,6 +53,9 @@ public: void removeSystemGroupHandler(SystemGroupHandler* handler); + void lockWorld(ContactLayerType type, void* a = nullptr, int b = 0, bool c = false); + void unlockWorld(ContactLayerType type, void* a = nullptr, int b = 0, bool c = false); + private: u8 _28[0xa8 - 0x28]; sead::CriticalSection mCS; diff --git a/src/KingSystem/Physics/System/physUserTag.cpp b/src/KingSystem/Physics/System/physUserTag.cpp index 165f8493..8a2fe69d 100644 --- a/src/KingSystem/Physics/System/physUserTag.cpp +++ b/src/KingSystem/Physics/System/physUserTag.cpp @@ -14,6 +14,6 @@ void UserTag::m4() {} void UserTag::m5() {} -void UserTag::m7() {} +void UserTag::m7(RigidBody* rigid_body, int a) {} } // namespace ksys::phys diff --git a/src/KingSystem/Physics/System/physUserTag.h b/src/KingSystem/Physics/System/physUserTag.h index e4fb471f..1d2ea480 100644 --- a/src/KingSystem/Physics/System/physUserTag.h +++ b/src/KingSystem/Physics/System/physUserTag.h @@ -6,6 +6,8 @@ namespace ksys::phys { +class RigidBody; + class UserTag { SEAD_RTTI_BASE(UserTag) public: @@ -18,7 +20,7 @@ public: virtual void m4(); virtual void m5(); virtual const sead::SafeString& getName() const { return sead::SafeString::cEmptyString; } - virtual void m7(); + virtual void m7(RigidBody* rigid_body, int a); virtual const sead::SafeString& getName2() const { return sead::SafeString::cEmptyString; } virtual ~UserTag() = default; }; |
