/*
 * 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