ksys/phys: Finish ModelBoneAccessor

5 files changed, 47 insertions, 1 deletions

diff --git a/lib/gsys/include/gsys/gsysModel.h b/lib/gsys/include/gsys/gsysModel.h
index 7195a65a..7cc7769b 100644
--- a/lib/gsys/include/gsys/gsysModel.h
+++ b/lib/gsys/include/gsys/gsysModel.h
@@ -39,6 +39,11 @@ public:
     sead::PtrArray<ModelInfo>& getUnits() { return mUnitAccess; }
     const sead::PtrArray<ModelInfo>& getUnits() const { return mUnitAccess; }
 
+    const sead::Vector3f& getScale() const { return _88; }
+
+    void setBoneLocalMatrix(const BoneAccessKey& key, const sead::Matrix34f& matrix,
+                            const sead::Vector3f& scale);
+
     // For internal use.
     void add_(IModelAccesssHandle* handle) const;
     void remove_(IModelAccesssHandle* handle) const;
diff --git a/lib/gsys/include/gsys/gsysModelAccessKey.h b/lib/gsys/include/gsys/gsysModelAccessKey.h
index a6015ac7..7c0569e9 100644
--- a/lib/gsys/include/gsys/gsysModelAccessKey.h
+++ b/lib/gsys/include/gsys/gsysModelAccessKey.h
@@ -1,6 +1,7 @@
 #pragma once
 
 #include <container/seadListImpl.h>
+#include <prim/seadBitUtil.h>
 #include <prim/seadSafeString.h>
 
 namespace gsys {
@@ -36,6 +37,17 @@ struct BoneAccessKey {
         bone_index = -1;
     }
 
+    bool isValid() const { return model_unit_index != -1 && bone_index != -1; }
+
+    friend bool operator==(const BoneAccessKey& lhs, const BoneAccessKey& rhs) {
+        static_assert(sizeof(BoneAccessKey) == sizeof(u32));
+        return sead::BitUtil::bitCastPtr<u32>(&lhs) == sead::BitUtil::bitCastPtr<u32>(&rhs);
+    }
+
+    friend bool operator!=(const BoneAccessKey& lhs, const BoneAccessKey& rhs) {
+        return !operator==(lhs, rhs);
+    }
+
     s16 model_unit_index{};
     s16 bone_index{};
 };
@@ -48,6 +60,11 @@ public:
     using IModelAccesssHandle::search;
     bool search(const Model* p_model, const BoneAccessKey& key);
 
+    bool isValid() const { return mKey.isValid(); }
+
+    BoneAccessKey& getKey() { return mKey; }
+    const BoneAccessKey& getKey() const { return mKey; }
+
 protected:
     bool searchImpl_() override;
     void removeImpl_() override;
diff --git a/lib/gsys/include/gsys/gsysModelUnit.h b/lib/gsys/include/gsys/gsysModelUnit.h
index 518333b8..fb48587c 100644
--- a/lib/gsys/include/gsys/gsysModelUnit.h
+++ b/lib/gsys/include/gsys/gsysModelUnit.h
@@ -73,7 +73,7 @@ public:
                                     int bone_idx);
     virtual void setBoneLocalRTMatrix(const sead::Matrix34f& matrix, int bone_idx);
     virtual void setBoneLocalScale(const sead::Vector3f& scale, int bone_idx);
-    virtual void getBoneLocalMatrix(sead::Matrix34f* matrix, sead::Vector3f* pos,
+    virtual void getBoneLocalMatrix(sead::Matrix34f* matrix, sead::Vector3f* scale,
                                     int bone_idx) const;
     virtual void setBoneWorldMatrix(const sead::Matrix34f& matrix, int bone_idx);
     virtual void getBoneWorldMatrix(sead::Matrix34f* matrix, int bone_idx) const;
diff --git a/lib/hkStubs/Havok/Common/Base/Math/QsTransform/hkQsTransformf.h b/lib/hkStubs/Havok/Common/Base/Math/QsTransform/hkQsTransformf.h
index 0955f489..d9025bbb 100644
--- a/lib/hkStubs/Havok/Common/Base/Math/QsTransform/hkQsTransformf.h
+++ b/lib/hkStubs/Havok/Common/Base/Math/QsTransform/hkQsTransformf.h
@@ -36,6 +36,11 @@ public:
     void setFromTransform(const hkQTransformf& qt);
     void copyToTransform(hkTransformf& transformOut) const;
 
+    HK_FORCE_INLINE void set(hkVector4fParameter translation, hkQuaternionfParameter rotation,
+                             hkVector4fParameter scale);
+
+    HK_FORCE_INLINE void set(hkVector4fParameter translation, hkQuaternionfParameter rotation);
+
     HK_FORCE_INLINE void setInverse(const hkQsTransformf& t);
 
     HK_FORCE_INLINE void setMul(const hkQsTransformf& t1, const hkQsTransformf& t2);
@@ -51,6 +56,8 @@ public:
     /// this *= b
     HK_FORCE_INLINE void setMulEq(const hkQsTransformf& b);
 
+    void fastRenormalize();
+
     hkVector4f m_translation;
     hkQuaternionf m_rotation;
     hkVector4f m_scale;
diff --git a/lib/hkStubs/Havok/Common/Base/Math/QsTransform/hkQsTransformf.inl b/lib/hkStubs/Havok/Common/Base/Math/QsTransform/hkQsTransformf.inl
index 4d8401f2..92704928 100644
--- a/lib/hkStubs/Havok/Common/Base/Math/QsTransform/hkQsTransformf.inl
+++ b/lib/hkStubs/Havok/Common/Base/Math/QsTransform/hkQsTransformf.inl
@@ -48,6 +48,19 @@ inline void hkQsTransformf::setZero() {
     m_scale.setZero();
 }
 
+inline void hkQsTransformf::set(hkVector4fParameter translation, hkQuaternionfParameter rotation,
+                                hkVector4fParameter scale) {
+    m_translation = translation;
+    m_rotation = rotation;
+    m_scale = scale;
+}
+
+inline void hkQsTransformf::set(hkVector4fParameter translation, hkQuaternionfParameter rotation) {
+    m_translation = translation;
+    m_rotation = rotation;
+    m_scale = hkVector4f::getConstant<HK_QUADREAL_1>();
+}
+
 inline void hkQsTransformf::setInverse(const hkQsTransformf& t) {
     m_translation.setRotatedInverseDir(t.m_rotation, t.m_translation);
     m_translation.setNeg<4>(m_translation);
@@ -82,3 +95,7 @@ inline void hkQsTransformf::setMulMulInverse(const hkQsTransformf& t1, const hkQ
 inline void hkQsTransformf::setMulEq(const hkQsTransformf& b) {
     setMul(*this, b);
 }
+
+inline void hkQsTransformf::fastRenormalize() {
+    m_rotation.normalize();
+}