ksys/phys: Add SkeletonMapper

3 files changed, 106 insertions, 0 deletions

diff --git a/src/KingSystem/Physics/CMakeLists.txt b/src/KingSystem/Physics/CMakeLists.txt
index 878d9a80..6a7506ec 100644
--- a/src/KingSystem/Physics/CMakeLists.txt
+++ b/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
diff --git a/src/KingSystem/Physics/Rig/physSkeletonMapper.cpp b/src/KingSystem/Physics/Rig/physSkeletonMapper.cpp
new file mode 100644
index 00000000..d554b7f7
--- /dev/null
+++ b/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
diff --git a/src/KingSystem/Physics/Rig/physSkeletonMapper.h b/src/KingSystem/Physics/Rig/physSkeletonMapper.h
new file mode 100644
index 00000000..04ebb16a
--- /dev/null
+++ b/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