ksys/phys: Add a bunch of easy RigidBody functions

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:

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(&current_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(&center);
+    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(&center);
+    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

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;

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

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();
 

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;

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

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;
 };