From 8d2f244d0db1296f99d57599c4d5d7aa1f1c45af Mon Sep 17 00:00:00 2001
From: lisyarus <lisyarus@gmail.com>
Date: Wed, 22 Jun 2022 18:04:39 +0300
Subject: [PATCH] Support gfx::compile armature pose ignoring local bone
 coordinate systems

---
 libs/gfx/include/psemek/gfx/armature.hpp | 28 +++++++++++++++++++-----
 1 file changed, 23 insertions(+), 5 deletions(-)

diff --git a/libs/gfx/include/psemek/gfx/armature.hpp b/libs/gfx/include/psemek/gfx/armature.hpp
index 36338c35..0364da51 100644
--- a/libs/gfx/include/psemek/gfx/armature.hpp
+++ b/libs/gfx/include/psemek/gfx/armature.hpp
@@ -118,9 +118,13 @@ namespace psemek::gfx
 			output[i] = lerp(pose1[i], pose2[i], t);
 	}
 
+	struct pose_compile_use_bone_axes_t {};
+	static const pose_compile_use_bone_axes_t use_bone_axes;
+
 	template <typename T, typename Pose, typename Bones>
-	void compile(Pose const & local_pose, Bones const & bones, bone_transform<T> const & transform, pose<T> & result)
+	void compile(Pose const & local_pose, Bones const & bones, bone_transform<T> const & transform, pose<T> & result, pose_compile_use_bone_axes_t)
 	{
+		assert(local_pose.size() == bones.size());
 		result.resize(local_pose.size());
 
 		for (std::size_t b = 0; b < bones.size(); ++b)
@@ -142,11 +146,25 @@ namespace psemek::gfx
 	}
 
 	template <typename T, typename Pose, typename Bones>
-	pose<T> compile(Pose const & local_pose, Bones const & bones, bone_transform<T> const & transform)
+	void compile(Pose const & local_pose, Bones const & bones, bone_transform<T> const & transform, pose<T> & result)
 	{
-		pose<T> result;
-		compile(local_pose, bones, transform, result);
-		return result;
+		assert(local_pose.size() == bones.size());
+		result.resize(local_pose.size());
+
+		for (std::size_t b = 0; b < bones.size(); ++b)
+		{
+			result[b] =
+				  gfx::bone_transform<float>{geom::quaternion<float>::identity(), 1.f, bones[b].offset}
+				* local_pose[b]
+				* gfx::bone_transform<float>{geom::quaternion<float>::identity(), 1.f, -bones[b].offset}
+				;
+
+			auto p = bones[b].parent;
+			if (p == gfx::bone::null)
+				result[b] = transform * result[b];
+			else
+				result[b] = result[p] * result[b];
+		}
 	}
 
 }