109 lines
2.4 KiB
C++
109 lines
2.4 KiB
C++
#include <wrmath/geom/quaternion.h>
|
|
|
|
|
|
namespace wr {
|
|
namespace geom {
|
|
|
|
|
|
Quaternionf
|
|
quaternionf(Vector3f axis, float angle)
|
|
{
|
|
return Quaternionf(axis.unitVector() * std::sin(angle / 2.0),
|
|
std::cos(angle / 2.0));
|
|
}
|
|
|
|
|
|
Quaterniond
|
|
quaterniond(Vector3d axis, double angle)
|
|
{
|
|
return Quaterniond(axis.unitVector() * std::sin(angle / 2.0),
|
|
std::cos(angle / 2.0));
|
|
}
|
|
|
|
|
|
Quaternionf
|
|
quaternionf_from_euler(Vector3f euler)
|
|
{
|
|
float x, y, z, w;
|
|
euler = euler / 2.0;
|
|
|
|
float cos_yaw = std::cos(euler[0]);
|
|
float cos_pitch = std::cos(euler[1]);
|
|
float cos_roll = std::cos(euler[2]);
|
|
float sin_yaw = std::sin(euler[0]);
|
|
float sin_pitch = std::sin(euler[1]);
|
|
float sin_roll = std::sin(euler[2]);
|
|
|
|
x = (sin_yaw * cos_pitch * cos_roll) + (cos_yaw * sin_pitch * sin_roll);
|
|
y = (sin_yaw * cos_pitch * sin_roll) - (cos_yaw * sin_pitch * cos_roll);
|
|
z = (cos_yaw * cos_pitch * sin_roll) + (sin_yaw * sin_pitch * cos_roll);
|
|
w = (cos_yaw * cos_pitch * cos_roll) - (sin_yaw * sin_pitch * sin_roll);
|
|
|
|
return Quaternionf(Vector4f{x, y, z, w});
|
|
}
|
|
|
|
|
|
Quaterniond
|
|
quaterniond_from_euler(Vector3d euler)
|
|
{
|
|
double x, y, z, w;
|
|
euler = euler / 2.0;
|
|
|
|
double cos_yaw = std::cos(euler[0]);
|
|
double cos_pitch = std::cos(euler[1]);
|
|
double cos_roll = std::cos(euler[2]);
|
|
double sin_yaw = std::sin(euler[0]);
|
|
double sin_pitch = std::sin(euler[1]);
|
|
double sin_roll = std::sin(euler[2]);
|
|
|
|
x = (sin_yaw * cos_pitch * cos_roll) + (cos_yaw * sin_pitch * sin_roll);
|
|
y = (sin_yaw * cos_pitch * sin_roll) - (cos_yaw * sin_pitch * cos_roll);
|
|
z = (cos_yaw * cos_pitch * sin_roll) + (sin_yaw * sin_pitch * cos_roll);
|
|
w = (cos_yaw * cos_pitch * cos_roll) - (sin_yaw * sin_pitch * sin_roll);
|
|
|
|
return Quaterniond(Vector4d{x, y, z, w});
|
|
}
|
|
|
|
|
|
template <typename T>
|
|
Quaternion<T>
|
|
LERP(Quaternion<T> p, Quaternion<T> q, T t)
|
|
{
|
|
return p + (q - p) * t;
|
|
}
|
|
|
|
|
|
template <typename T>
|
|
Quaternion<T>
|
|
ShortestSLERP(Quaternion<T> p, Quaternion<T> q, T t)
|
|
{
|
|
T innerProduct = p.dot(q);
|
|
T sign = innerProduct >= 0.0 ? -1.0 : 1.0;
|
|
T acip = std::acos(innerProduct);
|
|
|
|
return (p * std::sin((T)1.0 - t) * acip + p * sign * std::sin(t * acip)) / std::sin(acip);
|
|
}
|
|
|
|
|
|
void
|
|
Quaternion_SelfTest()
|
|
{
|
|
#ifndef NDEBUG
|
|
Vector3f v {1.0, 0.0, 0.0};
|
|
Vector3f yAxis {0.0, 1.0, 0.0};
|
|
float angle = M_PI / 2;
|
|
|
|
Quaternionf p = quaternionf(yAxis, angle);
|
|
Quaternionf q;
|
|
Vector3f vr {0.0, 0.0, 1.0};
|
|
|
|
assert(p.isUnitQuaternion());
|
|
assert(p.rotate(v) == vr);
|
|
assert(p * q == p);
|
|
#endif
|
|
}
|
|
|
|
|
|
} // namespace geom
|
|
} // namespace wr
|