76 lines
2.0 KiB
C++
76 lines
2.0 KiB
C++
|
|
#include <J3ML/LinearAlgebra/Quaternion.h>
|
|
|
|
using namespace J3ML::LinearAlgebra;
|
|
|
|
Quaternion PreciseSlerp(const Quaternion &a, const Quaternion& b, float t)
|
|
{
|
|
double angle = a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
|
|
|
|
double sign = 1.0;
|
|
|
|
if (angle > 0) {
|
|
angle = -angle;
|
|
sign = -1.0;
|
|
}
|
|
double A;
|
|
double B;
|
|
|
|
if (angle <= 1.0) // perform spherical linear interpolation
|
|
{
|
|
angle = std::acos(angle); // After this, angle is in the range pi / 2 -> 0 as the original angle variable ranged from 0 -> 1.
|
|
|
|
double angleT = t * angle;
|
|
|
|
double s[3] = { std::sin(angle), std::sin(angle - angleT), std::sin(angleT)};
|
|
double c = 1.0 / s[0];
|
|
|
|
A = s[1] * c;
|
|
B = s[2] * c;
|
|
} else { // If angle is close to taking the denominator to zero, resort to linear interpolation (and normalization).
|
|
A = 1.0 - t;
|
|
B = t;
|
|
}
|
|
|
|
Quaternion C;
|
|
C.x = (float)(a.x*A*sign + b.x*B);
|
|
C.y = (float)(a.y*A*sign + b.y*B);
|
|
C.z = (float)(a.z*A*sign + b.z*B);
|
|
C.w = (float)(a.w*A*sign + b.w*B);
|
|
return C.Normalized();
|
|
}
|
|
|
|
|
|
int QuaternionTests()
|
|
{
|
|
|
|
TEST("Quat::SlerpPrecision", [] {
|
|
RNG rng;
|
|
float maxError = 0;
|
|
float maxLerpError = 0;
|
|
float magnitudeError = 0;
|
|
for (int i = 0; i < 10000; ++i)
|
|
{
|
|
Quaternion q = Quaternion::RandomRotation(rng);
|
|
Quaternion q2 = Quaternion::RandomRotation(rng);
|
|
float t = rng.Float01Incl();
|
|
|
|
Quaternion correct = PreciseSlerp(q, q2, t);
|
|
Quaternion fast = q.Slerp(q2, t);
|
|
magnitudeError = std::max(magnitudeError, std::abs(1.f - fast.LengthSquared()));
|
|
Quaternion lerp = q.Lerp(q2, t);
|
|
}
|
|
});
|
|
TEST("Quat::Mat4x4Conversion", [] {});
|
|
TEST("Quat::MulOpQuat", [] {});
|
|
TEST("Quat::DivOpQuat", [] {});
|
|
TEST("Quat::Lerp", [] {});
|
|
TEST("Quat::RotateFromTo", [] {});
|
|
TEST("Quat::Transform", [] {});
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
|