ksys/phys: Add more RigidBody functions

4 files changed, 375 insertions, 26 deletions

diff --git a/src/KingSystem/Physics/RigidBody/physRigidBody.cpp b/src/KingSystem/Physics/RigidBody/physRigidBody.cpp
index 3689a25c..e9c984e5 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBody.cpp
+++ b/src/KingSystem/Physics/RigidBody/physRigidBody.cpp
@@ -243,6 +243,31 @@ void RigidBody::resetLinkedRigidBody() const {
     accessor->resetLinkedRigidBody();
 }
 
+bool RigidBody::setLinkedRigidBody(RigidBody* body) {
+    if (!isSensor())
+        return false;
+
+    if (body != nullptr && hasFlag(Flag::_20))
+        return false;
+
+    if (!mMotionAccessor)
+        return false;
+
+    auto* accessor = sead::DynamicCast<RigidBodyMotionSensor>(mMotionAccessor);
+    if (!accessor)
+        return false;
+
+    accessor->setLinkedRigidBody(body);
+    return true;
+}
+
+bool RigidBody::isSensorMotionFlag40000Set() const {
+    auto* accessor = getSensorMotionAccessor();
+    if (!accessor)
+        return false;
+    return accessor->isFlag40000Set();
+}
+
 MotionType RigidBody::getMotionType() const {
     if (mMotionFlags.isOn(MotionFlag::Dynamic))
         return MotionType::Dynamic;
@@ -276,6 +301,15 @@ void RigidBody::setContactNone() {
     mContactMask.makeAllZero();
 }
 
+void RigidBody::setPosition(const sead::Vector3f& position, bool propagate_to_linked_motions) {
+    if (isVectorInvalid(position)) {
+        onInvalidParameter();
+        return;
+    }
+
+    mMotionAccessor->setPosition(position, propagate_to_linked_motions);
+}
+
 void RigidBody::getPosition(sead::Vector3f* position) const {
     if (mMotionAccessor)
         mMotionAccessor->getPosition(position);
@@ -502,14 +536,264 @@ float RigidBody::getMassInv() const {
     return getEntityMotionAccessor()->getMassInv();
 }
 
+void RigidBody::setInertiaLocal(const sead::Vector3f& inertia) {
+    if (!isEntity())
+        return;
+    getEntityMotionAccessor()->setInertiaLocal(inertia);
+}
+
+void RigidBody::getInertiaLocal(sead::Vector3f* inertia) const {
+    if (isEntity()) {
+        getEntityMotionAccessor()->getInertiaLocal(inertia);
+    } else {
+        inertia->set(0, 0, 0);
+    }
+}
+
+void RigidBody::setLinearDamping(float value) {
+    if (!isEntity())
+        return;
+    getEntityMotionAccessor()->setLinearDamping(value);
+}
+
+float RigidBody::getLinearDamping() const {
+    if (!isEntity())
+        return 0.0;
+    return getEntityMotionAccessor()->getLinearDamping();
+}
+
+void RigidBody::setAngularDamping(float value) {
+    if (!isEntity())
+        return;
+    getEntityMotionAccessor()->setAngularDamping(value);
+}
+
+float RigidBody::getAngularDamping() const {
+    if (!isEntity())
+        return 0.0;
+    return getEntityMotionAccessor()->getAngularDamping();
+}
+
+void RigidBody::setGravityFactor(float value) {
+    if (!isEntity() || !mMotionAccessor)
+        return;
+    getEntityMotionAccessor()->setGravityFactor(value);
+}
+
+float RigidBody::getGravityFactor() const {
+    if (!mMotionAccessor || !isEntity())
+        return 1.0;
+    return getEntityMotionAccessor()->getGravityFactor();
+}
+
+bool RigidBody::setTimeFactor(float value) {
+    if (!mMotionAccessor)
+        return false;
+
+    const float current_time_factor = getTimeFactor();
+    if (sead::Mathf::equalsEpsilon(current_time_factor, value, 0.001))
+        return false;
+
+    if (hasFlag(Flag::_80000))
+        return false;
+
+    mMotionAccessor->setTimeFactor(value);
+
+    if (value != 0.0 && current_time_factor != 0.0 && isEntity()) {
+        setLinearDamping(getLinearDamping());
+        setAngularDamping(getAngularDamping());
+    }
+
+    return true;
+}
+
+float RigidBody::getTimeFactor() const {
+    return mMotionAccessor->getTimeFactor();
+}
+
+sead::Vector3f RigidBody::getInertiaLocal() const {
+    sead::Vector3f inertia;
+    getInertiaLocal(&inertia);
+    return inertia;
+}
+
+float RigidBody::m12(float x, float y) {
+    return y;
+}
+
+float RigidBody::m4() {
+    return 0.0;
+}
+
+void RigidBody::setWaterBuoyancyScale(float scale) {
+    if (!isEntity())
+        return;
+    getEntityMotionAccessor()->setWaterBuoyancyScale(scale);
+}
+
+float RigidBody::getWaterBuoyancyScale() const {
+    if (!isEntity())
+        return 0.0;
+    return getEntityMotionAccessor()->getWaterBuoyancyScale();
+}
+
+void RigidBody::setWaterFlowEffectiveRate(float rate) {
+    if (!isEntity())
+        return;
+    getEntityMotionAccessor()->setWaterFlowEffectiveRate(rate);
+}
+
+float RigidBody::getWaterFlowEffectiveRate() const {
+    if (!isEntity())
+        return 0.0;
+    return getEntityMotionAccessor()->getWaterFlowEffectiveRate();
+}
+
+void RigidBody::setMagneMassScalingFactor(float factor) {
+    if (!isEntity())
+        return;
+    getEntityMotionAccessor()->setMagneMassScalingFactor(factor);
+}
+
+float RigidBody::getMagneMassScalingFactor() const {
+    if (!isEntity())
+        return 0.0;
+    return getEntityMotionAccessor()->getMagneMassScalingFactor();
+}
+
+void RigidBody::setFrictionScale(float scale) {
+    if (!isEntity())
+        return;
+    getEntityMotionAccessor()->setFrictionScale(scale);
+}
+
+float RigidBody::getFrictionScale() const {
+    if (!isEntity() || !mMotionAccessor)
+        return 1.0;
+    return getEntityMotionAccessor()->getFrictionScale();
+}
+
+void RigidBody::setRestitutionScale(float scale) {
+    if (!isEntity())
+        return;
+    scale = sead::Mathf::clamp(scale, 0.0, 1.0);
+    getEntityMotionAccessor()->setRestitutionScale(scale);
+}
+
+float RigidBody::getRestitutionScale() const {
+    if (!isEntity() || !mMotionAccessor)
+        return 1.0;
+    return getEntityMotionAccessor()->getRestitutionScale();
+}
+
+float RigidBody::getEffectiveRestitutionScale() const {
+    if (hasFlag(Flag::_2000) || hasFlag(Flag::_4000) || hasFlag(Flag::_8000) ||
+        hasFlag(Flag::_10000)) {
+        return getRestitutionScale() * 0.5f;
+    }
+
+    return getRestitutionScale();
+}
+
+void RigidBody::setMaxImpulse(float max) {
+    if (!isEntity())
+        return;
+    getEntityMotionAccessor()->setMaxImpulse(max);
+}
+
+float RigidBody::getMaxImpulse() const {
+    if (!isEntity() || !mMotionAccessor)
+        return 1.0;
+    return getEntityMotionAccessor()->getMaxImpulse();
+}
+
+void RigidBody::clearEntityMotionFlag4(bool clear) {
+    if (!isEntity() || !mMotionAccessor)
+        return;
+    getEntityMotionAccessor()->changeFlag(RigidBodyMotionEntity::Flag::_4, !clear);
+}
+
+bool RigidBody::isEntityMotionFlag4Off() const {
+    if (!isEntity() || !mMotionAccessor)
+        return false;
+    return !getEntityMotionAccessor()->hasFlag(RigidBodyMotionEntity::Flag::_4);
+}
+
+void RigidBody::setEntityMotionFlag8(bool set) {
+    if (!isEntity() || !mMotionAccessor)
+        return;
+    getEntityMotionAccessor()->changeFlag(RigidBodyMotionEntity::Flag::_8, set);
+}
+
+bool RigidBody::isEntityMotionFlag8On() const {
+    if (!isEntity() || !mMotionAccessor)
+        return false;
+    return getEntityMotionAccessor()->hasFlag(RigidBodyMotionEntity::Flag::_8);
+}
+
+void RigidBody::clearEntityMotionFlag10(bool clear) {
+    if (!isEntity() || !mMotionAccessor)
+        return;
+    getEntityMotionAccessor()->changeFlag(RigidBodyMotionEntity::Flag::_10, !clear);
+}
+
+bool RigidBody::isEntityMotionFlag10Off() const {
+    if (!isEntity() || !mMotionAccessor)
+        return false;
+    return !getEntityMotionAccessor()->hasFlag(RigidBodyMotionEntity::Flag::_10);
+}
+
+void RigidBody::clearEntityMotionFlag20(bool clear) {
+    if (!isEntity() || !mMotionAccessor)
+        return;
+    getEntityMotionAccessor()->changeFlag(RigidBodyMotionEntity::Flag::_20, !clear);
+}
+
+bool RigidBody::isEntityMotionFlag20Off() const {
+    if (!isEntity() || !mMotionAccessor)
+        return false;
+    return !getEntityMotionAccessor()->hasFlag(RigidBodyMotionEntity::Flag::_20);
+}
+
+void RigidBody::setColImpulseScale(float scale) {
+    if (!isEntity())
+        return;
+    scale = sead::Mathf::max(scale, 0.0);
+    getEntityMotionAccessor()->setColImpulseScale(scale);
+}
+
+float RigidBody::getColImpulseScale() const {
+    if (!isEntity() || !mMotionAccessor)
+        return 1.0;
+    return getEntityMotionAccessor()->getColImpulseScale();
+}
+
+void RigidBody::resetPosition() {
+    // debug logging?
+    [[maybe_unused]] sead::Vector3f position = getPosition();
+    setPosition(sead::Vector3f::zero, true);
+}
+
+const char* RigidBody::getName() {
+    return mUserTag ? mUserTag->getName().cstr() : getHkBodyName().cstr();
+}
+
+void RigidBody::lock() {
+    mCS.lock();
+}
+
 void RigidBody::lock(bool also_lock_world) {
     if (also_lock_world)
         MemSystem::instance()->lockWorld(getLayerType());
-    mCS.lock();
+    lock();
 }
 
-void RigidBody::unlock(bool also_unlock_world) {
+void RigidBody::unlock() {
     mCS.unlock();
+}
+
+void RigidBody::unlock(bool also_unlock_world) {
+    unlock();
     if (also_unlock_world)
         MemSystem::instance()->unlockWorld(getLayerType());
 }
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBody.h b/src/KingSystem/Physics/RigidBody/physRigidBody.h
index 2726270a..b6013250 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBody.h
+++ b/src/KingSystem/Physics/RigidBody/physRigidBody.h
@@ -103,18 +103,10 @@ public:
     ~RigidBody() override;
 
     // FIXME: types and names
-    virtual void m4();
+    virtual float m4();
     virtual void m5();
     virtual void m6();
     virtual void m7();
-    virtual void m8(float);
-    // FIXME: should be pure
-    virtual void m9();
-    virtual void m10();
-    virtual void m11();
-    virtual void m12();
-    virtual void m13();
-    virtual const char* getName();
 
     bool initMotionAccessorForDynamicMotion(sead::Heap* heap);
     bool initMotionAccessor(const RigidBodyInstanceParam& param, sead::Heap* heap,
@@ -149,6 +141,9 @@ public:
     RigidBody* getLinkedRigidBody() const;
     /// Reset the linked rigid body if we have a sensor motion accessor.
     void resetLinkedRigidBody() const;
+    /// Set the linked rigid body. This can only be done for sensor rigid bodies.
+    bool setLinkedRigidBody(RigidBody* body);
+    bool isSensorMotionFlag40000Set() const;
 
     // 0x0000007100f8d840
     void x_8();
@@ -189,6 +184,7 @@ public:
     void sub_7100F8F9E8(ReceiverMask*, void*);
     void sub_7100F8FA44(ContactLayer, u32);
 
+    void setPosition(const sead::Vector3f& position, bool propagate_to_linked_motions);
     void getPosition(sead::Vector3f* position) const;
     sead::Vector3f getPosition() const;
 
@@ -210,7 +206,6 @@ public:
     void getAngularVelocity(sead::Vector3f* velocity) const;
     sead::Vector3f getAngularVelocity() const;
 
-    // 0x0000007100f91264
     void getPointVelocity(sead::Vector3f* velocity, const sead::Vector3f& point) const;
 
     // 0x0000007100f92b74
@@ -236,29 +231,58 @@ public:
 
     void setMass(float mass);
     float getMass() const;
-    // 0x0000007100f93498
     float getMassInv() const;
 
-    // 0x0000007100f93534
     void setInertiaLocal(const sead::Vector3f& inertia);
-    // 0x0000007100f935dc
     void getInertiaLocal(sead::Vector3f* inertia) const;
-    // 0x0000007100f9368c
     sead::Vector3f getInertiaLocal() const;
 
-    // 0x0000007100f93750
     void setLinearDamping(float value);
-    // 0x0000007100f93804
     float getLinearDamping() const;
-    // 0x0000007100f938a0
+
     void setAngularDamping(float value);
-    // 0x0000007100f93954
     float getAngularDamping() const;
-    // 0x0000007100f939f0
+
     void setGravityFactor(float value);
-    // 0x0000007100f93a9c
     float getGravityFactor() const;
 
+    virtual bool setTimeFactor(float value);
+    float getTimeFactor() const;
+
+    void setWaterBuoyancyScale(float scale);
+    float getWaterBuoyancyScale() const;
+
+    void setWaterFlowEffectiveRate(float rate);
+    float getWaterFlowEffectiveRate() const;
+
+    void setMagneMassScalingFactor(float factor);
+    float getMagneMassScalingFactor() const;
+
+    void setFrictionScale(float scale);
+    float getFrictionScale() const;
+
+    void setRestitutionScale(float scale);
+    float getRestitutionScale() const;
+    float getEffectiveRestitutionScale() const;
+
+    void setMaxImpulse(float max);
+    float getMaxImpulse() const;
+
+    void setColImpulseScale(float scale);
+    float getColImpulseScale() const;
+
+    void clearEntityMotionFlag4(bool clear);
+    bool isEntityMotionFlag4Off() const;
+
+    void setEntityMotionFlag8(bool set);
+    bool isEntityMotionFlag8On() const;
+
+    void clearEntityMotionFlag10(bool clear);
+    bool isEntityMotionFlag10Off() const;
+
+    void clearEntityMotionFlag20(bool clear);
+    bool isEntityMotionFlag20Off() const;
+
     bool isSensor() const { return mFlags.isOn(Flag::IsSensor); }
     bool isEntity() const { return !mFlags.isOn(Flag::IsSensor); }
     ContactLayerType getLayerType() const {
@@ -275,9 +299,9 @@ public:
     Type getType() const { return mType; }
     bool isCharacterControllerType() const { return mType == Type::CharacterController; }
 
-    // 0x0000007100f969a0
+    void lock();
     void lock(bool also_lock_world);
-    // 0x0000007100f969e8
+    void unlock();
     void unlock(bool also_unlock_world);
 
     hkpMotion* getMotion() const;
@@ -301,6 +325,14 @@ public:
         return ScopedLock(this, also_lock_world);
     }
 
+    // FIXME: should be pure
+    virtual void m9();
+    virtual void m10();
+    virtual void m11();
+    virtual float m12(float x, float y);
+    virtual void resetPosition();
+    virtual const char* getName();
+
 private:
     void createMotionAccessor(sead::Heap* heap);
     void onInvalidParameter(int code = 0);
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodyMotionEntity.cpp b/src/KingSystem/Physics/RigidBody/physRigidBodyMotionEntity.cpp
index c5f4657f..1ef30076 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBodyMotionEntity.cpp
+++ b/src/KingSystem/Physics/RigidBody/physRigidBodyMotionEntity.cpp
@@ -688,7 +688,7 @@ void RigidBodyMotionEntity::setImpulseEpsilon(float epsilon) {
     sImpulseEpsilon = epsilon;
 }
 
-void RigidBodyMotionEntity::setMaxImpulse(float max_impulse) {
+void RigidBodyMotionEntity::setDefaultMaxImpulse(float max_impulse) {
     sMaxImpulse = max_impulse;
 }
 
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodyMotionEntity.h b/src/KingSystem/Physics/RigidBody/physRigidBodyMotionEntity.h
index dba3e30b..f47437f0 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBodyMotionEntity.h
+++ b/src/KingSystem/Physics/RigidBody/physRigidBodyMotionEntity.h
@@ -20,6 +20,13 @@ public:
     enum class Flag {
         _1 = 1 << 0,
         _2 = 1 << 1,
+        _4 = 1 << 2,
+        _8 = 1 << 3,
+        _10 = 1 << 4,
+        _20 = 1 << 5,
+        _40 = 1 << 6,
+        _80 = 1 << 7,
+        _100 = 1 << 8,
         _200 = 1 << 9,
     };
 
@@ -79,6 +86,27 @@ public:
     void setGravityFactor(float value);
     float getGravityFactor() const;
 
+    float getWaterBuoyancyScale() const { return mWaterBuoyancyScale; }
+    void setWaterBuoyancyScale(float scale) { mWaterBuoyancyScale = scale; }
+
+    float getWaterFlowEffectiveRate() const { return mWaterFlowEffectiveRate; }
+    void setWaterFlowEffectiveRate(float rate) { mWaterFlowEffectiveRate = rate; }
+
+    float getMagneMassScalingFactor() const { return mMagneMassScalingFactor; }
+    void setMagneMassScalingFactor(float factor) { mMagneMassScalingFactor = factor; }
+
+    float getFrictionScale() const { return mFrictionScale; }
+    void setFrictionScale(float scale) { mFrictionScale = scale; }
+
+    float getRestitutionScale() const { return mRestitutionScale; }
+    void setRestitutionScale(float scale) { mRestitutionScale = scale; }
+
+    float getMaxImpulse() const { return mMaxImpulse; }
+    void setMaxImpulse(float max) { mMaxImpulse = max; }
+
+    float getColImpulseScale() const { return mColImpulseScale; }
+    void setColImpulseScale(float scale) { mColImpulseScale = scale; }
+
     void processUpdateFlags();
     void updateRigidBodyMotionExceptState();
     void updateRigidBodyMotionExceptStateAndVel();
@@ -89,8 +117,13 @@ public:
     void copyTransformToAllLinkedBodies();
     void copyMotionToAllLinkedBodies();
 
+    bool hasFlag(Flag flag) const { return mFlags.isOn(flag); }
+    void setFlag(Flag flag) { mFlags.set(flag); }
+    void resetFlag(Flag flag) { mFlags.reset(flag); }
+    void changeFlag(Flag flag, bool on) { mFlags.change(flag, on); }
+
     static void setImpulseEpsilon(float epsilon);
-    static void setMaxImpulse(float max_impulse);
+    static void setDefaultMaxImpulse(float max_impulse);
 
 private:
     hkpMotion*