ksys/phys: Add BoneAccessor

data/uking_functions.csv

@@ -93748,22 +93748,22 @@ Address,Quality,Size,Name
 0x000000710122ca34,O,000008,_ZN17hkMemoryAllocator20getExtendedInterfaceEv
 0x000000710122ca3c,U,000716,
 0x000000710122cd08,U,000088,
-0x000000710122cd60,U,000028,
-0x000000710122cd7c,U,000020,
-0x000000710122cd90,U,000052,
-0x000000710122cdc4,U,000276,
-0x000000710122ced8,U,000320,
-0x000000710122d018,U,000020,
-0x000000710122d02c,U,000012,
-0x000000710122d038,U,000008,
-0x000000710122d040,U,000008,
-0x000000710122d048,U,000020,
-0x000000710122d05c,U,000384,
-0x000000710122d1dc,U,000676,
-0x000000710122d480,U,000552,
-0x000000710122d6a8,U,000804,
-0x000000710122d9cc,U,000044,
-0x000000710122d9f8,U,000284,
+0x000000710122cd60,O,000028,_ZN4ksys4phys12BoneAccessorC1Ev
+0x000000710122cd7c,O,000020,_ZN4ksys4phys12BoneAccessorD1Ev
+0x000000710122cd90,O,000052,_ZN4ksys4phys12BoneAccessorD0Ev
+0x000000710122cdc4,O,000276,_ZN4ksys4phys12BoneAccessor4initEPK11hkaSkeletonPN4sead4HeapE
+0x000000710122ced8,O,000320,_ZN4ksys4phys12BoneAccessor8finalizeEv
+0x000000710122d018,O,000020,_ZN4ksys4phys12BoneAccessor13resetPoseDataEv
+0x000000710122d02c,O,000012,_ZNK4ksys4phys12BoneAccessor11getSkeletonEv
+0x000000710122d038,O,000008,_ZNK4ksys4phys12BoneAccessor15getPoseInternalEv
+0x000000710122d040,O,000008,_ZNK4ksys4phys12BoneAccessor7getPoseEv
+0x000000710122d048,O,000020,_ZN4ksys4phys12BoneAccessor8setScaleEf
+0x000000710122d05c,O,000384,_ZNK4ksys4phys12BoneAccessor12getBoneIndexERKN4sead14SafeStringBaseIcEE
+0x000000710122d1dc,O,000676,_ZN4ksys4phys12BoneAccessor17setBoneLocalSpaceERKNS1_7IndicesERK14hkQsTransformf
+0x000000710122d480,O,000552,_ZNK4ksys4phys12BoneAccessor17getBoneLocalSpaceERKNS1_7IndicesE
+0x000000710122d6a8,m,000804,_ZN4ksys4phys12BoneAccessor17setBoneModelSpaceERKNS1_7IndicesERK14hkQsTransformfNS1_9PropagateE
+0x000000710122d9cc,O,000044,_ZNK4ksys4phys12BoneAccessor17getBoneModelSpaceERKNS1_7IndicesE
+0x000000710122d9f8,O,000284,_ZN4ksys4phys12BoneAccessor11getBoneNameERKN4sead14SafeStringBaseIcEE
 0x000000710122db14,U,000332,
 0x000000710122dc60,U,000072,
 0x000000710122dca8,O,000096,_ZN4ksys10KingEditor18SingletonDisposer_D1Ev

lib/hkStubs/CMakeLists.txt

@@ -7,6 +7,7 @@ add_library(hkStubs OBJECT
   Havok/Animation/Animation/Mapper/hkaSkeletonMapper.h
   Havok/Animation/Animation/Mapper/hkaSkeletonMapperData.h
   Havok/Animation/Animation/Motion/hkaAnimatedReferenceFrame.h
+  Havok/Animation/Animation/Playback/Utilities/hkaSampleAndCombineUtils.h
   Havok/Animation/Animation/Rig/hkaBone.h
   Havok/Animation/Animation/Rig/hkaBoneAttachment.h
   Havok/Animation/Animation/Rig/hkaSkeleton.h

lib/hkStubs/Havok/Animation/Animation/Playback/Utilities/hkaSampleAndCombineUtils.h

@@ -0,0 +1,9 @@
+#pragma once
+
+#include <Havok/Common/Base/hkBase.h>
+
+// TODO
+class hkaSampleAndCombineUtils {
+public:
+    static void scaleTranslations(hkReal scale, hkQsTransform* poseInOut, int numTransforms);
+};

src/KingSystem/Physics/CMakeLists.txt

@@ -22,6 +22,9 @@ target_sources(uking PRIVATE
   Ragdoll/physRagdollRigidBody.cpp
   Ragdoll/physRagdollRigidBody.h
 
+  Rig/physBoneAccessor.cpp
+  Rig/physBoneAccessor.h
+
   RigidBody/physEdgeRigidBodyParam.cpp
   RigidBody/physEdgeRigidBodyParam.h
   RigidBody/physMotionAccessor.cpp

src/KingSystem/Physics/Rig/physBoneAccessor.cpp

@@ -0,0 +1,116 @@
+#include "KingSystem/Physics/Rig/physBoneAccessor.h"
+#include <Havok/Animation/Animation/Playback/Utilities/hkaSampleAndCombineUtils.h>
+#include <Havok/Animation/Animation/Rig/hkaPose.h>
+#include <Havok/Animation/Animation/Rig/hkaSkeleton.h>
+#include <math/seadMathCalcCommon.h>
+#include <prim/seadMemUtil.h>
+
+namespace ksys::phys {
+
+BoneAccessor::BoneAccessor() = default;
+
+BoneAccessor::~BoneAccessor() {
+    BoneAccessor::finalize();
+}
+
+bool BoneAccessor::init(const hkaSkeleton* skeleton, sead::Heap* heap) {
+    static constexpr int Alignment = 0x20;
+
+    mPoseDataSize = sead::Mathu::roundUp(u32(hkaPose::getRequiredMemorySize(skeleton)), Alignment);
+    if (mPoseDataSize == 0)
+        return false;
+
+    mPoseData = new (heap, Alignment) u8[mPoseDataSize];
+    if (!mPoseData)
+        return false;
+
+    mPose = new hkaPose(skeleton, mPoseData);
+    if (!mPose)
+        return false;
+
+    mPose->setToReferencePose();
+    return true;
+}
+
+void BoneAccessor::finalize() {
+    if (!mPose)
+        return;
+
+    delete mPose;
+    mPose = nullptr;
+
+    delete[] mPoseData;
+    mPoseData = nullptr;
+}
+
+void BoneAccessor::resetPoseData() {
+    sead::MemUtil::fillZero(mPoseData, mPoseDataSize);
+}
+
+const hkaSkeleton* BoneAccessor::getSkeleton() const {
+    return mPose->getSkeleton();
+}
+
+hkaPose* BoneAccessor::getPoseInternal() const {
+    return mPose;
+}
+
+hkaPose* BoneAccessor::getPose() const {
+    return mPose;
+}
+
+void BoneAccessor::setScale(float scale) {
+    hkaSampleAndCombineUtils::scaleTranslations(
+        scale, const_cast<hkQsTransform*>(getSkeleton()->m_referencePose.data()),
+        getSkeleton()->m_bones.getSize());
+}
+
+BoneAccessor::Indices BoneAccessor::getBoneIndex(const sead::SafeString& name) const {
+    const sead::SafeString actual_name = getBoneName(name);
+
+    for (int i = 0, n = getSkeleton()->m_bones.getSize(); i < n; ++i) {
+        if (actual_name == getBoneName(getSkeleton()->m_bones[i].m_name.cString()))
+            return {0, static_cast<s16>(i)};
+    }
+
+    return {-1, -1};
+}
+
+void BoneAccessor::setBoneLocalSpace(const BoneAccessor::Indices& index,
+                                     const hkQsTransformf& bone) {
+    mPose->setBoneLocalSpace(index.havok_index, bone);
+}
+
+const hkQsTransformf& BoneAccessor::getBoneLocalSpace(const BoneAccessor::Indices& index) const {
+    return mPose->getBoneLocalSpace(index.havok_index);
+}
+
+// NON_MATCHING: regalloc
+void BoneAccessor::setBoneModelSpace(const BoneAccessor::Indices& index,
+                                     const hkQsTransformf& bone_model, Propagate propagate) {
+    const auto havok_index = index.havok_index;
+    mPose->setBoneModelSpace(havok_index, bone_model, hkaPose::PropagateOrNot(propagate));
+}
+
+const hkQsTransformf& BoneAccessor::getBoneModelSpace(const BoneAccessor::Indices& index) const {
+    return mPose->getBoneModelSpace(index.havok_index);
+}
+
+#ifdef MATCHING_HACK_NX_CLANG
+[[gnu::noinline]]
+#endif
+sead::SafeString
+BoneAccessor::getBoneName(const sead::SafeString& name) {
+    int substr_index = 0;
+
+    int separator_index = name.rfindIndex(":");
+    if (separator_index < 0) {
+        substr_index = 0;
+    } else if (separator_index < name.calcLength() - 1) {
+        substr_index = separator_index + 1;
+    }
+
+    return name.cstr() + substr_index;
+}
+
+}  // namespace ksys::phys

src/KingSystem/Physics/Rig/physBoneAccessor.h

@@ -0,0 +1,61 @@
+#pragma once
+
+#include <basis/seadTypes.h>
+#include <hostio/seadHostIONode.h>
+#include <prim/seadSafeString.h>
+
+class hkaPose;
+class hkaSkeleton;
+class hkQsTransformf;
+
+namespace sead {
+class Heap;
+}
+
+namespace ksys::phys {
+
+class BoneAccessor : public sead::hostio::Node {
+public:
+    enum class Propagate : bool { Yes = true, No = false };
+
+    struct Indices {
+        Indices(s16 unk_, s16 havok_index) : unk(unk_), havok_index(havok_index) {}
+
+        s16 unk;
+        s16 havok_index;
+    };
+
+    BoneAccessor();
+    virtual ~BoneAccessor();
+
+    bool init(const hkaSkeleton* skeleton, sead::Heap* heap);
+    virtual void finalize();
+
+    void resetPoseData();
+
+    const hkaSkeleton* getSkeleton() const;
+    hkaPose* getPoseInternal() const;
+    hkaPose* getPose() const;
+
+    void setScale(float scale);
+
+    void setBoneLocalSpace(const Indices& index, const hkQsTransformf& bone_local);
+    const hkQsTransformf& getBoneLocalSpace(const Indices& index) const;
+
+    void setBoneModelSpace(const Indices& index, const hkQsTransformf& bone_model,
+                           Propagate propagate);
+    const hkQsTransformf& getBoneModelSpace(const Indices& index) const;
+
+    Indices getBoneIndex(const sead::SafeString& name) const;
+
+protected:
+    /// Extracts the actual bone name from a complex bone name like "Link:Leg_1_R".
+    /// @example Link:Leg_1_R -> Leg_1_R, Head -> Head
+    static sead::SafeString getBoneName(const sead::SafeString& name);
+
+    hkaPose* mPose{};
+    u32 mPoseDataSize{};
+    u8* mPoseData{};
+};
+
+}  // namespace ksys::phys