ksys/phys: Add RigidBodyAccessor

8 files changed, 241 insertions, 13 deletions

diff --git a/src/KingSystem/Physics/CMakeLists.txt b/src/KingSystem/Physics/CMakeLists.txt
index 194fb98b..09d0f8db 100644
--- a/src/KingSystem/Physics/CMakeLists.txt
+++ b/src/KingSystem/Physics/CMakeLists.txt
@@ -19,6 +19,8 @@ target_sources(uking PRIVATE
   RigidBody/physMotionAccessor.h
   RigidBody/physRigidBody.cpp
   RigidBody/physRigidBody.h
+  RigidBody/physRigidBodyAccessor.cpp
+  RigidBody/physRigidBodyAccessor.h
   RigidBody/physRigidBodyFactory.cpp
   RigidBody/physRigidBodyFactory.h
   RigidBody/physRigidBodyParam.cpp
@@ -92,4 +94,6 @@ target_sources(uking PRIVATE
   System/physSystemData.h
   System/physUserTag.cpp
   System/physUserTag.h
+
+  physConversions.h
 )
diff --git a/src/KingSystem/Physics/RigidBody/Shape/physCapsuleShape.cpp b/src/KingSystem/Physics/RigidBody/Shape/physCapsuleShape.cpp
index 4eba6781..8db65e3a 100644
--- a/src/KingSystem/Physics/RigidBody/Shape/physCapsuleShape.cpp
+++ b/src/KingSystem/Physics/RigidBody/Shape/physCapsuleShape.cpp
@@ -2,6 +2,7 @@
 #include <Havok/Physics2012/Collide/Shape/Convex/Capsule/hkpCapsuleShape.h>
 #include <heap/seadHeap.h>
 #include <math/seadMathCalcCommon.h>
+#include "KingSystem/Physics/physConversions.h"
 
 namespace ksys::phys {
 
@@ -98,12 +99,12 @@ void CapsuleBody::sub_7100FABE80(sead::Vector3f* veca, sead::Vector3f* vecb,
                                  const hkTransformf& rb_vec) {
     if (veca != nullptr) {
         hkVector4 tmp;
-        tmp.setTransformedPos(rb_vec, hkVector4(vertex_a.x, vertex_a.y, vertex_a.z));
+        tmp.setTransformedPos(rb_vec, toHkVec4(vertex_a));
         tmp.store<3>(veca->e.data());
     }
     if (vecb != nullptr) {
         hkVector4 tmp;
-        tmp.setTransformedPos(rb_vec, hkVector4(vertex_b.x, vertex_b.y, vertex_b.z));
+        tmp.setTransformedPos(rb_vec, toHkVec4(vertex_b));
         tmp.store<3>(vecb->e.data());
     }
 }
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBody.cpp b/src/KingSystem/Physics/RigidBody/physRigidBody.cpp
index 1e42fb83..04fb2e3c 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBody.cpp
+++ b/src/KingSystem/Physics/RigidBody/physRigidBody.cpp
@@ -7,7 +7,7 @@ namespace ksys::phys {
 
 RigidBody::RigidBody(u32 a, u32 mass_scaling, hkpRigidBody* hk_body, const sead::SafeString& name,
                      sead::Heap* heap, bool a7)
-    : mCS(heap), mHkBody(hk_body), mHkBodyMgr(hk_body), _b4(a) {
+    : mCS(heap), mHkBody(hk_body), mRigidBodyAccessor(hk_body), _b4(a) {
     if (!name.isEmpty()) {
         mHkBody->setName(name.cstr());
     }
@@ -69,7 +69,7 @@ MotionType RigidBody::getMotionInfo() const {
         return MotionType::Keyframed;
     if (mMotionFlags.isOn(MotionFlag::Fixed))
         return MotionType::Fixed;
-    return mHkBodyMgr.getMotionInfo();
+    return mRigidBodyAccessor.getMotionType();
 }
 
 void RigidBody::setContactMask(u32 value) {
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBody.h b/src/KingSystem/Physics/RigidBody/physRigidBody.h
index 48e3736d..8c2de80d 100644
--- a/src/KingSystem/Physics/RigidBody/physRigidBody.h
+++ b/src/KingSystem/Physics/RigidBody/physRigidBody.h
@@ -6,6 +6,7 @@
 #include <prim/seadTypedBitFlag.h>
 #include <thread/seadAtomic.h>
 #include <thread/seadCriticalSection.h>
+#include "KingSystem/Physics/RigidBody/physRigidBodyAccessor.h"
 #include "KingSystem/Physics/RigidBody/physRigidBodyParam.h"
 #include "KingSystem/Physics/System/physDefines.h"
 #include "KingSystem/Utils/Types.h"
@@ -25,14 +26,6 @@ public:
 class RigidBody : public sead::IDisposer, public RigidBase {
     SEAD_RTTI_BASE(RigidBody)
 public:
-    struct HkBodyMgr {
-        explicit HkBodyMgr(hkpRigidBody* body);
-        virtual ~HkBodyMgr();
-        MotionType getMotionInfo() const;
-
-        void* p;
-    };
-
     enum class Flag1 {
         MassScaling = 1 << 0,
         _2 = 1 << 1,
@@ -107,7 +100,7 @@ private:
     void* _88 = nullptr;
     void* _90 = nullptr;
     u16 _98 = 0;
-    HkBodyMgr mHkBodyMgr;
+    RigidBodyAccessor mRigidBodyAccessor;
     f32 _b0 = 1.0f;
     u32 _b4 = 0;
     MotionAccessor* mMotionAccessor = nullptr;
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodyAccessor.cpp b/src/KingSystem/Physics/RigidBody/physRigidBodyAccessor.cpp
new file mode 100644
index 00000000..a68567b5
--- /dev/null
+++ b/src/KingSystem/Physics/RigidBody/physRigidBodyAccessor.cpp
@@ -0,0 +1,99 @@
+#include "KingSystem/Physics/RigidBody/physRigidBodyAccessor.h"
+#include <Havok/Physics2012/Dynamics/Entity/hkpRigidBody.h>
+#include "KingSystem/Physics/physConversions.h"
+
+namespace ksys::phys {
+
+RigidBodyAccessor::RigidBodyAccessor(hkpRigidBody* body) : mBody(body) {}
+
+RigidBodyAccessor::~RigidBodyAccessor() = default;
+
+MotionType RigidBodyAccessor::getMotionType() const {
+    switch (getBody()->getMotionType()) {
+    case hkpMotion::MOTION_INVALID:
+        break;
+    case hkpMotion::MOTION_DYNAMIC:
+    case hkpMotion::MOTION_SPHERE_INERTIA:
+    case hkpMotion::MOTION_BOX_INERTIA:
+        return MotionType::Dynamic;
+    case hkpMotion::MOTION_KEYFRAMED:
+        return MotionType::Keyframed;
+    case hkpMotion::MOTION_FIXED:
+        return MotionType::Fixed;
+    case hkpMotion::MOTION_THIN_BOX_INERTIA:
+        break;
+    case hkpMotion::MOTION_CHARACTER:
+        return MotionType::Dynamic;
+    case hkpMotion::MOTION_MAX_ID:
+        break;
+    }
+    return MotionType::Invalid;
+}
+
+void RigidBodyAccessor::getPosition(sead::Vector3f* pos) const {
+    toVec3(pos, getBody()->getPosition());
+}
+
+void RigidBodyAccessor::getRotation(sead::Quatf* rot) const {
+    toQuat(rot, getBody()->getRotation());
+}
+
+void RigidBodyAccessor::getTransform(sead::Matrix34f* mtx) const {
+    toMtx34(mtx, getBody()->getTransform());
+}
+
+void RigidBodyAccessor::getLinearVelocity(sead::Vector3f* vel) const {
+    toVec3(vel, getBody()->getLinearVelocity());
+}
+
+void RigidBodyAccessor::getAngularVelocity(sead::Vector3f* vel) const {
+    toVec3(vel, getBody()->getAngularVelocity());
+}
+
+bool RigidBodyAccessor::isVelocityGreaterEqual(float vel) const {
+    hkVector4f v;
+    v.setAll(vel);
+
+    hkVector4f linvel;
+    linvel.setAbs(getBody()->getLinearVelocity());
+
+    hkVector4f angvel;
+    angvel.setAbs(getBody()->getAngularVelocity());
+
+    return v.greaterEqual(linvel).allAreSet<hkVector4fComparison::MASK_XYZ>() &&
+           v.greaterEqual(angvel).allAreSet<hkVector4fComparison::MASK_XYZ>();
+}
+
+void RigidBodyAccessor::getPointVelocity(sead::Vector3f* vel, const sead::Vector3f& point) const {
+    const auto p = toHkVec4(point);
+    hkVector4f out;
+    getBody()->getPointVelocity(p, out);
+    toVec3(vel, out);
+}
+
+float RigidBodyAccessor::getTimeFactor() const {
+    return getBody()->getTimeFactor();
+}
+
+void RigidBodyAccessor::getDeltaCenterOfMass(sead::Vector3f* out_delta_pos,
+                                             sead::Vector3f* out_delta_angle) const {
+    const hkMotionState* state = getBody()->getMotion()->getMotionState();
+    const auto center1 = state->getSweptTransform().m_centerOfMass1;
+    const auto center0 = state->getSweptTransform().m_centerOfMass0;
+    const auto delta_angle = state->m_deltaAngle;
+
+    if (out_delta_pos != nullptr) {
+        hkVector4f value;
+        value.setSub(center1, center0);
+        value.mul(center1.getW());  // W is the time step
+        value.store<3>(out_delta_pos->e.data());
+    }
+
+    if (out_delta_angle != nullptr) {
+        hkVector4f value;
+        value.setMul(delta_angle, center1.getW());
+        value.store<3>(out_delta_angle->e.data());
+    }
+}
+
+}  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/RigidBody/physRigidBodyAccessor.h b/src/KingSystem/Physics/RigidBody/physRigidBodyAccessor.h
new file mode 100644
index 00000000..8e32ed40
--- /dev/null
+++ b/src/KingSystem/Physics/RigidBody/physRigidBodyAccessor.h
@@ -0,0 +1,44 @@
+#pragma once
+
+#include <math/seadMatrix.h>
+#include <math/seadQuat.h>
+#include <math/seadVector.h>
+#include "KingSystem/Physics/System/physDefines.h"
+
+class hkpRigidBody;
+
+namespace ksys::phys {
+
+class RigidBodyAccessor {
+public:
+    explicit RigidBodyAccessor(hkpRigidBody* body);
+    virtual ~RigidBodyAccessor();
+
+    hkpRigidBody* getBody() const { return mBody; }
+
+    MotionType getMotionType() const;
+
+    void getPosition(sead::Vector3f* pos) const;
+    void getRotation(sead::Quatf* rot) const;
+    void getTransform(sead::Matrix34f* mtx) const;
+
+    void getLinearVelocity(sead::Vector3f* vel) const;
+    void getAngularVelocity(sead::Vector3f* vel) const;
+
+    /// Whether `vel` >= abs(c) for each component c of the linear velocity and angular velocity.
+    // XXX: does this function even make sense? We're comparing `vel` against two quantities
+    // that have different units!
+    bool isVelocityGreaterEqual(float vel) const;
+
+    void getPointVelocity(sead::Vector3f* vel, const sead::Vector3f& point) const;
+
+    float getTimeFactor() const;
+
+    /// Both parameters are allowed to be null.
+    void getDeltaCenterOfMass(sead::Vector3f* out_delta_pos, sead::Vector3f* out_delta_angle) const;
+
+private:
+    hkpRigidBody* mBody;
+};
+
+}  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/System/physDefines.h b/src/KingSystem/Physics/System/physDefines.h
index 4c8d045b..96058c7c 100644
--- a/src/KingSystem/Physics/System/physDefines.h
+++ b/src/KingSystem/Physics/System/physDefines.h
@@ -171,6 +171,7 @@ enum class MotionType {
     Fixed = 1,
     Keyframed = 2,
     Unknown = 3,
+    Invalid = -1,
 };
 
 union ReceiverMask {
diff --git a/src/KingSystem/Physics/physConversions.h b/src/KingSystem/Physics/physConversions.h
new file mode 100644
index 00000000..48db1dd9
--- /dev/null
+++ b/src/KingSystem/Physics/physConversions.h
@@ -0,0 +1,86 @@
+#pragma once
+
+#include <Havok/Common/Base/hkBase.h>
+#include <math/seadMatrix.h>
+#include <math/seadQuat.h>
+#include <math/seadVector.h>
+
+#ifdef __aarch64__
+#include <arm_neon.h>
+#endif
+
+namespace ksys::phys {
+
+inline void toVec3(sead::Vector3f* out, const hkVector4f& vec) {
+    out->x = vec.getX();
+    out->y = vec.getY();
+    out->z = vec.getZ();
+}
+
+[[nodiscard]] inline sead::Vector3f toVec3(const hkVector4f& vec) {
+    return {vec.getX(), vec.getY(), vec.getZ()};
+}
+
+inline void toHkVec4(hkVector4f* out, const sead::Vector3f& vec) {
+    out->set(vec.x, vec.y, vec.z);
+}
+
+[[nodiscard]] inline hkVector4f toHkVec4(const sead::Vector3f& vec) {
+    return {vec.x, vec.y, vec.z};
+}
+
+inline void toQuat(sead::Quatf* out, const hkQuaternionf& quat) {
+    out->set(quat.m_vec.getW(), quat.m_vec.getX(), quat.m_vec.getY(), quat.m_vec.getZ());
+}
+
+[[nodiscard]] inline sead::Quatf toQuat(const hkQuaternionf& quat) {
+    return {quat.m_vec.getW(), quat.m_vec.getX(), quat.m_vec.getY(), quat.m_vec.getZ()};
+}
+
+inline void toHkQuat(hkQuaternionf* out, const sead::Quatf& quat) {
+    out->set(quat.x, quat.y, quat.x, quat.w);
+}
+
+[[nodiscard]] inline hkQuaternionf toHkQuat(const sead::Quatf& quat) {
+    return {quat.x, quat.y, quat.x, quat.w};
+}
+
+inline void toMtx34(sead::Matrix34f* out, const hkTransformf& transform) {
+    const hkRotationf& rotate = transform.getRotation();
+    const hkVector4f& translate = transform.getTranslation();
+
+    hkVector4f row0, row1, row2;
+
+#ifdef __aarch64__
+    // XXX: this leads to really poor codegen (compared to using getRows, which
+    // is optimised into Neon zip/transpose instructions). Is Nintendo to blame
+    // for this bad usage of Neon intrinsics, or did Havok mess up their Neon getRows?
+
+    row0.v = vld1q_lane_f32(&rotate(0, 0), row0.v, 0);
+    row1.v = vld1q_lane_f32(&rotate(1, 0), row1.v, 0);
+    row2.v = vld1q_lane_f32(&rotate(2, 0), row2.v, 0);
+
+    row0.v = vld1q_lane_f32(&rotate(0, 1), row0.v, 1);
+    row1.v = vld1q_lane_f32(&rotate(1, 1), row1.v, 1);
+    row2.v = vld1q_lane_f32(&rotate(2, 1), row2.v, 1);
+
+    row0.v = vld1q_lane_f32(&rotate(0, 2), row0.v, 2);
+    row1.v = vld1q_lane_f32(&rotate(1, 2), row1.v, 2);
+    row2.v = vld1q_lane_f32(&rotate(2, 2), row2.v, 2);
+
+    row0.v = vld1q_lane_f32(&translate(0), row0.v, 3);
+    row1.v = vld1q_lane_f32(&translate(1), row1.v, 3);
+    row2.v = vld1q_lane_f32(&translate(2), row2.v, 3);
+#else
+    rotate.getRows(row0, row1, row2);
+    row0[3] = translate[0];
+    row1[3] = translate[1];
+    row2[3] = translate[2];
+#endif
+
+    row0.store<4>(out->m[0]);
+    row1.store<4>(out->m[1]);
+    row2.store<4>(out->m[2]);
+}
+
+}  // namespace ksys::phys