From b2012aaa77db62853be1d573e02a0025555e5f3e Mon Sep 17 00:00:00 2001
From: lisyarus <lisyarus@gmail.com>
Date: Mon, 25 Jan 2021 22:14:54 +0300
Subject: [PATCH] Add quaternion slerp

---
 libs/geom/include/psemek/geom/quaternion.hpp | 27 ++++++++++++++++++++
 1 file changed, 27 insertions(+)

diff --git a/libs/geom/include/psemek/geom/quaternion.hpp b/libs/geom/include/psemek/geom/quaternion.hpp
index 89b49175..ee4ba907 100644
--- a/libs/geom/include/psemek/geom/quaternion.hpp
+++ b/libs/geom/include/psemek/geom/quaternion.hpp
@@ -134,4 +134,31 @@ namespace psemek::geom
 		return {res[1], res[2], res[3]};
 	}
 
+	template <typename T>
+	quaternion<T> slerp(quaternion<T> const & q0, quaternion<T> const & q1, T const & t)
+	{
+		// threshold is chosen so that for abs(x) < threshold the second term in
+		// sin(x) Taylor series is less than the minimum value representable by T
+		static auto const threshold = std::pow(6 * std::numeric_limits<T>::min(), T{1}/T{3});
+
+		auto const d = clamp(dot(normalized(q0.coords), normalized(q1.coords)), {T(-1), T(1)});
+		auto const omega = std::acos(std::abs(d));
+
+		// NB: the case of omega ~ pi is ambiguous and isn't handled in any special way
+
+		if (std::abs(omega) < threshold)
+		{
+			// prevent division by zero
+			return quaternion<T>{normalized(lerp(q0.coords, q1.coords, t))};
+		}
+		else
+		{
+			auto const s = std::sin(omega);
+			auto const w0 = std::sin((1 - t) * omega) / s;
+			auto const w1 = std::sin(t * omega) / s * ((d > 0) ? 1 : -1);
+			return quaternion<T>{q0.coords * w0 + q1.coords * w1};
+		}
+	}
+
+
 }