From 420695a42b19d2c18a61ed43886789f303385820 Mon Sep 17 00:00:00 2001
From: lisyarus <lisyarus@gmail.com>
Date: Thu, 8 Oct 2020 10:56:55 +0300
Subject: [PATCH] Add slerp implementation for vectors

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

diff --git a/libs/geom/include/psemek/geom/vector.hpp b/libs/geom/include/psemek/geom/vector.hpp
index 247e5fb6..154521f7 100644
--- a/libs/geom/include/psemek/geom/vector.hpp
+++ b/libs/geom/include/psemek/geom/vector.hpp
@@ -290,6 +290,29 @@ namespace psemek::geom
 		return v0 * (T(1) - t) + v1 * t;
 	}
 
+	template <typename T, std::size_t N>
+	vector<T, N> slerp(vector<T, N> const & v0, vector<T, N> const & v1, 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 omega = std::acos(dot(normalized(v0), normalized(v1)));
+
+		// 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 normalized(lerp(v0, v1, t));
+		}
+		else
+		{
+			auto const s = std::sin(omega);
+			return v0 * std::sin((1 - t) * omega) / s + v1 * std::sin(t * omega) / s;
+		}
+	}
+
 	template <typename Stream, typename T, std::size_t N>
 	Stream & operator << (Stream & os, vector<T, N> const & v)
 	{