#pragma once
#include "core/math/math_defs.h"
#include "core/math/math_funcs.h"
#include "core/math/quaternion.h"
#include "core/math/vector3.h"
#include "tests/test_macros.h"
namespace TestQuaternion {
Quaternion quat_euler_yxz_deg(Vector3 angle) {
double yaw = Math::deg_to_rad(angle[1]);
double pitch = Math::deg_to_rad(angle[0]);
double roll = Math::deg_to_rad(angle[2]);
Quaternion q_y = Quaternion::from_euler(Vector3(0.0, yaw, 0.0));
Quaternion q_p = Quaternion::from_euler(Vector3(pitch, 0.0, 0.0));
Quaternion q_r = Quaternion::from_euler(Vector3(0.0, 0.0, roll));
Quaternion q_yxz = q_y * q_p * q_r;
return q_yxz;
}
TEST_CASE("[Quaternion] Default Construct") {
constexpr Quaternion q;
CHECK(q[0] == 0.0);
CHECK(q[1] == 0.0);
CHECK(q[2] == 0.0);
CHECK(q[3] == 1.0);
}
TEST_CASE("[Quaternion] Construct x,y,z,w") {
constexpr Quaternion q(0.2391, 0.099, 0.3696, 0.8924);
CHECK(q[0] == doctest::Approx(0.2391));
CHECK(q[1] == doctest::Approx(0.099));
CHECK(q[2] == doctest::Approx(0.3696));
CHECK(q[3] == doctest::Approx(0.8924));
}
TEST_CASE("[Quaternion] Construct AxisAngle 1") {
Quaternion q(Vector3(1.0, 0.0, 0.0), Math::deg_to_rad(120.0));
CHECK(q[0] == doctest::Approx(0.866025));
CHECK(q[1] == doctest::Approx(0.0));
CHECK(q[2] == doctest::Approx(0.0));
CHECK(q[3] == doctest::Approx(0.5));
}
TEST_CASE("[Quaternion] Construct AxisAngle 2") {
Quaternion q(Vector3(0.0, 1.0, 0.0), Math::deg_to_rad(30.0));
CHECK(q[0] == doctest::Approx(0.0));
CHECK(q[1] == doctest::Approx(0.258819));
CHECK(q[2] == doctest::Approx(0.0));
CHECK(q[3] == doctest::Approx(0.965926));
}
TEST_CASE("[Quaternion] Construct AxisAngle 3") {
Quaternion q(Vector3(0.0, 0.0, 1.0), Math::deg_to_rad(60.0));
CHECK(q[0] == doctest::Approx(0.0));
CHECK(q[1] == doctest::Approx(0.0));
CHECK(q[2] == doctest::Approx(0.5));
CHECK(q[3] == doctest::Approx(0.866025));
}
TEST_CASE("[Quaternion] Construct AxisAngle 4") {
constexpr Vector3 axis(1.0, 2.0, 0.5);
Quaternion q(axis.normalized(), Math::deg_to_rad(35.0));
CHECK(q[0] == doctest::Approx(0.131239));
CHECK(q[1] == doctest::Approx(0.262478));
CHECK(q[2] == doctest::Approx(0.0656194));
CHECK(q[3] == doctest::Approx(0.953717));
}
TEST_CASE("[Quaternion] Construct from Quaternion") {
constexpr Vector3 axis(1.0, 2.0, 0.5);
Quaternion q_src(axis.normalized(), Math::deg_to_rad(35.0));
Quaternion q(q_src);
CHECK(q[0] == doctest::Approx(0.131239));
CHECK(q[1] == doctest::Approx(0.262478));
CHECK(q[2] == doctest::Approx(0.0656194));
CHECK(q[3] == doctest::Approx(0.953717));
}
TEST_CASE("[Quaternion] Construct Euler SingleAxis") {
double yaw = Math::deg_to_rad(45.0);
double pitch = Math::deg_to_rad(30.0);
double roll = Math::deg_to_rad(10.0);
Vector3 euler_y(0.0, yaw, 0.0);
Quaternion q_y = Quaternion::from_euler(euler_y);
CHECK(q_y[0] == doctest::Approx(0.0));
CHECK(q_y[1] == doctest::Approx(0.382684));
CHECK(q_y[2] == doctest::Approx(0.0));
CHECK(q_y[3] == doctest::Approx(0.923879));
Vector3 euler_p(pitch, 0.0, 0.0);
Quaternion q_p = Quaternion::from_euler(euler_p);
CHECK(q_p[0] == doctest::Approx(0.258819));
CHECK(q_p[1] == doctest::Approx(0.0));
CHECK(q_p[2] == doctest::Approx(0.0));
CHECK(q_p[3] == doctest::Approx(0.965926));
Vector3 euler_r(0.0, 0.0, roll);
Quaternion q_r = Quaternion::from_euler(euler_r);
CHECK(q_r[0] == doctest::Approx(0.0));
CHECK(q_r[1] == doctest::Approx(0.0));
CHECK(q_r[2] == doctest::Approx(0.0871558));
CHECK(q_r[3] == doctest::Approx(0.996195));
}
TEST_CASE("[Quaternion] Construct Euler YXZ dynamic axes") {
double yaw = Math::deg_to_rad(45.0);
double pitch = Math::deg_to_rad(30.0);
double roll = Math::deg_to_rad(10.0);
Vector3 euler_y(0.0, yaw, 0.0);
Quaternion q_y = Quaternion::from_euler(euler_y);
Vector3 euler_p(pitch, 0.0, 0.0);
Quaternion q_p = Quaternion::from_euler(euler_p);
Vector3 euler_r(0.0, 0.0, roll);
Quaternion q_r = Quaternion::from_euler(euler_r);
Quaternion check_yxz = q_y * q_p * q_r;
Vector3 euler_yxz(pitch, yaw, roll);
Quaternion q = Quaternion::from_euler(euler_yxz);
CHECK(q[0] == doctest::Approx(check_yxz[0]));
CHECK(q[1] == doctest::Approx(check_yxz[1]));
CHECK(q[2] == doctest::Approx(check_yxz[2]));
CHECK(q[3] == doctest::Approx(check_yxz[3]));
CHECK(q.is_equal_approx(check_yxz));
CHECK(q.get_euler().is_equal_approx(euler_yxz));
CHECK(check_yxz.get_euler().is_equal_approx(euler_yxz));
}
TEST_CASE("[Quaternion] Construct Basis Euler") {
double yaw = Math::deg_to_rad(45.0);
double pitch = Math::deg_to_rad(30.0);
double roll = Math::deg_to_rad(10.0);
Vector3 euler_yxz(pitch, yaw, roll);
Quaternion q_yxz = Quaternion::from_euler(euler_yxz);
Basis basis_axes = Basis::from_euler(euler_yxz);
Quaternion q(basis_axes);
CHECK(q.is_equal_approx(q_yxz));
}
TEST_CASE("[Quaternion] Construct Basis Axes") {
const Vector3 euler_yxz(Math::deg_to_rad(31.41), Math::deg_to_rad(-49.16), Math::deg_to_rad(12.34));
constexpr Vector3 i_unit(0.5545787, 0.1823950, 0.8118957);
constexpr Vector3 j_unit(-0.5249245, 0.8337420, 0.1712555);
constexpr Vector3 k_unit(-0.6456754, -0.5211586, 0.5581192);
constexpr Quaternion q_calc(0.2016913, -0.4245716, 0.206033, 0.8582598);
const Quaternion q_local = quat_euler_yxz_deg(Vector3(31.41, -49.16, 12.34));
const Quaternion q_euler = Quaternion::from_euler(euler_yxz);
CHECK(q_calc.is_equal_approx(q_local));
CHECK(q_local.is_equal_approx(q_euler));
Basis basis_axes = Basis::from_euler(euler_yxz);
Quaternion q(basis_axes);
CHECK(basis_axes.get_column(0).is_equal_approx(i_unit));
CHECK(basis_axes.get_column(1).is_equal_approx(j_unit));
CHECK(basis_axes.get_column(2).is_equal_approx(k_unit));
CHECK(q.is_equal_approx(q_calc));
CHECK_FALSE(q.inverse().is_equal_approx(q_calc));
CHECK(q.is_equal_approx(q_local));
CHECK(q.is_equal_approx(q_euler));
CHECK(q[0] == doctest::Approx(0.2016913));
CHECK(q[1] == doctest::Approx(-0.4245716));
CHECK(q[2] == doctest::Approx(0.206033));
CHECK(q[3] == doctest::Approx(0.8582598));
}
TEST_CASE("[Quaternion] Construct Shortest Arc For 180 Degree Arc") {
Vector3 up(0, 1, 0);
Vector3 down(0, -1, 0);
Vector3 left(-1, 0, 0);
Vector3 right(1, 0, 0);
Vector3 forward(0, 0, -1);
Vector3 back(0, 0, 1);
Quaternion left_to_right(left, right);
Quaternion right_to_left(right, left);
CHECK(left_to_right.xform(left).is_equal_approx(right));
CHECK(Quaternion(right, left).xform(right).is_equal_approx(left));
CHECK(Quaternion(up, down).xform(up).is_equal_approx(down));
CHECK(Quaternion(down, up).xform(down).is_equal_approx(up));
CHECK(Quaternion(forward, back).xform(forward).is_equal_approx(back));
CHECK(Quaternion(back, forward).xform(back).is_equal_approx(forward));
Vector3 diagonal_up = Vector3(1.2, 2.3, 4.5).normalized();
Vector3 diagonal_down = -diagonal_up;
Quaternion q1(diagonal_up, diagonal_down);
CHECK(q1.xform(diagonal_down).is_equal_approx(diagonal_up));
CHECK(q1.xform(diagonal_up).is_equal_approx(diagonal_down));
CHECK(left_to_right.is_equal_approx(right_to_left.inverse()));
CHECK(Quaternion(diagonal_up, diagonal_up).is_equal_approx(Quaternion()));
}
TEST_CASE("[Quaternion] Get Euler Orders") {
double x = Math::deg_to_rad(30.0);
double y = Math::deg_to_rad(45.0);
double z = Math::deg_to_rad(10.0);
Vector3 euler(x, y, z);
for (int i = 0; i < 6; i++) {
EulerOrder order = (EulerOrder)i;
Basis basis = Basis::from_euler(euler, order);
Quaternion q = Quaternion(basis);
Vector3 check = q.get_euler(order);
CHECK_MESSAGE(check.is_equal_approx(euler),
"Quaternion get_euler method should return the original angles.");
CHECK_MESSAGE(check.is_equal_approx(basis.get_euler(order)),
"Quaternion get_euler method should behave the same as Basis get_euler.");
}
}
TEST_CASE("[Quaternion] Product (book)") {
constexpr Quaternion p(1.0, -2.0, 1.0, 3.0);
constexpr Quaternion q(-1.0, 2.0, 3.0, 2.0);
constexpr Quaternion pq = p * q;
CHECK(pq[0] == doctest::Approx(-9.0));
CHECK(pq[1] == doctest::Approx(-2.0));
CHECK(pq[2] == doctest::Approx(11.0));
CHECK(pq[3] == doctest::Approx(8.0));
}
TEST_CASE("[Quaternion] Product") {
double yaw = Math::deg_to_rad(45.0);
double pitch = Math::deg_to_rad(30.0);
double roll = Math::deg_to_rad(10.0);
Vector3 euler_y(0.0, yaw, 0.0);
Quaternion q_y = Quaternion::from_euler(euler_y);
CHECK(q_y[0] == doctest::Approx(0.0));
CHECK(q_y[1] == doctest::Approx(0.382684));
CHECK(q_y[2] == doctest::Approx(0.0));
CHECK(q_y[3] == doctest::Approx(0.923879));
Vector3 euler_p(pitch, 0.0, 0.0);
Quaternion q_p = Quaternion::from_euler(euler_p);
CHECK(q_p[0] == doctest::Approx(0.258819));
CHECK(q_p[1] == doctest::Approx(0.0));
CHECK(q_p[2] == doctest::Approx(0.0));
CHECK(q_p[3] == doctest::Approx(0.965926));
Vector3 euler_r(0.0, 0.0, roll);
Quaternion q_r = Quaternion::from_euler(euler_r);
CHECK(q_r[0] == doctest::Approx(0.0));
CHECK(q_r[1] == doctest::Approx(0.0));
CHECK(q_r[2] == doctest::Approx(0.0871558));
CHECK(q_r[3] == doctest::Approx(0.996195));
Quaternion q_yp = q_y * q_p;
CHECK(q_yp[0] == doctest::Approx(0.239118));
CHECK(q_yp[1] == doctest::Approx(0.369644));
CHECK(q_yp[2] == doctest::Approx(-0.099046));
CHECK(q_yp[3] == doctest::Approx(0.892399));
Quaternion q_ryp = q_r * q_yp;
CHECK(q_ryp[0] == doctest::Approx(0.205991));
CHECK(q_ryp[1] == doctest::Approx(0.389078));
CHECK(q_ryp[2] == doctest::Approx(-0.0208912));
CHECK(q_ryp[3] == doctest::Approx(0.897636));
}
TEST_CASE("[Quaternion] xform unit vectors") {
Quaternion q(Vector3(1.0, 0.0, 0.0), Math::deg_to_rad(120.0));
Vector3 i_t = q.xform(Vector3(1.0, 0.0, 0.0));
Vector3 j_t = q.xform(Vector3(0.0, 1.0, 0.0));
Vector3 k_t = q.xform(Vector3(0.0, 0.0, 1.0));
CHECK(i_t.is_equal_approx(Vector3(1.0, 0.0, 0.0)));
CHECK(j_t.is_equal_approx(Vector3(0.0, -0.5, 0.866025)));
CHECK(k_t.is_equal_approx(Vector3(0.0, -0.866025, -0.5)));
CHECK(i_t.length_squared() == doctest::Approx(1.0));
CHECK(j_t.length_squared() == doctest::Approx(1.0));
CHECK(k_t.length_squared() == doctest::Approx(1.0));
q = Quaternion(Vector3(0.0, 1.0, 0.0), Math::deg_to_rad(30.0));
i_t = q.xform(Vector3(1.0, 0.0, 0.0));
j_t = q.xform(Vector3(0.0, 1.0, 0.0));
k_t = q.xform(Vector3(0.0, 0.0, 1.0));
CHECK(i_t.is_equal_approx(Vector3(0.866025, 0.0, -0.5)));
CHECK(j_t.is_equal_approx(Vector3(0.0, 1.0, 0.0)));
CHECK(k_t.is_equal_approx(Vector3(0.5, 0.0, 0.866025)));
CHECK(i_t.length_squared() == doctest::Approx(1.0));
CHECK(j_t.length_squared() == doctest::Approx(1.0));
CHECK(k_t.length_squared() == doctest::Approx(1.0));
q = Quaternion(Vector3(0.0, 0.0, 1.0), Math::deg_to_rad(60.0));
i_t = q.xform(Vector3(1.0, 0.0, 0.0));
j_t = q.xform(Vector3(0.0, 1.0, 0.0));
k_t = q.xform(Vector3(0.0, 0.0, 1.0));
CHECK(i_t.is_equal_approx(Vector3(0.5, 0.866025, 0.0)));
CHECK(j_t.is_equal_approx(Vector3(-0.866025, 0.5, 0.0)));
CHECK(k_t.is_equal_approx(Vector3(0.0, 0.0, 1.0)));
CHECK(i_t.length_squared() == doctest::Approx(1.0));
CHECK(j_t.length_squared() == doctest::Approx(1.0));
CHECK(k_t.length_squared() == doctest::Approx(1.0));
}
TEST_CASE("[Quaternion] xform vector") {
const Vector3 euler_yzx(Math::deg_to_rad(31.41), Math::deg_to_rad(-49.16), Math::deg_to_rad(12.34));
const Basis basis_axes = Basis::from_euler(euler_yzx);
const Quaternion q(basis_axes);
constexpr Vector3 v_arb(3.0, 4.0, 5.0);
const Vector3 v_rot = q.xform(v_arb);
const Vector3 v_compare = basis_axes.xform(v_arb);
CHECK(v_rot.length_squared() == doctest::Approx(v_arb.length_squared()));
CHECK(v_rot.is_equal_approx(v_compare));
}
void test_quat_vec_rotate(Vector3 euler_yzx, Vector3 v_in) {
const Basis basis_axes = Basis::from_euler(euler_yzx);
const Quaternion q(basis_axes);
const Vector3 v_rot = q.xform(v_in);
const Vector3 v_compare = basis_axes.xform(v_in);
CHECK(v_rot.length_squared() == doctest::Approx(v_in.length_squared()));
CHECK(v_rot.is_equal_approx(v_compare));
}
TEST_CASE("[Stress][Quaternion] Many vector xforms") {
constexpr int STEPS = 100;
constexpr double delta = 2.0 * Math::PI / STEPS;
constexpr double delta_vec = 20.0 / STEPS;
Vector3 vec_arb(1.0, 1.0, 1.0);
double x_angle = -Math::PI;
double y_angle = -Math::PI;
double z_angle = -Math::PI;
for (double i = 0; i < STEPS; ++i) {
vec_arb[0] = -10.0 + i * delta_vec;
x_angle = i * delta - Math::PI;
for (double j = 0; j < STEPS; ++j) {
vec_arb[1] = -10.0 + j * delta_vec;
y_angle = j * delta - Math::PI;
for (double k = 0; k < STEPS; ++k) {
vec_arb[2] = -10.0 + k * delta_vec;
z_angle = k * delta - Math::PI;
Vector3 euler_yzx(x_angle, y_angle, z_angle);
test_quat_vec_rotate(euler_yzx, vec_arb);
}
}
}
}
TEST_CASE("[Quaternion] Finite number checks") {
constexpr real_t x = Math::NaN;
CHECK_MESSAGE(
Quaternion(0, 1, 2, 3).is_finite(),
"Quaternion with all components finite should be finite");
CHECK_FALSE_MESSAGE(
Quaternion(x, 1, 2, 3).is_finite(),
"Quaternion with one component infinite should not be finite.");
CHECK_FALSE_MESSAGE(
Quaternion(0, x, 2, 3).is_finite(),
"Quaternion with one component infinite should not be finite.");
CHECK_FALSE_MESSAGE(
Quaternion(0, 1, x, 3).is_finite(),
"Quaternion with one component infinite should not be finite.");
CHECK_FALSE_MESSAGE(
Quaternion(0, 1, 2, x).is_finite(),
"Quaternion with one component infinite should not be finite.");
CHECK_FALSE_MESSAGE(
Quaternion(x, x, 2, 3).is_finite(),
"Quaternion with two components infinite should not be finite.");
CHECK_FALSE_MESSAGE(
Quaternion(x, 1, x, 3).is_finite(),
"Quaternion with two components infinite should not be finite.");
CHECK_FALSE_MESSAGE(
Quaternion(x, 1, 2, x).is_finite(),
"Quaternion with two components infinite should not be finite.");
CHECK_FALSE_MESSAGE(
Quaternion(0, x, x, 3).is_finite(),
"Quaternion with two components infinite should not be finite.");
CHECK_FALSE_MESSAGE(
Quaternion(0, x, 2, x).is_finite(),
"Quaternion with two components infinite should not be finite.");
CHECK_FALSE_MESSAGE(
Quaternion(0, 1, x, x).is_finite(),
"Quaternion with two components infinite should not be finite.");
CHECK_FALSE_MESSAGE(
Quaternion(0, x, x, x).is_finite(),
"Quaternion with three components infinite should not be finite.");
CHECK_FALSE_MESSAGE(
Quaternion(x, 1, x, x).is_finite(),
"Quaternion with three components infinite should not be finite.");
CHECK_FALSE_MESSAGE(
Quaternion(x, x, 2, x).is_finite(),
"Quaternion with three components infinite should not be finite.");
CHECK_FALSE_MESSAGE(
Quaternion(x, x, x, 3).is_finite(),
"Quaternion with three components infinite should not be finite.");
CHECK_FALSE_MESSAGE(
Quaternion(x, x, x, x).is_finite(),
"Quaternion with four components infinite should not be finite.");
}
}