mirror of
https://git.intern.spaceteamaachen.de/ALPAKA/sta-peak.git
synced 2025-12-16 10:08:03 +00:00
37 lines
634 B
C++
37 lines
634 B
C++
/*
|
|
* 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
|
|
|