ksys/phys: Start adding RigidBodyMotionProxy

src/KingSystem/Physics/CMakeLists.txt

@@ -23,6 +23,10 @@ target_sources(uking PRIVATE
   RigidBody/physRigidBodyAccessor.h
   RigidBody/physRigidBodyFactory.cpp
   RigidBody/physRigidBodyFactory.h
+  RigidBody/physRigidBodyMotion.cpp
+  RigidBody/physRigidBodyMotion.h
+  RigidBody/physRigidBodyMotionProxy.cpp
+  RigidBody/physRigidBodyMotionProxy.h
   RigidBody/physRigidBodyParam.cpp
   RigidBody/physRigidBodyParam.h
   RigidBody/physRigidBodyResource.cpp

src/KingSystem/Physics/RigidBody/physMotionAccessor.h

@@ -1,17 +1,23 @@
 #pragma once
 
 #include <basis/seadTypes.h>
+#include <math/seadMatrix.h>
+#include <math/seadQuat.h>
+#include <math/seadVector.h>
 #include "KingSystem/Physics/RigidBody/physRigidBody.h"
 
+class hkQuaternionf;
+class hkVector4f;
 class hkpMotion;
 
 namespace ksys::phys {
 
+struct RigidBodyInstanceParam;
+
 class MotionAccessor {
     SEAD_RTTI_BASE(MotionAccessor)
 public:
     explicit MotionAccessor(RigidBody* body);
-    virtual ~MotionAccessor();
 
     MotionType getMotionInfo() const;
     hkpMotion* getMotion() const;
@@ -20,9 +26,40 @@ public:
     bool hasMotionFlagDisabled(RigidBody::MotionFlag flag) const;
     void disableMotionFlag(RigidBody::MotionFlag flag);
 
-private:
-    RigidBody* mBody;
-    void* _10 = nullptr;
+    virtual void setTransform(const sead::Matrix34f& mtx, bool notify) = 0;
+    virtual void setPosition(const sead::Vector3f& position, bool notify) = 0;
+    virtual void getPosition(sead::Vector3f* position) = 0;
+    virtual void getRotation(sead::Quatf* rotation) = 0;
+    virtual void getTransform(sead::Matrix34f* mtx) = 0;
+
+    virtual void setCenterOfMassInLocal(const sead::Vector3f& center) = 0;
+    virtual void getCenterOfMassInLocal(sead::Vector3f* center) = 0;
+
+    virtual bool setLinearVelocity(const sead::Vector3f& velocity, float epsilon) = 0;
+    virtual bool setLinearVelocity(const hkVector4f& velocity, float epsilon) = 0;
+    virtual void getLinearVelocity(sead::Vector3f* velocity) = 0;
+    virtual bool setAngularVelocity(const sead::Vector3f& velocity, float epsilon) = 0;
+    virtual bool setAngularVelocity(const hkVector4f& velocity, float epsilon) = 0;
+    virtual void getAngularVelocity(sead::Vector3f* velocity) = 0;
+
+    virtual void setMaxLinearVelocity(float max) = 0;
+    virtual float getMaxLinearVelocity() = 0;
+    virtual void setMaxAngularVelocity(float max) = 0;
+    virtual float getMaxAngularVelocity() = 0;
+
+    virtual ~MotionAccessor();
+
+    virtual bool init(const RigidBodyInstanceParam& params, sead::Heap* heap) = 0;
+    virtual void getRotation(hkQuaternionf* quat) = 0;
+    virtual void setTimeFactor(float factor) = 0;
+    virtual float getTimeFactor() = 0;
+    virtual void m25(bool a, bool b, bool c) = 0;
+    virtual void m26() = 0;
+
+protected:
+    RigidBody* mBody = nullptr;
+    u32 _10 = 0;
+    u32 _14 = 0;
 };
 
 }  // namespace ksys::phys

src/KingSystem/Physics/RigidBody/physRigidBody.cpp

@@ -49,15 +49,15 @@ bool RigidBody::isActive() const {
     return mHkBody->isActive();
 }
 
-bool RigidBody::sub_7100F8D1F8() const {
+bool RigidBody::isFlag8Set() const {
     return mFlags.isOn(Flag::_8);
 }
 
-bool RigidBody::sub_7100F8D204() const {
+bool RigidBody::isMotionFlag1Set() const {
     return mMotionFlags.isOn(MotionFlag::_1);
 }
 
-bool RigidBody::sub_7100F8D210() const {
+bool RigidBody::isMotionFlag2Set() const {
     return mMotionFlags.isOn(MotionFlag::_2);
 }
 

src/KingSystem/Physics/RigidBody/physRigidBody.h

@@ -47,6 +47,12 @@ public:
         Keyframed = 1 << 3,
         Fixed = 1 << 4,
         _20 = 1 << 5,
+        _40 = 1 << 6,
+        _80 = 1 << 7,
+        _100 = 1 << 8,
+        _200 = 1 << 9,
+        _400 = 1 << 10,
+        _800 = 1 << 11,
     };
 
     RigidBody(u32 a, u32 mass_scaling, hkpRigidBody* hk_body, const sead::SafeString& name,
@@ -84,11 +90,11 @@ public:
     bool isActive() const;
 
     // 0x0000007100f8d1f8
-    bool sub_7100F8D1F8() const;
+    bool isFlag8Set() const;
     // 0x0000007100f8d204
-    bool sub_7100F8D204() const;
+    bool isMotionFlag1Set() const;
     // 0x0000007100f8d210
-    bool sub_7100F8D210() const;
+    bool isMotionFlag2Set() const;
     // 0x0000007100f8d21c
     void sub_7100F8D21C();
     // 0x0000007100f8d308
@@ -149,6 +155,8 @@ public:
     const auto& getMotionFlags() const { return mMotionFlags; }
     void resetMotionFlagDirect(const MotionFlag flag) { mMotionFlags.reset(flag); }
 
+    hkpRigidBody* getHkBody() const { return mHkBody; }
+
 private:
     sead::CriticalSection mCS;
     sead::TypedBitFlag<Flag, sead::Atomic<u32>> mFlags{};

src/KingSystem/Physics/RigidBody/physRigidBodyMotion.cpp

@@ -0,0 +1 @@
+#include "KingSystem/Physics/RigidBody/physRigidBodyMotion.h"

src/KingSystem/Physics/RigidBody/physRigidBodyMotion.h

@@ -0,0 +1,19 @@
+#pragma once
+
+#include "KingSystem/Physics/RigidBody/physMotionAccessor.h"
+
+namespace ksys::phys {
+
+class RigidBodyMotion : public MotionAccessor {
+    SEAD_RTTI_OVERRIDE(RigidBodyMotion, MotionAccessor)
+public:
+    explicit RigidBodyMotion(RigidBody* body);
+
+    bool applyLinearImpulse(const sead::Vector3f& impulse);
+    bool applyAngularImpulse(const sead::Vector3f& impulse);
+    bool applyPointImpulse(const sead::Vector3f& impulse, const sead::Vector3f& point);
+
+    void setMass(float mass);
+};
+
+}  // namespace ksys::phys

src/KingSystem/Physics/RigidBody/physRigidBodyMotionProxy.cpp

@@ -0,0 +1,186 @@
+#include "KingSystem/Physics/RigidBody/physRigidBodyMotionProxy.h"
+#include <Havok/Common/Base/hkBase.h>
+#include <Havok/Physics2012/Dynamics/Entity/hkpRigidBody.h>
+#include "KingSystem/Physics/physConversions.h"
+
+namespace ksys::phys {
+
+RigidBodyMotionProxy::RigidBodyMotionProxy(RigidBody* body) : MotionAccessor(body) {}
+
+bool RigidBodyMotionProxy::init(const RigidBodyInstanceParam& params, sead::Heap* heap) {
+    mMaxLinearVelocity = params.max_linear_velocity;
+    mMaxAngularVelocity = params.max_angular_velocity_rad;
+    mTimeFactor = params.time_factor;
+    return true;
+}
+
+KSYS_ALWAYS_INLINE void RigidBodyMotionProxy::setTransformImpl(const sead::Matrix34f& mtx) {
+    if (mBody->isFlag8Set()) {  // flag 8 = block updates?
+        setMotionFlag(RigidBody::MotionFlag::_20);
+        return;
+    }
+
+    hkTransformf transform;
+    toHkTransform(&transform, mtx);
+    mBody->getHkBody()->getMotion()->setTransform(transform);
+}
+
+void RigidBodyMotionProxy::setTransform(const sead::Matrix34f& mtx, bool notify) {
+    mTransform = mtx;
+    setTransformImpl(mtx);
+}
+
+void RigidBodyMotionProxy::setPosition(const sead::Vector3f& position, bool notify) {
+    if (hasMotionFlagDisabled(RigidBody::MotionFlag::_20)) {
+        getTransform(&mTransform);
+    }
+
+    mTransform.setTranslation(position);
+    setTransformImpl(mTransform);
+}
+
+void RigidBodyMotionProxy::getPosition(sead::Vector3f* position) {
+    if (hasMotionFlagSet(RigidBody::MotionFlag::_20)) {
+        mTransform.getTranslation(*position);
+    } else {
+        const auto hk_position = getMotion()->getPosition();
+        storeToVec3(position, hk_position);
+    }
+}
+
+void RigidBodyMotionProxy::getRotation(sead::Quatf* rotation) {
+    if (hasMotionFlagSet(RigidBody::MotionFlag::_20)) {
+        mTransform.toQuat(*rotation);
+    } else {
+        toQuat(rotation, getMotion()->getRotation());
+    }
+
+    rotation->normalize();
+}
+
+void RigidBodyMotionProxy::getTransform(sead::Matrix34f* mtx) {
+    if (hasMotionFlagSet(RigidBody::MotionFlag::_20)) {
+        *mtx = mTransform;
+    } else {
+        const auto& transform = getMotion()->getTransform();
+        setMtxRotation(mtx, transform.getRotation());
+        setMtxTranslation(mtx, transform.getTranslation());
+    }
+}
+
+void RigidBodyMotionProxy::setCenterOfMassInLocal(const sead::Vector3f& center) {
+    mCenterOfMassInLocal.e = center.e;
+
+    if (mBody->isFlag8Set()) {
+        setMotionFlag(RigidBody::MotionFlag::_800);
+        return;
+    }
+
+    mBody->getHkBody()->setCenterOfMassLocal(toHkVec4(center));
+}
+
+void RigidBodyMotionProxy::getCenterOfMassInLocal(sead::Vector3f* center) {
+    if (hasMotionFlagSet(RigidBody::MotionFlag::_800)) {
+        center->e = mCenterOfMassInLocal.e;
+    } else {
+        const auto hk_center = getMotion()->getCenterOfMassLocal();
+        storeToVec3(center, hk_center);
+    }
+}
+
+bool RigidBodyMotionProxy::setLinearVelocity(const sead::Vector3f& velocity, float epsilon) {
+    if (velocity.equals(mLinearVelocity, epsilon))
+        return false;
+
+    mLinearVelocity.e = velocity.e;
+    setMotionFlag(RigidBody::MotionFlag::_40);
+    return true;
+}
+
+bool RigidBodyMotionProxy::setLinearVelocity(const hkVector4f& velocity, float epsilon) {
+    sead::Vector3f vec;
+    storeToVec3(&vec, velocity);
+    return setLinearVelocity(vec, epsilon);
+}
+
+void RigidBodyMotionProxy::getLinearVelocity(sead::Vector3f* velocity) {
+    if (hasMotionFlagSet(RigidBody::MotionFlag::_40)) {
+        velocity->e = mLinearVelocity.e;
+    } else {
+        const auto hk_velocity = getMotion()->getLinearVelocity();
+        storeToVec3(velocity, hk_velocity);
+    }
+}
+
+bool RigidBodyMotionProxy::setAngularVelocity(const sead::Vector3f& velocity, float epsilon) {
+    if (velocity.equals(mAngularVelocity, sead::Mathf::epsilon()))
+        return false;
+
+    mAngularVelocity.e = velocity.e;
+    setMotionFlag(RigidBody::MotionFlag::_80);
+    return true;
+}
+
+bool RigidBodyMotionProxy::setAngularVelocity(const hkVector4f& velocity, float epsilon) {
+    sead::Vector3f vec;
+    storeToVec3(&vec, velocity);
+    return setAngularVelocity(vec, epsilon);
+}
+
+void RigidBodyMotionProxy::getAngularVelocity(sead::Vector3f* velocity) {
+    if (hasMotionFlagSet(RigidBody::MotionFlag::_80)) {
+        velocity->e = mAngularVelocity.e;
+    } else {
+        const auto hk_velocity = getMotion()->getAngularVelocity();
+        storeToVec3(velocity, hk_velocity);
+    }
+}
+
+void RigidBodyMotionProxy::setMaxLinearVelocity(float max) {
+    mMaxLinearVelocity = max;
+    setMotionFlag(RigidBody::MotionFlag::_100);
+}
+
+float RigidBodyMotionProxy::getMaxLinearVelocity() {
+    if (hasMotionFlagSet(RigidBody::MotionFlag::_100))
+        return mMaxLinearVelocity;
+
+    return getMotion()->getMotionState()->m_maxLinearVelocity;
+}
+
+void RigidBodyMotionProxy::setMaxAngularVelocity(float max) {
+    mMaxAngularVelocity = max;
+    setMotionFlag(RigidBody::MotionFlag::_100);
+}
+
+float RigidBodyMotionProxy::getMaxAngularVelocity() {
+    if (hasMotionFlagSet(RigidBody::MotionFlag::_100))
+        return mMaxAngularVelocity;
+
+    return getMotion()->getMotionState()->m_maxAngularVelocity;
+}
+
+RigidBody* RigidBodyMotionProxy::getLinkedRigidBody() const {
+    return mLinkedRigidBody;
+}
+
+bool RigidBodyMotionProxy::isFlag40000Set() const {
+    return mFlags.isOn(Flag::_40000);
+}
+
+void RigidBodyMotionProxy::getRotation(hkQuaternionf* quat) {
+    sead::Quatf rotation;
+    getRotation(&rotation);
+    toHkQuat(quat, rotation);
+}
+
+void RigidBodyMotionProxy::setTimeFactor(float factor) {
+    mTimeFactor = factor;
+    setMotionFlag(RigidBody::MotionFlag::_100);
+}
+
+float RigidBodyMotionProxy::getTimeFactor() {
+    return mTimeFactor;
+}
+
+}  // namespace ksys::phys

src/KingSystem/Physics/RigidBody/physRigidBodyMotionProxy.h

@@ -0,0 +1,80 @@
+#pragma once
+
+#include <prim/seadTypedBitFlag.h>
+#include <thread/seadAtomic.h>
+#include "KingSystem/Physics/RigidBody/physMotionAccessor.h"
+
+namespace ksys::phys {
+
+/// A MotionAccessor that uses the RigidBody's internal motion instance directly.
+class RigidBodyMotionProxy : public MotionAccessor {
+    SEAD_RTTI_OVERRIDE(RigidBodyMotionProxy, MotionAccessor)
+public:
+    enum class Flag {
+        _40000 = 1 << 18,
+    };
+
+    explicit RigidBodyMotionProxy(RigidBody* body);
+
+    void setTransform(const sead::Matrix34f& mtx, bool notify) override;
+    void setPosition(const sead::Vector3f& position, bool notify) override;
+    // 0x0000007100fa4318
+    void setTransformMaybe(const sead::Matrix34f& mtx);
+    // 0x0000007100fa4594
+    void setTransformMaybe(const hkVector4f& translate, const hkQuaternionf& rotate);
+    void getPosition(sead::Vector3f* position) override;
+    void getRotation(sead::Quatf* rotation) override;
+    void getTransform(sead::Matrix34f* mtx) override;
+
+    void setCenterOfMassInLocal(const sead::Vector3f& center) override;
+    void getCenterOfMassInLocal(sead::Vector3f* center) override;
+
+    bool setLinearVelocity(const sead::Vector3f& velocity, float epsilon) override;
+    bool setLinearVelocity(const hkVector4f& velocity, float epsilon) override;
+    void getLinearVelocity(sead::Vector3f* velocity) override;
+    bool setAngularVelocity(const sead::Vector3f& velocity, float epsilon) override;
+    bool setAngularVelocity(const hkVector4f& velocity, float epsilon) override;
+    void getAngularVelocity(sead::Vector3f* velocity) override;
+
+    void setMaxLinearVelocity(float max) override;
+    float getMaxLinearVelocity() override;
+    void setMaxAngularVelocity(float max) override;
+    float getMaxAngularVelocity() override;
+
+    // 0x0000007100fa4f48 - called from RigidBody, sets a secondary rigid body
+    void setLinkedRigidBody(RigidBody* body);
+    // 0x0000007100fa3f8c - called from RigidBody and inlined by the destructor
+    void resetLinkedRigidBody();
+    RigidBody* getLinkedRigidBody() const;
+    bool isFlag40000Set() const;
+    // 0x0000007100fa5058 - main update function? triggers shape, position, velocity updates
+    void update();
+
+    ~RigidBodyMotionProxy() override;
+
+    bool init(const RigidBodyInstanceParam& params, sead::Heap* heap) override;
+    void getRotation(hkQuaternionf* quat) override;
+    void setTimeFactor(float factor) override;
+    float getTimeFactor() override;
+    void m25(bool a, bool b, bool c) override;
+    void m26() override;
+
+private:
+    void setTransformImpl(const sead::Matrix34f& mtx);
+
+    sead::Vector3f _18 = sead::Vector3f::zero;
+    sead::Vector3f _24 = sead::Vector3f::zero;
+    sead::Vector3f mLinearVelocity = sead::Vector3f::zero;
+    float mMaxLinearVelocity{};
+    sead::Vector3f mAngularVelocity = sead::Vector3f::zero;
+    float mMaxAngularVelocity{};
+    sead::Vector3f mCenterOfMassInLocal = sead::Vector3f::zero;
+    RigidBody* mLinkedRigidBody{};
+    sead::Matrix34f mTransform = sead::Matrix34f::ident;
+    float mTimeFactor = 1.0f;
+    sead::Vector3f _9c = sead::Vector3f::zero;
+    sead::Quatf _a8 = sead::Quatf::unit;
+    sead::TypedBitFlag<Flag, sead::Atomic<u32>> mFlags{};
+};
+
+}  // namespace ksys::phys

src/KingSystem/Physics/RigidBody/physRigidBodyParam.h

@@ -58,7 +58,7 @@ public:
     float linear_damping = 0.0f;
     float angular_damping = 0.05f;
     f32 _3c = 1.0f;
-    f32 _40 = 1.0f;
+    f32 time_factor = 1.0f;
     float max_linear_velocity = 200.0f;
     float max_angular_velocity_rad = 200.0f;
     float max_impulse = -1.0f;

src/KingSystem/Physics/physConversions.h

@@ -17,6 +17,10 @@ inline void toVec3(sead::Vector3f* out, const hkVector4f& vec) {
     return {vec.getX(), vec.getY(), vec.getZ()};
 }
 
+inline void storeToVec3(sead::Vector3f* out, const hkVector4f& vec) {
+    vec.store<3>(out->e.data());
+}
+
 inline void toHkVec4(hkVector4f* out, const sead::Vector3f& vec) {
     out->set(vec.x, vec.y, vec.z);
 }
@@ -59,4 +63,27 @@ inline void toMtx34(sead::Matrix34f* out, const hkTransformf& transform) {
     mtx[2].store<4>(out->m[2]);
 }
 
+inline void toHkTransform(hkTransformf* out, const sead::Matrix34f& mtx) {
+    sead::Quatf rotate;
+    mtx.toQuat(rotate);
+    rotate.normalize();
+
+    sead::Vector3f translate;
+    mtx.getTranslation(translate);
+
+    out->set(toHkQuat(rotate), toHkVec4(translate));
+}
+
+// Consider using toMtx34 if you have an hkTransform and wish to set both rotation and translation.
+inline void setMtxRotation(sead::Matrix34f* mtx, const hkRotationf& rotation) {
+    mtx->setBase(0, toVec3(rotation.getColumn(0)));
+    mtx->setBase(1, toVec3(rotation.getColumn(1)));
+    mtx->setBase(2, toVec3(rotation.getColumn(2)));
+}
+
+// Consider using toMtx34 if you have an hkTransform and wish to set both rotation and translation.
+inline void setMtxTranslation(sead::Matrix34f* mtx, const hkVector4f& translation) {
+    mtx->setTranslation(toVec3(translation));
+}
+
 }  // namespace ksys::phys