ksys/phys: Add SkeletonMapper

data/uking_functions.csv

@@ -93721,12 +93721,12 @@ Address,Quality,Size,Name
 0x000000710122a954,O,000088,_ZN4ksys4phys19SupportBoneResource11SupportBoneD0Ev
 0x000000710122a9ac,U,000904,
 0x000000710122ad34,U,000628,
-0x000000710122afa8,U,000064,
-0x000000710122afe8,U,000076,
-0x000000710122b034,U,000096,
-0x000000710122b094,U,000280,
-0x000000710122b1ac,U,000116,
-0x000000710122b220,U,000108,
+0x000000710122afa8,O,000064,_ZN4ksys4phys14SkeletonMapperC1Ev
+0x000000710122afe8,O,000076,_ZN4ksys4phys14SkeletonMapperD1Ev
+0x000000710122b034,O,000096,_ZN4ksys4phys14SkeletonMapperD0Ev
+0x000000710122b094,O,000280,_ZN4ksys4phys14SkeletonMapper4initEP17hkaSkeletonMapperS3_PN4gsys5ModelEPN4sead4HeapE
+0x000000710122b1ac,O,000116,_ZN4ksys4phys14SkeletonMapper8mapPoseAEv
+0x000000710122b220,O,000108,_ZN4ksys4phys14SkeletonMapper8mapPoseBEv
 0x000000710122b28c,O,000028,_ZN4ksys4phys6detail13ModelSkeleton9Allocator10blockAllocEi
 0x000000710122b2a8,O,000016,_ZN4ksys4phys6detail13ModelSkeleton9Allocator9blockFreeEPvi
 0x000000710122b2b8,O,000152,_ZNK4ksys4phys6detail13ModelSkeleton9Allocator16getAllocatedSizeEPKvi

src/KingSystem/Physics/CMakeLists.txt

@@ -26,6 +26,8 @@ target_sources(uking PRIVATE
   Rig/physBoneAccessor.h
   Rig/physModelBoneAccessor.cpp
   Rig/physModelBoneAccessor.h
+  Rig/physSkeletonMapper.cpp
+  Rig/physSkeletonMapper.h
 
   RigidBody/physEdgeRigidBodyParam.cpp
   RigidBody/physEdgeRigidBodyParam.h

src/KingSystem/Physics/Rig/physSkeletonMapper.cpp

@@ -0,0 +1,75 @@
+#include "KingSystem/Physics/Rig/physSkeletonMapper.h"
+#include <Havok/Animation/Animation/Mapper/hkaSkeletonMapper.h>
+#include <Havok/Animation/Animation/Rig/hkaPose.h>
+
+namespace ksys::phys {
+
+SkeletonMapper::SkeletonMapper() = default;
+
+SkeletonMapper::~SkeletonMapper() {
+    mModelBoneAccessor.finalize();
+    mBoneAccessor.finalize();
+}
+
+bool SkeletonMapper::init(hkaSkeletonMapper* skeleton_mapper,
+                          hkaSkeletonMapper* model_skeleton_mapper, gsys::Model* model,
+                          sead::Heap* heap) {
+    if (!model || !skeleton_mapper || !model_skeleton_mapper)
+        return false;
+
+    hkaSkeleton* skel_a = skeleton_mapper->m_mapping.m_skeletonA.val();
+    if (skel_a != model_skeleton_mapper->m_mapping.m_skeletonB.val())
+        return false;
+
+    hkaSkeleton* skel_b = skeleton_mapper->m_mapping.m_skeletonB.val();
+    if (skel_b != model_skeleton_mapper->m_mapping.m_skeletonA.val())
+        return false;
+
+    const int num_bones_a = skel_a->m_bones.getSize();
+    const int num_bones_b = skel_b->m_bones.getSize();
+    if (num_bones_a <= 0 || num_bones_b <= 0)
+        return false;
+
+    const auto cleanup = [this] {
+        mModelBoneAccessor.finalize();
+        mBoneAccessor.finalize();
+        return false;
+    };
+
+    if (num_bones_a <= num_bones_b) {
+        mMapperA = model_skeleton_mapper;
+        mMapperB = skeleton_mapper;
+
+        if (!mModelBoneAccessor.init(skeleton_mapper->m_mapping.m_skeletonB.val(), model, heap) ||
+            !mBoneAccessor.init(skeleton_mapper->m_mapping.m_skeletonA.val(), heap)) {
+            return cleanup();
+        }
+    } else {
+        mMapperA = skeleton_mapper;
+        mMapperB = model_skeleton_mapper;
+
+        if (!mModelBoneAccessor.init(skeleton_mapper->m_mapping.m_skeletonA.val(), model, heap) ||
+            !mBoneAccessor.init(skeleton_mapper->m_mapping.m_skeletonB.val(), heap)) {
+            return cleanup();
+        }
+    }
+
+    return true;
+}
+
+void SkeletonMapper::mapPoseA() {
+    mBoneAccessor.resetPoseData();
+    mMapperA->mapPose(mModelBoneAccessor.getPose()->getSyncedPoseModelSpace().data(),
+                      mBoneAccessor.getPose()->getSkeleton()->m_referencePose.data(),
+                      mBoneAccessor.getPose()->accessUnsyncedPoseModelSpace().data(),
+                      hkaSkeletonMapper::REFERENCE_POSE);
+}
+
+void SkeletonMapper::mapPoseB() {
+    const auto* original = mModelBoneAccessor.getPose()->getSyncedPoseLocalSpace().data();
+    auto* out = mModelBoneAccessor.getPose()->accessUnsyncedPoseModelSpace().data();
+    mMapperB->mapPose(mBoneAccessor.getPose()->getSyncedPoseModelSpace().data(), original, out,
+                      hkaSkeletonMapper::NO_CONSTRAINTS);
+}
+
+}  // namespace ksys::phys

src/KingSystem/Physics/Rig/physSkeletonMapper.h

@@ -0,0 +1,29 @@
+#pragma once
+
+#include <hostio/seadHostIONode.h>
+#include "KingSystem/Physics/Rig/physBoneAccessor.h"
+#include "KingSystem/Physics/Rig/physModelBoneAccessor.h"
+
+class hkaSkeletonMapper;
+
+namespace ksys::phys {
+
+class SkeletonMapper : public sead::hostio::Node {
+public:
+    SkeletonMapper();
+    virtual ~SkeletonMapper();
+
+    bool init(hkaSkeletonMapper* skeleton_mapper, hkaSkeletonMapper* model_skeleton_mapper,
+              gsys::Model* model, sead::Heap* heap);
+
+    void mapPoseA();
+    void mapPoseB();
+
+private:
+    BoneAccessor mBoneAccessor;
+    ModelBoneAccessor mModelBoneAccessor;
+    hkaSkeletonMapper* mMapperA{};
+    hkaSkeletonMapper* mMapperB{};
+};
+
+}  // namespace ksys::phys