From fc47633c42b681e3b2c4061a5ef6e757ae13f043 Mon Sep 17 00:00:00 2001
From: lisyarus <lisyarus@gmail.com>
Date: Tue, 13 Jul 2021 11:55:46 +0300
Subject: [PATCH] Add bone armature helpers

---
 libs/gfx/include/psemek/gfx/armature.hpp | 121 +++++++++++++++++++++++
 1 file changed, 121 insertions(+)
 create mode 100644 libs/gfx/include/psemek/gfx/armature.hpp

diff --git a/libs/gfx/include/psemek/gfx/armature.hpp b/libs/gfx/include/psemek/gfx/armature.hpp
new file mode 100644
index 00000000..1461713d
--- /dev/null
+++ b/libs/gfx/include/psemek/gfx/armature.hpp
@@ -0,0 +1,121 @@
+#pragma once
+
+#include <psemek/geom/vector.hpp>
+#include <psemek/geom/quaternion.hpp>
+#include <psemek/geom/matrix.hpp>
+#include <psemek/geom/affine_transform.hpp>
+#include <psemek/geom/rotation.hpp>
+
+#include <cstdint>
+
+namespace psemek::gfx
+{
+
+	struct bone
+	{
+		static constexpr std::uint32_t null = static_cast<std::uint32_t>(-1);
+
+		std::uint32_t parent = null;
+		geom::vector<float, 3> offset = geom::vector<float, 3>::zero();
+		geom::matrix<float, 3, 3> axes = geom::matrix<float, 3, 3>::identity();
+	};
+
+	template <typename T>
+	struct bone_transform
+	{
+		geom::quaternion<T> rotation;
+		T scale;
+		geom::vector<T, 3> translation;
+
+		static bone_transform identity();
+
+		static bone_transform from_rotation(geom::quaternion<T> const & rotation);
+		static bone_transform from_scale(T const & scale);
+		static bone_transform from_translation(geom::vector<T, 3> const & translation);
+
+		geom::matrix<T, 3, 4> matrix() const;
+	};
+
+	template <typename T>
+	bone_transform<T> bone_transform<T>::identity()
+	{
+		return {geom::quaternion<T>::identity(), static_cast<T>(1), geom::vector<T, 3>::zero()};
+	}
+
+	template <typename T>
+	bone_transform<T> bone_transform<T>::from_rotation(geom::quaternion<T> const & rotation)
+	{
+		return {rotation, static_cast<T>(1), geom::vector<T, 3>::zero()};
+	}
+
+	template <typename T>
+	bone_transform<T> bone_transform<T>::from_scale(T const & scale)
+	{
+		return {geom::quaternion<T>::identity(), scale, geom::vector<T, 3>::zero()};
+	}
+
+	template <typename T>
+	bone_transform<T> bone_transform<T>::from_translation(geom::vector<T, 3> const & translation)
+	{
+		return {geom::quaternion<T>::identity(), static_cast<T>(1), translation};
+	}
+
+	template <typename T>
+	geom::matrix<T, 3, 4> bone_transform<T>::matrix() const
+	{
+		return geom::affine_transform<T, 3, 3>(geom::quaternion_rotation<T>(geom::quaternion<T>(std::sqrt(scale) * rotation.coords)), translation).affine_matrix();
+	}
+
+	template <typename T>
+	bone_transform<T> operator * (bone_transform<T> const & m1, bone_transform<T> const & m2)
+	{
+		// (1, t1) * (S1, 0) * (R1, 0) * (1, t2) * (S2, 0) * (R2, 0) =
+		// (1, t1) * (S1, 0) * (1, R1 * t2) * (R1, 0) * (S2, 0) * (R2, 0) =
+		// (1, t1) * (1, S1 * R1 * t2) * (S1, 0) * (R1, 0) * (S2, 0) * (R2, 0) =
+		// (1, t1) * (1, S1 * R1 * t2) * (S1, 0) * (S2, 0) * (R1, 0) * (R2, 0) =
+		// (1, t1 + S1 * R1 * t2) * (S1 * S2, 0) * (R1 * R2, 0)
+
+		return bone_transform<T>{m1.rotation * m2.rotation, m1.scale * m2.scale, m1.translation + m1.scale * geom::rotate(m1.rotation, m2.translation)};
+	}
+
+	template <typename T>
+	bone_transform<T> lerp(bone_transform<T> const & m1, bone_transform<T> const & m2, T t)
+	{
+		return {geom::slerp(m1.rotation, m2.rotation, t), std::exp(geom::lerp(std::log(m1.scale), std::log(m2.scale), t)), geom::lerp(m1.translation, m2.translation, t)};
+	}
+
+	template <typename T>
+	using pose = std::vector<bone_transform<T>>;
+
+	template <typename T, typename Pose, typename Bones>
+	void compile(Pose const & local_pose, Bones const & bones, bone_transform<T> const & transform, pose<T> & result)
+	{
+		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}
+				* gfx::bone_transform<float>{geom::quaternion<float>::rotation(bones[b].axes), 1.f, {0.f, 0.f, 0.f}}
+				* local_pose[b]
+				* gfx::bone_transform<float>{geom::inverse(geom::quaternion<float>::rotation(bones[b].axes)), 1.f, {0.f, 0.f, 0.f}}
+				* 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];
+		}
+	}
+
+	template <typename T, typename Pose, typename Bones>
+	pose<T> compile(Pose const & local_pose, Bones const & bones, bone_transform<T> const & transform)
+	{
+		pose<T> result;
+		compile(local_pose, bones, transform, result);
+		return result;
+	}
+
+}