Added slerp and updated quaternion class

src/algorithms/attitude/slerp.cpp

@@ -0,0 +1,36 @@
+/*
+ * slerp.cpp
+ *
+ *  Created on: Jun 19, 2024
+ *      Author: Dario
+ */
+
+#include <sta/math/algorithms/attitude/slerp.hpp>
+#include <sta/math/utils.hpp>
+
+namespace sta
+{
+	namespace math
+	{
+		Quaternion slerp(Quaternion q1, Quaternion q2, float alpha)
+		{
+			float dot = q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z;
+
+			if (dot < 0.0)
+			{
+				q1 = q1 * -1;
+				dot = -1;
+			}
+
+			float theta = std::acos(clip(dot, -1, 1));
+
+			if (theta < 0.00001)
+			{
+				return q1;
+			}
+
+			return (q1 * std::sin((1-alpha) * theta) + q2 * std::sin(alpha * theta)) * (1 / std::sin(theta));
+		}
+	}  // namespace math
+}  // namespace sta
+