ksys/phys: Start adding ModelBoneAccessor

Needs gsys::Model stuff before I can continue

data/uking_functions.csv

@@ -68434,21 +68434,21 @@ Address,Quality,Size,Name
 0x0000007100bf9c08,U,000080,_ZN4gsys5Model8pushBackEiPNS_12IModelRigObjE
 0x0000007100bf9c58,U,000080,_ZN4gsys5Model5eraseEiPNS_12IModelRigObjE
 0x0000007100bf9ca8,U,000076,
-0x0000007100bf9cf4,U,000020,
-0x0000007100bf9d08,U,000004,j__ZdlPv_764
-0x0000007100bf9d0c,U,000204,_ZN4gsys19IModelAccesssHandle6searchEPKNS_5ModelERKN4sead14SafeStringBaseIcEE
-0x0000007100bf9dd8,U,000112,_ZN4gsys19IModelAccesssHandle6removeEv
-0x0000007100bf9e48,U,000036,_ZN4gsys19IModelAccesssHandle6searchEv
-0x0000007100bf9e6c,U,000080,_ZN4gsys15BoneAccessKeyExC2Ev
-0x0000007100bf9ebc,U,000136,_ZN4gsys15BoneAccessKeyExD2Ev
-0x0000007100bf9f44,U,000124,_ZN4gsys15BoneAccessKeyExD0Ev
+0x0000007100bf9cf4,O,000020,_ZN4gsys19IModelAccesssHandleD1Ev
+0x0000007100bf9d08,O,000004,_ZN4gsys19IModelAccesssHandleD0Ev
+0x0000007100bf9d0c,O,000204,_ZN4gsys19IModelAccesssHandle6searchEPKNS_5ModelERKN4sead14SafeStringBaseIcEE
+0x0000007100bf9dd8,O,000112,_ZN4gsys19IModelAccesssHandle6removeEv
+0x0000007100bf9e48,O,000036,_ZN4gsys19IModelAccesssHandle6searchEv
+0x0000007100bf9e6c,m,000080,_ZN4gsys15BoneAccessKeyExC1Ev
+0x0000007100bf9ebc,O,000136,_ZN4gsys15BoneAccessKeyExD1Ev
+0x0000007100bf9f44,O,000124,_ZN4gsys15BoneAccessKeyExD0Ev
 0x0000007100bf9fc0,U,000096,_ZN4gsys15BoneAccessKeyEx6searchEPKNS_5ModelERKNS_13BoneAccessKeyE
 0x0000007100bfa020,U,000068,_ZN4gsys15BoneAccessKeyEx11searchImpl_Ev
 0x0000007100bfa064,U,000080,_ZN4gsys19MaterialAccessKeyExC1Ev
 0x0000007100bfa0b4,U,000136,_ZN4gsys19MaterialAccessKeyExD2Ev
 0x0000007100bfa13c,U,000124,
 0x0000007100bfa1b8,U,000068,
-0x0000007100bfa1fc,U,000012,
+0x0000007100bfa1fc,O,000012,_ZN4gsys15BoneAccessKeyEx11removeImpl_Ev
 0x0000007100bfa208,U,000012,
 0x0000007100bfa214,U,000016,_ZN4gsys14ModelAnimation9CreateArgC2Ev
 0x0000007100bfa224,U,000240,_ZN4gsys14ModelAnimation6createEPPNS_13ModelResourceEPPKN2nn3g3d8ResModelEiRKNS0_9CreateArgEPNS_5ModelEPNS0_12CreateResultEPN4sead4HeapESJ_
@@ -93727,24 +93727,24 @@ Address,Quality,Size,Name
 0x000000710122b094,U,000280,
 0x000000710122b1ac,U,000116,
 0x000000710122b220,U,000108,
-0x000000710122b28c,U,000028,
-0x000000710122b2a8,U,000016,
-0x000000710122b2b8,U,000152,
-0x000000710122b350,U,000296,
-0x000000710122b478,U,000036,
+0x000000710122b28c,O,000028,_ZN4ksys4phys6detail13ModelSkeleton9Allocator10blockAllocEi
+0x000000710122b2a8,O,000016,_ZN4ksys4phys6detail13ModelSkeleton9Allocator9blockFreeEPvi
+0x000000710122b2b8,O,000152,_ZNK4ksys4phys6detail13ModelSkeleton9Allocator16getAllocatedSizeEPKvi
+0x000000710122b350,O,000296,_ZN4ksys4phys6detail13ModelSkeletonD1Ev
+0x000000710122b478,O,000036,_ZN4ksys4phys6detail13ModelSkeletonD0Ev
 0x000000710122b49c,U,000812,
 0x000000710122b7c8,U,001048,
-0x000000710122bbe0,U,000088,
-0x000000710122bc38,U,000148,
-0x000000710122bccc,U,000156,
-0x000000710122bd68,U,000608,
-0x000000710122bfc8,U,000344,
-0x000000710122c120,U,000128,
+0x000000710122bbe0,O,000088,_ZN4ksys4phys17ModelBoneAccessorC1Ev
+0x000000710122bc38,O,000148,_ZN4ksys4phys17ModelBoneAccessorD1Ev
+0x000000710122bccc,O,000156,_ZN4ksys4phys17ModelBoneAccessorD0Ev
+0x000000710122bd68,O,000608,_ZN4ksys4phys17ModelBoneAccessor4initEPK11hkaSkeletonPKN4gsys5ModelEPN4sead4HeapE
+0x000000710122bfc8,O,000344,_ZN4ksys4phys17ModelBoneAccessor4initEPKN4gsys5ModelEiPN4sead4HeapEPNS1_15ModelBoneFilterE
+0x000000710122c120,O,000128,_ZN4ksys4phys17ModelBoneAccessor8finalizeEv
 0x000000710122c1a0,U,000092,
 0x000000710122c1fc,U,000088,
 0x000000710122c254,U,001028,
 0x000000710122c658,U,000984,
-0x000000710122ca30,U,000004,j_nullsub_5077
+0x000000710122ca30,O,000004,_ZN4ksys4phys6detail13ModelSkeleton9AllocatorD0Ev
 0x000000710122ca34,O,000008,_ZN17hkMemoryAllocator20getExtendedInterfaceEv
 0x000000710122ca3c,U,000716,
 0x000000710122cd08,U,000088,

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

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

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

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

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