ksys/phys: Start adding ModelBoneAccessor

Needs gsys::Model stuff before I can continue

5 files changed, 302 insertions, 0 deletions

diff --git a/src/KingSystem/Physics/CMakeLists.txt b/src/KingSystem/Physics/CMakeLists.txt
index 98c59485..878d9a80 100644
--- a/src/KingSystem/Physics/CMakeLists.txt
+++ b/src/KingSystem/Physics/CMakeLists.txt
@@ -24,6 +24,8 @@ target_sources(uking PRIVATE
 
   Rig/physBoneAccessor.cpp
   Rig/physBoneAccessor.h
+  Rig/physModelBoneAccessor.cpp
+  Rig/physModelBoneAccessor.h
 
   RigidBody/physEdgeRigidBodyParam.cpp
   RigidBody/physEdgeRigidBodyParam.h
diff --git a/src/KingSystem/Physics/Rig/physModelBoneAccessor.cpp b/src/KingSystem/Physics/Rig/physModelBoneAccessor.cpp
new file mode 100644
index 00000000..52be352a
--- /dev/null
+++ b/src/KingSystem/Physics/Rig/physModelBoneAccessor.cpp
@@ -0,0 +1,132 @@
+#include "KingSystem/Physics/Rig/physModelBoneAccessor.h"
+#include <Havok/Animation/Animation/Rig/hkaSkeleton.h>
+#include <Havok/Common/Base/Memory/Allocator/Malloc/hkMallocAllocator.h>
+#include <gsys/gsysModel.h>
+#include <gsys/gsysModelNW.h>
+#include <heap/seadExpHeap.h>
+#include "KingSystem/Utils/SafeDelete.h"
+
+namespace ksys::phys {
+
+namespace detail {
+
+class ModelSkeleton {
+public:
+    class Allocator : public hkMallocAllocator {
+    public:
+        void* blockAlloc(int numBytes) override;
+        void blockFree(void* p, int numBytes) override;
+        int getAllocatedSize(const void* obj, int nbytes) const override;
+
+        sead::Heap* mHeap{};
+    };
+
+    ModelSkeleton() = default;
+    virtual ~ModelSkeleton();
+
+    /// Construct a Havok skeleton from a gsys model.
+    bool constructFromModel(ModelBoneAccessor::ModelBoneFilter* bone_filter,
+                            const gsys::ModelNW& model_unit, sead::Heap* heap);
+
+    hkaSkeleton* mHavokSkeleton = nullptr;
+    u8* _10 = nullptr;
+    void* _18;
+    Allocator mHavokAllocator;
+};
+
+void* ModelSkeleton::Allocator::blockAlloc(int numBytes) {
+    return mHeap->alloc(numBytes, m_align);
+}
+
+void ModelSkeleton::Allocator::blockFree(void* p, int numBytes) {
+    mHeap->free(p);
+}
+
+int ModelSkeleton::Allocator::getAllocatedSize(const void* obj, int nbytes) const {
+    return static_cast<int>(
+        sead::DynamicCast<sead::ExpHeap>(mHeap)->getAllocatedSize(const_cast<void*>(obj)));
+}
+
+ModelSkeleton::~ModelSkeleton() {
+    if (mHavokSkeleton) {
+        mHavokSkeleton->m_bones._clearAndDeallocate(mHavokAllocator);
+        mHavokSkeleton->m_referencePose._clearAndDeallocate(mHavokAllocator);
+        mHavokSkeleton->m_parentIndices._clearAndDeallocate(mHavokAllocator);
+        mHavokSkeleton = nullptr;
+    }
+
+    if (_10)
+        util::safeDeleteArray(_10);
+}
+
+}  // namespace detail
+
+ModelBoneAccessor::ModelBoneAccessor() = default;
+
+ModelBoneAccessor::~ModelBoneAccessor() {
+    ModelBoneAccessor::finalize();
+}
+
+bool ModelBoneAccessor::init(const hkaSkeleton* skeleton, const gsys::Model* model,
+                             sead::Heap* heap) {
+    if (!skeleton || !model)
+        return false;
+
+    if (!BoneAccessor::init(skeleton, heap))
+        return false;
+
+    const int num_bones = skeleton->m_bones.getSize();
+
+    if (num_bones > 0)
+        mBoneAccessKeys.allocBufferAssert(num_bones, heap);
+
+    for (int i = 0; i < num_bones; ++i) {
+        sead::SafeString bone_name = skeleton->m_bones[i].m_name.cString();
+        int separator_index = bone_name.rfindIndex(":");
+        if (separator_index >= 0 && separator_index < bone_name.calcLength() - 1)
+            bone_name = bone_name.cstr() + separator_index + 1;
+
+        mBoneAccessKeys[i].key.search(model, bone_name);
+        mBoneAccessKeys[i]._38 = true;
+        mBoneAccessKeys[i]._39 = true;
+    }
+
+    mModel = model;
+    return true;
+}
+
+bool ModelBoneAccessor::init(const gsys::Model* model, int model_unit_index, sead::Heap* heap,
+                             ModelBoneAccessor::ModelBoneFilter* bone_filter) {
+    if (!model)
+        return false;
+
+    if (model_unit_index >= model->getUnits().size())
+        return false;
+
+    const gsys::ModelUnit* unit = model->getUnits()[model_unit_index]->mModelUnit;
+
+    const auto* nw_unit = sead::DynamicCast<const gsys::ModelNW>(unit);
+    if (!nw_unit)
+        return false;
+
+    mModelSkeleton = new (heap) detail::ModelSkeleton;
+    if (!mModelSkeleton)
+        return false;
+
+    if (!mModelSkeleton->constructFromModel(bone_filter, *nw_unit, heap)) {
+        delete mModelSkeleton;
+        mModelSkeleton = nullptr;
+        return false;
+    }
+
+    return init(mModelSkeleton->mHavokSkeleton, model, heap);
+}
+
+void ModelBoneAccessor::finalize() {
+    mBoneAccessKeys.freeBuffer();
+    BoneAccessor::finalize();
+    if (mModelSkeleton)
+        util::safeDelete(mModelSkeleton);
+}
+
+}  // namespace ksys::phys
diff --git a/src/KingSystem/Physics/Rig/physModelBoneAccessor.h b/src/KingSystem/Physics/Rig/physModelBoneAccessor.h
new file mode 100644
index 00000000..7cefd835
--- /dev/null
+++ b/src/KingSystem/Physics/Rig/physModelBoneAccessor.h
@@ -0,0 +1,51 @@
+#pragma once
+
+#include <container/seadBuffer.h>
+#include <gsys/gsysModelAccessKey.h>
+#include <math/seadVector.h>
+#include "KingSystem/Physics/Rig/physBoneAccessor.h"
+#include "KingSystem/Utils/BitSet.h"
+
+namespace gsys {
+class Model;
+class ModelUnit;
+}  // namespace gsys
+
+namespace ksys::phys {
+
+namespace detail {
+class ModelSkeleton;
+}
+
+class ModelBoneAccessor : public BoneAccessor {
+public:
+    class ModelBoneFilter {
+    public:
+        virtual void filter(util::BitSet<1024>* bones_to_keep,
+                            const gsys::ModelUnit& model_unit) = 0;
+    };
+
+    ModelBoneAccessor();
+    ~ModelBoneAccessor() override;
+
+    bool init(const hkaSkeleton* skeleton, const gsys::Model* model, sead::Heap* heap);
+
+    bool init(const gsys::Model* model, int model_unit_index, sead::Heap* heap,
+              ModelBoneFilter* bone_filter);
+
+    void finalize() override;
+
+protected:
+    struct BoneAccessKey {
+        gsys::BoneAccessKeyEx key;
+        bool _38;
+        bool _39;
+    };
+
+    const gsys::Model* mModel{};
+    sead::Buffer<BoneAccessKey> mBoneAccessKeys;
+    sead::Vector3f _38 = sead::Vector3f::zero;
+    detail::ModelSkeleton* mModelSkeleton{};
+};
+
+}  // namespace ksys::phys
diff --git a/src/KingSystem/Utils/BitSet.h b/src/KingSystem/Utils/BitSet.h
new file mode 100644
index 00000000..ad48fc3d
--- /dev/null
+++ b/src/KingSystem/Utils/BitSet.h
@@ -0,0 +1,115 @@
+#pragma once
+
+#include <array>
+#include <basis/seadTypes.h>
+#include <math/seadMathCalcCommon.h>
+#include <prim/seadBitFlag.h>
+
+namespace ksys::util {
+
+template <int N>
+class BitSet {
+public:
+    using Word = u32;
+
+    void makeAllZero() { mStorage.fill(0); }
+    void makeAllOne() { mStorage.fill(~Word(0)); }
+
+    Word& getWord(int bit);
+    const Word& getWord(int bit) const;
+
+    bool isZero() const;
+
+    void setBit(int bit);
+    void resetBit(int bit);
+    void changeBit(int bit, bool on);
+    void toggleBit(int bit);
+    bool isOnBit(int bit) const;
+    bool isOffBit(int bit) const;
+
+    /// Popcount.
+    int countOnBit() const;
+    int countRightOnBit(int bit) const;
+
+    static Word makeMask(int bit) { return 1u << (bit % BitsPerWord); }
+
+protected:
+    static constexpr u32 BitsPerWord = 8 * sizeof(Word);
+
+    static_assert(N % BitsPerWord == 0, "N must be a multiple of the number of bits per word");
+    std::array<Word, N / BitsPerWord> mStorage{};
+};
+
+template <int N>
+inline typename BitSet<N>::Word& BitSet<N>::getWord(int bit) {
+    return mStorage[bit / BitsPerWord];
+}
+
+template <int N>
+inline const typename BitSet<N>::Word& BitSet<N>::getWord(int bit) const {
+    return mStorage[bit / BitsPerWord];
+}
+
+template <int N>
+inline void BitSet<N>::setBit(int bit) {
+    getWord(bit) |= makeMask(bit);
+}
+
+template <int N>
+inline void BitSet<N>::resetBit(int bit) {
+    getWord(bit) &= ~makeMask(bit);
+}
+
+template <int N>
+inline void BitSet<N>::changeBit(int bit, bool on) {
+    if (on)
+        setBit(bit);
+    else
+        resetBit(bit);
+}
+
+template <int N>
+inline void BitSet<N>::toggleBit(int bit) {
+    getWord(bit) ^= makeMask(bit);
+}
+
+template <int N>
+inline bool BitSet<N>::isOnBit(int bit) const {
+    return (getWord(bit) & makeMask(bit)) != 0;
+}
+
+template <int N>
+inline bool BitSet<N>::isOffBit(int bit) const {
+    return !isOnBit(bit);
+}
+
+template <int N>
+inline bool BitSet<N>::isZero() const {
+    for (const Word word : mStorage) {
+        if (word != 0)
+            return false;
+    }
+    return true;
+}
+
+template <int N>
+inline int BitSet<N>::countOnBit() const {
+    int count = 0;
+    for (const Word word : mStorage) {
+        count += sead::BitFlagUtil::countOnBit(word);
+    }
+    return count;
+}
+
+template <int N>
+inline int BitSet<N>::countRightOnBit(int bit) const {
+    int count = 0;
+    const auto last_word_index = u32(bit / BitsPerWord);
+    for (u32 i = 0; i < last_word_index; ++i) {
+        count += sead::BitFlagUtil::countOnBit(mStorage[i]);
+    }
+    count += sead::BitFlagUtil::countRightOnBit(mStorage[last_word_index], bit % BitsPerWord);
+    return count;
+}
+
+}  // namespace ksys::util
diff --git a/src/KingSystem/Utils/CMakeLists.txt b/src/KingSystem/Utils/CMakeLists.txt
index 2f5085e9..ba280ccd 100644
--- a/src/KingSystem/Utils/CMakeLists.txt
+++ b/src/KingSystem/Utils/CMakeLists.txt
@@ -71,6 +71,8 @@ target_sources(uking PRIVATE
   Container/UniqueArrayPtr.h
 
   AtomicLongBitFlag.h
+  BitField.h
+  BitSet.h
   Debug.h
   FixedString.h
   HashUtil.h